void DemodulatorThread::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
ReBuffer<AudioThreadInput> audioVisBuffers;
std::cout << "Demodulator thread started.." << std::endl;
iqInputQueue = (DemodulatorThreadPostInputQueue*)getInputQueue("IQDataInput");
audioOutputQueue = (AudioThreadInputQueue*)getOutputQueue("AudioDataOutput");
threadQueueControl = (DemodulatorThreadControlCommandQueue *)getInputQueue("ControlQueue");
threadQueueNotify = (DemodulatorThreadCommandQueue*)getOutputQueue("NotifyQueue");
ModemIQData modemData;
while (!terminated) {
DemodulatorThreadPostIQData *inp;
iqInputQueue->pop(inp);
// std::lock_guard < std::mutex > lock(inp->m_mutex);
audioSampleRate = demodInstance->getAudioSampleRate();
int bufSize = inp->data.size();
if (!bufSize) {
inp->decRefCount();
continue;
}
if (inp->modemKit && inp->modemKit != cModemKit) {
if (cModemKit != nullptr) {
cModem->disposeKit(cModemKit);
}
cModemKit = inp->modemKit;
}
if (inp->modem && inp->modem != cModem) {
delete cModem;
cModem = inp->modem;
}
if (!cModem || !cModemKit) {
inp->decRefCount();
continue;
}
float currentSignalLevel = 0;
float accum = 0;
for (std::vector<liquid_float_complex>::iterator i = inp->data.begin(); i != inp->data.end(); i++) {
accum += abMagnitude(0.948059448969, 0.392699081699, i->real, i->imag);
}
currentSignalLevel = linearToDb(accum / float(inp->data.size()));
if (currentSignalLevel < DEMOD_SIGNAL_MIN+1) {
currentSignalLevel = DEMOD_SIGNAL_MIN+1;
}
std::vector<liquid_float_complex> *inputData;
inputData = &inp->data;
modemData.sampleRate = inp->sampleRate;
modemData.data.assign(inputData->begin(), inputData->end());
modemData.setRefCount(1);
AudioThreadInput *ati = NULL;
ModemAnalog *modemAnalog = (cModem->getType() == "analog")?((ModemAnalog *)cModem):nullptr;
// ModemDigital *modemDigital = (cModem->getType() == "digital")?((ModemDigital *)cModem):nullptr;
if (modemAnalog != nullptr) {
ati = outputBuffers.getBuffer();
ati->sampleRate = audioSampleRate;
ati->inputRate = inp->sampleRate;
ati->setRefCount(1);
}
cModem->demodulate(cModemKit, &modemData, ati);
if (currentSignalLevel > signalLevel) {
signalLevel = signalLevel + (currentSignalLevel - signalLevel) * 0.5;
} else {
signalLevel = signalLevel + (currentSignalLevel - signalLevel) * 0.05;
}
bool squelched = (squelchEnabled && (signalLevel < squelchLevel));
if (audioOutputQueue != NULL && ati && !squelched) {
std::vector<float>::iterator data_i;
ati->peak = 0;
for (data_i = ati->data.begin(); data_i != ati->data.end(); data_i++) {
float p = fabs(*data_i);
if (p > ati->peak) {
ati->peak = p;
}
}
} else if (ati) {
ati->decRefCount();
ati = nullptr;
}
if (ati && audioVisOutputQueue != NULL && audioVisOutputQueue->empty()) {
AudioThreadInput *ati_vis = audioVisBuffers.getBuffer();
ati_vis->setRefCount(1);
ati_vis->sampleRate = inp->sampleRate;
ati_vis->inputRate = inp->sampleRate;
int num_vis = DEMOD_VIS_SIZE;
if (ati->channels==2) {
ati_vis->channels = 2;
int stereoSize = ati->data.size();
if (stereoSize > DEMOD_VIS_SIZE * 2) {
stereoSize = DEMOD_VIS_SIZE * 2;
}
ati_vis->data.resize(stereoSize);
if (inp->modemType == "I/Q") {
for (int i = 0; i < stereoSize / 2; i++) {
ati_vis->data[i] = (*inputData)[i].real * 0.75;
ati_vis->data[i + stereoSize / 2] = (*inputData)[i].imag * 0.75;
}
} else {
for (int i = 0; i < stereoSize / 2; i++) {
ati_vis->inputRate = audioSampleRate;
ati_vis->sampleRate = 36000;
ati_vis->data[i] = ati->data[i * 2];
ati_vis->data[i + stereoSize / 2] = ati->data[i * 2 + 1];
}
}
} else {
int numAudioWritten = ati->data.size();
ati_vis->channels = 1;
std::vector<float> *demodOutData = (modemAnalog != nullptr)?modemAnalog->getDemodOutputData():nullptr;
if ((numAudioWritten > bufSize) || (demodOutData == nullptr)) {
ati_vis->inputRate = audioSampleRate;
if (num_vis > numAudioWritten) {
num_vis = numAudioWritten;
}
ati_vis->data.assign(ati->data.begin(), ati->data.begin() + num_vis);
} else {
if (num_vis > demodOutData->size()) {
num_vis = demodOutData->size();
}
ati_vis->data.assign(demodOutData->begin(), demodOutData->begin() + num_vis);
}
}
audioVisOutputQueue->push(ati_vis);
}
if (ati != NULL) {
if (!muted.load()) {
audioOutputQueue->push(ati);
} else {
ati->setRefCount(0);
}
}
if (!threadQueueControl->empty()) {
while (!threadQueueControl->empty()) {
DemodulatorThreadControlCommand command;
threadQueueControl->pop(command);
switch (command.cmd) {
case DemodulatorThreadControlCommand::DEMOD_THREAD_CMD_CTL_SQUELCH_ON:
squelchEnabled = true;
break;
case DemodulatorThreadControlCommand::DEMOD_THREAD_CMD_CTL_SQUELCH_OFF:
squelchEnabled = false;
break;
default:
break;
}
}
}
inp->decRefCount();
}
// end while !terminated
outputBuffers.purge();
if (audioVisOutputQueue && !audioVisOutputQueue->empty()) {
AudioThreadInput *dummy_vis;
audioVisOutputQueue->pop(dummy_vis);
}
audioVisBuffers.purge();
DemodulatorThreadCommand tCmd(DemodulatorThreadCommand::DEMOD_THREAD_CMD_DEMOD_TERMINATED);
tCmd.context = this;
threadQueueNotify->push(tCmd);
std::cout << "Demodulator thread done." << std::endl;
}
void SDRPostThread::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
dcFilter = iirfilt_crcf_create_dc_blocker(0.0005);
std::cout << "SDR post-processing thread started.." << std::endl;
iqDataInQueue = (SDRThreadIQDataQueue*)getInputQueue("IQDataInput");
iqDataOutQueue = (DemodulatorThreadInputQueue*)getOutputQueue("IQDataOutput");
iqVisualQueue = (DemodulatorThreadInputQueue*)getOutputQueue("IQVisualDataOutput");
ReBuffer<DemodulatorThreadIQData> buffers;
std::vector<liquid_float_complex> fpData;
std::vector<liquid_float_complex> dataOut;
iqDataInQueue->set_max_num_items(0);
while (!terminated) {
SDRThreadIQData *data_in;
iqDataInQueue->pop(data_in);
// std::lock_guard < std::mutex > lock(data_in->m_mutex);
if (data_in && data_in->data.size()) {
int dataSize = data_in->data.size()/2;
if (dataSize > fpData.capacity()) {
fpData.reserve(dataSize);
dataOut.reserve(dataSize);
}
if (dataSize != fpData.size()) {
fpData.resize(dataSize);
dataOut.resize(dataSize);
}
if (swapIQ) {
for (int i = 0; i < dataSize; i++) {
fpData[i] = _lut_swap[*((uint16_t*)&data_in->data[2*i])];
}
} else {
for (int i = 0; i < dataSize; i++) {
fpData[i] = _lut[*((uint16_t*)&data_in->data[2*i])];
}
}
iirfilt_crcf_execute_block(dcFilter, &fpData[0], dataSize, &dataOut[0]);
if (iqVisualQueue != NULL && !iqVisualQueue->full()) {
DemodulatorThreadIQData *visualDataOut = visualDataBuffers.getBuffer();
visualDataOut->setRefCount(1);
int num_vis_samples = dataOut.size();
// if (visualDataOut->data.size() < num_vis_samples) {
// if (visualDataOut->data.capacity() < num_vis_samples) {
// visualDataOut->data.reserve(num_vis_samples);
// }
// visualDataOut->data.resize(num_vis_samples);
// }
//
visualDataOut->frequency = data_in->frequency;
visualDataOut->sampleRate = data_in->sampleRate;
visualDataOut->data.assign(dataOut.begin(), dataOut.begin() + num_vis_samples);
iqVisualQueue->push(visualDataOut);
}
busy_demod.lock();
int activeDemods = 0;
bool pushedData = false;
if (demodulators.size() || iqDataOutQueue != NULL) {
std::vector<DemodulatorInstance *>::iterator demod_i;
for (demod_i = demodulators.begin(); demod_i != demodulators.end(); demod_i++) {
DemodulatorInstance *demod = *demod_i;
if (demod->getFrequency() != data_in->frequency
&& abs(data_in->frequency - demod->getFrequency()) > (wxGetApp().getSampleRate() / 2)) {
continue;
}
activeDemods++;
}
if (iqDataOutQueue != NULL) {
activeDemods++;
}
DemodulatorThreadIQData *demodDataOut = buffers.getBuffer();
// std::lock_guard < std::mutex > lock(demodDataOut->m_mutex);
demodDataOut->frequency = data_in->frequency;
demodDataOut->sampleRate = data_in->sampleRate;
demodDataOut->setRefCount(activeDemods);
demodDataOut->data.assign(dataOut.begin(), dataOut.end());
for (demod_i = demodulators.begin(); demod_i != demodulators.end(); demod_i++) {
DemodulatorInstance *demod = *demod_i;
DemodulatorThreadInputQueue *demodQueue = demod->getIQInputDataPipe();
if (abs(data_in->frequency - demod->getFrequency()) > (wxGetApp().getSampleRate() / 2)) {
if (demod->isActive() && !demod->isFollow() && !demod->isTracking()) {
demod->setActive(false);
DemodulatorThreadIQData *dummyDataOut = new DemodulatorThreadIQData;
dummyDataOut->frequency = data_in->frequency;
dummyDataOut->sampleRate = data_in->sampleRate;
demodQueue->push(dummyDataOut);
}
if (demod->isFollow() && wxGetApp().getFrequency() != demod->getFrequency()) {
wxGetApp().setFrequency(demod->getFrequency());
}
} else if (!demod->isActive()) {
demod->setActive(true);
if (wxGetApp().getDemodMgr().getLastActiveDemodulator() == NULL) {
wxGetApp().getDemodMgr().setActiveDemodulator(demod);
}
}
if (!demod->isActive()) {
continue;
}
if (demod->isFollow()) {
demod->setFollow(false);
}
demodQueue->push(demodDataOut);
pushedData = true;
}
if (iqDataOutQueue != NULL) {
if (!iqDataOutQueue->full()) {
iqDataOutQueue->push(demodDataOut);
pushedData = true;
} else {
demodDataOut->decRefCount();
}
}
if (!pushedData && iqDataOutQueue == NULL) {
demodDataOut->setRefCount(0);
}
}
busy_demod.unlock();
}
data_in->decRefCount();
}
// buffers.purge();
if (iqVisualQueue && !iqVisualQueue->empty()) {
DemodulatorThreadIQData *visualDataDummy;
iqVisualQueue->pop(visualDataDummy);
}
// visualDataBuffers.purge();
std::cout << "SDR post-processing thread done." << std::endl;
}
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;
}
