334 lines
12 KiB
C++
334 lines
12 KiB
C++
#include "CubicSDRDefs.h"
|
|
#include <vector>
|
|
|
|
#ifdef __APPLE__
|
|
#include <pthread.h>
|
|
#endif
|
|
|
|
#include "DemodulatorPreThread.h"
|
|
#include "CubicSDR.h"
|
|
|
|
DemodulatorPreThread::DemodulatorPreThread() : IOThread(), iqResampler(NULL), iqResampleRatio(1), audioResampler(NULL), stereoResampler(NULL), audioResampleRatio(1), firStereoLeft(NULL), firStereoRight(NULL), iirStereoPilot(NULL), iqInputQueue(NULL), iqOutputQueue(NULL), threadQueueNotify(NULL), commandQueue(NULL)
|
|
{
|
|
initialized.store(false);
|
|
|
|
freqShifter = nco_crcf_create(LIQUID_VCO);
|
|
shiftFrequency = 0;
|
|
|
|
workerQueue = new DemodulatorThreadWorkerCommandQueue;
|
|
workerResults = new DemodulatorThreadWorkerResultQueue;
|
|
|
|
workerThread = new DemodulatorWorkerThread();
|
|
workerThread->setInputQueue("WorkerCommandQueue",workerQueue);
|
|
workerThread->setOutputQueue("WorkerResultQueue",workerResults);
|
|
}
|
|
|
|
void DemodulatorPreThread::initialize() {
|
|
initialized = false;
|
|
|
|
iqResampleRatio = (double) (params.bandwidth) / (double) params.sampleRate;
|
|
audioResampleRatio = (double) (params.audioSampleRate) / (double) params.bandwidth;
|
|
|
|
float As = 120.0f; // stop-band attenuation [dB]
|
|
|
|
iqResampler = msresamp_crcf_create(iqResampleRatio, As);
|
|
audioResampler = msresamp_rrrf_create(audioResampleRatio, As);
|
|
stereoResampler = msresamp_rrrf_create(audioResampleRatio, As);
|
|
|
|
// Stereo filters / shifters
|
|
double firStereoCutoff = ((double) 16000 / (double) params.audioSampleRate);
|
|
float ft = ((double) 1000 / (double) params.audioSampleRate); // filter transition
|
|
float mu = 0.0f; // fractional timing offset
|
|
|
|
if (firStereoCutoff < 0) {
|
|
firStereoCutoff = 0;
|
|
}
|
|
|
|
if (firStereoCutoff > 0.5) {
|
|
firStereoCutoff = 0.5;
|
|
}
|
|
|
|
unsigned int h_len = estimate_req_filter_len(ft, As);
|
|
float *h = new float[h_len];
|
|
liquid_firdes_kaiser(h_len, firStereoCutoff, As, mu, h);
|
|
|
|
firStereoLeft = firfilt_rrrf_create(h, h_len);
|
|
firStereoRight = firfilt_rrrf_create(h, h_len);
|
|
|
|
// stereo pilot filter
|
|
float bw = params.bandwidth;
|
|
if (bw < 100000.0) {
|
|
bw = 100000.0;
|
|
}
|
|
unsigned int order = 5; // filter order
|
|
float f0 = ((double) 19000 / bw);
|
|
float fc = ((double) 19500 / bw);
|
|
float Ap = 1.0f;
|
|
As = 60.0f;
|
|
iirStereoPilot = iirfilt_crcf_create_prototype(LIQUID_IIRDES_CHEBY2, LIQUID_IIRDES_BANDPASS, LIQUID_IIRDES_SOS, order, fc, f0, Ap, As);
|
|
|
|
initialized = true;
|
|
lastParams = params;
|
|
}
|
|
|
|
DemodulatorPreThread::~DemodulatorPreThread() {
|
|
}
|
|
|
|
void DemodulatorPreThread::run() {
|
|
#ifdef __APPLE__
|
|
pthread_t tID = pthread_self(); // ID of this thread
|
|
int priority = sched_get_priority_max( SCHED_FIFO) - 1;
|
|
sched_param prio = {priority}; // scheduling priority of thread
|
|
pthread_setschedparam(tID, SCHED_FIFO, &prio);
|
|
#endif
|
|
|
|
if (!initialized) {
|
|
initialize();
|
|
}
|
|
|
|
std::cout << "Demodulator preprocessor thread started.." << std::endl;
|
|
|
|
t_Worker = new std::thread(&DemodulatorWorkerThread::threadMain, workerThread);
|
|
|
|
ReBuffer<DemodulatorThreadPostIQData> buffers;
|
|
|
|
iqInputQueue = (DemodulatorThreadInputQueue*)getInputQueue("IQDataInput");
|
|
iqOutputQueue = (DemodulatorThreadPostInputQueue*)getOutputQueue("IQDataOutput");
|
|
threadQueueNotify = (DemodulatorThreadCommandQueue*)getOutputQueue("NotifyQueue");
|
|
commandQueue = ( DemodulatorThreadCommandQueue*)getInputQueue("CommandQueue");
|
|
|
|
std::vector<liquid_float_complex> in_buf_data;
|
|
std::vector<liquid_float_complex> out_buf_data;
|
|
// liquid_float_complex carrySample; // Keep the stream count even to simplify some demod operations
|
|
// bool carrySampleFlag = false;
|
|
|
|
while (!terminated) {
|
|
DemodulatorThreadIQData *inp;
|
|
iqInputQueue->pop(inp);
|
|
|
|
bool bandwidthChanged = false;
|
|
bool rateChanged = false;
|
|
DemodulatorThreadParameters tempParams = params;
|
|
|
|
if (!commandQueue->empty()) {
|
|
while (!commandQueue->empty()) {
|
|
DemodulatorThreadCommand command;
|
|
commandQueue->pop(command);
|
|
switch (command.cmd) {
|
|
case DemodulatorThreadCommand::DEMOD_THREAD_CMD_SET_BANDWIDTH:
|
|
if (command.llong_value < 1500) {
|
|
command.llong_value = 1500;
|
|
}
|
|
if (command.llong_value > params.sampleRate) {
|
|
tempParams.bandwidth = params.sampleRate;
|
|
} else {
|
|
tempParams.bandwidth = command.llong_value;
|
|
}
|
|
bandwidthChanged = true;
|
|
break;
|
|
case DemodulatorThreadCommand::DEMOD_THREAD_CMD_SET_FREQUENCY:
|
|
params.frequency = tempParams.frequency = command.llong_value;
|
|
break;
|
|
case DemodulatorThreadCommand::DEMOD_THREAD_CMD_SET_AUDIO_RATE:
|
|
tempParams.audioSampleRate = (int)command.llong_value;
|
|
rateChanged = true;
|
|
break;
|
|
default:
|
|
break;
|
|
}
|
|
}
|
|
}
|
|
|
|
if (inp->sampleRate != tempParams.sampleRate && inp->sampleRate) {
|
|
tempParams.sampleRate = inp->sampleRate;
|
|
rateChanged = true;
|
|
}
|
|
|
|
if (bandwidthChanged || rateChanged) {
|
|
DemodulatorWorkerThreadCommand command(DemodulatorWorkerThreadCommand::DEMOD_WORKER_THREAD_CMD_BUILD_FILTERS);
|
|
command.sampleRate = tempParams.sampleRate;
|
|
command.audioSampleRate = tempParams.audioSampleRate;
|
|
command.bandwidth = tempParams.bandwidth;
|
|
command.frequency = tempParams.frequency;
|
|
|
|
workerQueue->push(command);
|
|
}
|
|
|
|
if (!initialized) {
|
|
inp->decRefCount();
|
|
continue;
|
|
}
|
|
|
|
// Requested frequency is not center, shift it into the center!
|
|
if ((params.frequency - inp->frequency) != shiftFrequency || rateChanged) {
|
|
shiftFrequency = params.frequency - inp->frequency;
|
|
if (abs(shiftFrequency) <= (int) ((double) (inp->sampleRate / 2) * 1.5)) {
|
|
nco_crcf_set_frequency(freqShifter, (2.0 * M_PI) * (((double) abs(shiftFrequency)) / ((double) inp->sampleRate)));
|
|
}
|
|
}
|
|
|
|
if (abs(shiftFrequency) > (int) ((double) (inp->sampleRate / 2) * 1.5)) {
|
|
inp->decRefCount();
|
|
continue;
|
|
}
|
|
|
|
// std::lock_guard < std::mutex > lock(inp->m_mutex);
|
|
std::vector<liquid_float_complex> *data = &inp->data;
|
|
if (data->size() && (inp->sampleRate == params.sampleRate)) {
|
|
int bufSize = data->size();
|
|
|
|
if (in_buf_data.size() != bufSize) {
|
|
if (in_buf_data.capacity() < bufSize) {
|
|
in_buf_data.reserve(bufSize);
|
|
out_buf_data.reserve(bufSize);
|
|
}
|
|
in_buf_data.resize(bufSize);
|
|
out_buf_data.resize(bufSize);
|
|
}
|
|
|
|
in_buf_data.assign(inp->data.begin(), inp->data.end());
|
|
|
|
liquid_float_complex *in_buf = &in_buf_data[0];
|
|
liquid_float_complex *out_buf = &out_buf_data[0];
|
|
liquid_float_complex *temp_buf = NULL;
|
|
|
|
if (shiftFrequency != 0) {
|
|
if (shiftFrequency < 0) {
|
|
nco_crcf_mix_block_up(freqShifter, in_buf, out_buf, bufSize);
|
|
} else {
|
|
nco_crcf_mix_block_down(freqShifter, in_buf, out_buf, bufSize);
|
|
}
|
|
temp_buf = in_buf;
|
|
in_buf = out_buf;
|
|
out_buf = temp_buf;
|
|
}
|
|
|
|
DemodulatorThreadPostIQData *resamp = buffers.getBuffer();
|
|
|
|
int out_size = ceil((double) (bufSize) * iqResampleRatio) + 512;
|
|
|
|
if (resampledData.size() != out_size) {
|
|
if (resampledData.capacity() < out_size) {
|
|
resampledData.reserve(out_size);
|
|
}
|
|
resampledData.resize(out_size);
|
|
}
|
|
|
|
unsigned int numWritten;
|
|
msresamp_crcf_execute(iqResampler, in_buf, bufSize, &resampledData[0], &numWritten);
|
|
|
|
resamp->setRefCount(1);
|
|
|
|
resamp->data.assign(resampledData.begin(), resampledData.begin() + numWritten);
|
|
|
|
// bool uneven = (numWritten % 2 != 0);
|
|
|
|
// if (!carrySampleFlag && !uneven) {
|
|
// resamp->data.assign(resampledData.begin(), resampledData.begin() + numWritten);
|
|
// carrySampleFlag = false;
|
|
// } else if (!carrySampleFlag && uneven) {
|
|
// resamp->data.assign(resampledData.begin(), resampledData.begin() + (numWritten-1));
|
|
// carrySample = resampledData.back();
|
|
// carrySampleFlag = true;
|
|
// } else if (carrySampleFlag && uneven) {
|
|
// resamp->data.resize(numWritten+1);
|
|
// resamp->data[0] = carrySample;
|
|
// memcpy(&resamp->data[1],&resampledData[0],sizeof(liquid_float_complex)*numWritten);
|
|
// carrySampleFlag = false;
|
|
// } else if (carrySampleFlag && !uneven) {
|
|
// resamp->data.resize(numWritten);
|
|
// resamp->data[0] = carrySample;
|
|
// memcpy(&resamp->data[1],&resampledData[0],sizeof(liquid_float_complex)*(numWritten-1));
|
|
// carrySample = resampledData.back();
|
|
// carrySampleFlag = true;
|
|
// }
|
|
|
|
|
|
|
|
resamp->audioResampleRatio = audioResampleRatio;
|
|
resamp->audioResampler = audioResampler;
|
|
resamp->audioSampleRate = params.audioSampleRate;
|
|
resamp->stereoResampler = stereoResampler;
|
|
resamp->firStereoLeft = firStereoLeft;
|
|
resamp->firStereoRight = firStereoRight;
|
|
resamp->iirStereoPilot = iirStereoPilot;
|
|
resamp->sampleRate = params.bandwidth;
|
|
|
|
iqOutputQueue->push(resamp);
|
|
}
|
|
|
|
inp->decRefCount();
|
|
|
|
if (!terminated && !workerResults->empty()) {
|
|
while (!workerResults->empty()) {
|
|
DemodulatorWorkerThreadResult result;
|
|
workerResults->pop(result);
|
|
|
|
switch (result.cmd) {
|
|
case DemodulatorWorkerThreadResult::DEMOD_WORKER_THREAD_RESULT_FILTERS:
|
|
msresamp_crcf_destroy(iqResampler);
|
|
|
|
if (result.iqResampler) {
|
|
iqResampler = result.iqResampler;
|
|
iqResampleRatio = result.iqResampleRatio;
|
|
}
|
|
|
|
if (result.firStereoLeft) {
|
|
firStereoLeft = result.firStereoLeft;
|
|
}
|
|
|
|
if (result.firStereoRight) {
|
|
firStereoRight = result.firStereoRight;
|
|
}
|
|
|
|
if (result.iirStereoPilot) {
|
|
iirStereoPilot = result.iirStereoPilot;
|
|
}
|
|
|
|
if (result.audioResampler) {
|
|
audioResampler = result.audioResampler;
|
|
audioResampleRatio = result.audioResamplerRatio;
|
|
stereoResampler = result.stereoResampler;
|
|
}
|
|
|
|
if (result.audioSampleRate) {
|
|
params.audioSampleRate = result.audioSampleRate;
|
|
}
|
|
|
|
if (result.bandwidth) {
|
|
params.bandwidth = result.bandwidth;
|
|
}
|
|
|
|
if (result.sampleRate) {
|
|
params.sampleRate = result.sampleRate;
|
|
}
|
|
break;
|
|
default:
|
|
break;
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
buffers.purge();
|
|
|
|
DemodulatorThreadCommand tCmd(DemodulatorThreadCommand::DEMOD_THREAD_CMD_DEMOD_PREPROCESS_TERMINATED);
|
|
tCmd.context = this;
|
|
threadQueueNotify->push(tCmd);
|
|
std::cout << "Demodulator preprocessor thread done." << std::endl;
|
|
}
|
|
|
|
void DemodulatorPreThread::terminate() {
|
|
terminated = true;
|
|
DemodulatorThreadIQData *inp = new DemodulatorThreadIQData; // push dummy to nudge queue
|
|
iqInputQueue->push(inp);
|
|
DemodulatorWorkerThreadCommand command(DemodulatorWorkerThreadCommand::DEMOD_WORKER_THREAD_CMD_NULL);
|
|
workerQueue->push(command);
|
|
workerThread->terminate();
|
|
t_Worker->join();
|
|
delete t_Worker;
|
|
delete workerThread;
|
|
delete workerResults;
|
|
delete workerQueue;
|
|
}
|