405 lines
14 KiB
C++
405 lines
14 KiB
C++
// Copyright (c) Charles J. Cliffe
|
|
// SPDX-License-Identifier: GPL-2.0+
|
|
|
|
#include "CubicSDRDefs.h"
|
|
#include <vector>
|
|
|
|
#ifdef __APPLE__
|
|
#include <pthread.h>
|
|
#endif
|
|
|
|
#include "DemodulatorPreThread.h"
|
|
#include "CubicSDR.h"
|
|
#include "DemodulatorInstance.h"
|
|
|
|
DemodulatorPreThread::DemodulatorPreThread(DemodulatorInstance *parent) : IOThread(), iqResampler(NULL), iqResampleRatio(1), cModem(nullptr), cModemKit(nullptr), iqInputQueue(NULL), iqOutputQueue(NULL)
|
|
{
|
|
initialized.store(false);
|
|
this->parent = parent;
|
|
|
|
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);
|
|
|
|
newSampleRate = currentSampleRate = 0;
|
|
newBandwidth = currentBandwidth = 0;
|
|
newAudioSampleRate = currentAudioSampleRate = 0;
|
|
newFrequency = currentFrequency = 0;
|
|
|
|
sampleRateChanged.store(false);
|
|
frequencyChanged.store(false);
|
|
bandwidthChanged.store(false);
|
|
audioSampleRateChanged.store(false);
|
|
modemSettingsChanged.store(false);
|
|
demodTypeChanged.store(false);
|
|
}
|
|
|
|
bool DemodulatorPreThread::isInitialized() {
|
|
return initialized.load();
|
|
}
|
|
|
|
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
|
|
|
|
// std::cout << "Demodulator preprocessor thread started.." << std::endl;
|
|
|
|
ReBuffer<DemodulatorThreadPostIQData> buffers("DemodulatorPreThreadBuffers");
|
|
|
|
iqInputQueue = static_cast<DemodulatorThreadInputQueue*>(getInputQueue("IQDataInput"));
|
|
iqOutputQueue = static_cast<DemodulatorThreadPostInputQueue*>(getOutputQueue("IQDataOutput"));
|
|
|
|
std::vector<liquid_float_complex> in_buf_data;
|
|
std::vector<liquid_float_complex> out_buf_data;
|
|
|
|
t_Worker = new std::thread(&DemodulatorWorkerThread::threadMain, workerThread);
|
|
|
|
while (!stopping) {
|
|
DemodulatorThreadIQData *inp;
|
|
|
|
iqInputQueue->pop(inp);
|
|
|
|
if (frequencyChanged.load()) {
|
|
currentFrequency = newFrequency;
|
|
frequencyChanged.store(false);
|
|
}
|
|
|
|
if (inp->sampleRate != currentSampleRate) {
|
|
newSampleRate = inp->sampleRate;
|
|
if (newSampleRate) {
|
|
sampleRateChanged.store(true);
|
|
}
|
|
}
|
|
|
|
if (!newAudioSampleRate) {
|
|
newAudioSampleRate = parent->getAudioSampleRate();
|
|
if (newAudioSampleRate) {
|
|
audioSampleRateChanged.store(true);
|
|
}
|
|
} else if (parent->getAudioSampleRate() != newAudioSampleRate) {
|
|
int newRate;
|
|
if ((newRate = parent->getAudioSampleRate())) {
|
|
newAudioSampleRate = parent->getAudioSampleRate();
|
|
audioSampleRateChanged.store(true);
|
|
}
|
|
}
|
|
|
|
if (demodTypeChanged.load() && (newSampleRate && newAudioSampleRate && newBandwidth)) {
|
|
DemodulatorWorkerThreadCommand command(DemodulatorWorkerThreadCommand::DEMOD_WORKER_THREAD_CMD_MAKE_DEMOD);
|
|
command.frequency = newFrequency;
|
|
command.sampleRate = newSampleRate;
|
|
command.demodType = newDemodType;
|
|
command.bandwidth = newBandwidth;
|
|
command.audioSampleRate = newAudioSampleRate;
|
|
demodType = newDemodType;
|
|
sampleRateChanged.store(false);
|
|
audioSampleRateChanged.store(false);
|
|
ModemSettings lastSettings = parent->getLastModemSettings(newDemodType);
|
|
if (lastSettings.size() != 0) {
|
|
command.settings = lastSettings;
|
|
if (modemSettingsBuffered.size()) {
|
|
for (ModemSettings::const_iterator msi = modemSettingsBuffered.begin(); msi != modemSettingsBuffered.end(); msi++) {
|
|
command.settings[msi->first] = msi->second;
|
|
}
|
|
}
|
|
} else {
|
|
command.settings = modemSettingsBuffered;
|
|
}
|
|
modemSettingsBuffered.clear();
|
|
modemSettingsChanged.store(false);
|
|
workerQueue->push(command);
|
|
cModem = nullptr;
|
|
cModemKit = nullptr;
|
|
demodTypeChanged.store(false);
|
|
initialized.store(false);
|
|
}
|
|
else if (
|
|
cModemKit && cModem &&
|
|
(bandwidthChanged.load() || sampleRateChanged.load() || audioSampleRateChanged.load() || cModem->shouldRebuildKit()) &&
|
|
(newSampleRate && newAudioSampleRate && newBandwidth)
|
|
) {
|
|
DemodulatorWorkerThreadCommand command(DemodulatorWorkerThreadCommand::DEMOD_WORKER_THREAD_CMD_BUILD_FILTERS);
|
|
command.frequency = newFrequency;
|
|
command.sampleRate = newSampleRate;
|
|
command.bandwidth = newBandwidth;
|
|
command.audioSampleRate = newAudioSampleRate;
|
|
bandwidthChanged.store(false);
|
|
sampleRateChanged.store(false);
|
|
audioSampleRateChanged.store(false);
|
|
modemSettingsBuffered.clear();
|
|
workerQueue->push(command);
|
|
}
|
|
|
|
// Requested frequency is not center, shift it into the center!
|
|
if ((currentFrequency - inp->frequency) != shiftFrequency) {
|
|
shiftFrequency = currentFrequency - 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 (cModem && cModemKit && 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 == currentSampleRate) && cModem && cModemKit) {
|
|
size_t 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();
|
|
|
|
size_t 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->data.assign(resampledData.begin(), resampledData.begin() + numWritten);
|
|
|
|
resamp->modemType = cModem->getType();
|
|
resamp->modemName = cModem->getName();
|
|
resamp->modem = cModem;
|
|
resamp->modemKit = cModemKit;
|
|
resamp->sampleRate = currentBandwidth;
|
|
|
|
if (!iqOutputQueue->push(resamp)) {
|
|
resamp->setRefCount(0);
|
|
std::cout << "DemodulatorPreThread::run() cannot push resamp into iqOutputQueue, is full !" << std::endl;
|
|
std::this_thread::yield();
|
|
}
|
|
}
|
|
|
|
inp->decRefCount();
|
|
|
|
DemodulatorWorkerThreadResult result;
|
|
//process all worker results until
|
|
while (!stopping && workerResults->try_pop(result)) {
|
|
|
|
switch (result.cmd) {
|
|
case DemodulatorWorkerThreadResult::DEMOD_WORKER_THREAD_RESULT_FILTERS:
|
|
if (result.iqResampler) {
|
|
if (iqResampler) {
|
|
msresamp_crcf_destroy(iqResampler);
|
|
}
|
|
iqResampler = result.iqResampler;
|
|
iqResampleRatio = result.iqResampleRatio;
|
|
}
|
|
|
|
if (result.modem != nullptr) {
|
|
cModem = result.modem;
|
|
#if ENABLE_DIGITAL_LAB
|
|
if (cModem->getType() == "digital") {
|
|
ModemDigital *mDigi = (ModemDigital *)cModem;
|
|
mDigi->setOutput(parent->getOutput());
|
|
}
|
|
#endif
|
|
}
|
|
|
|
if (result.modemKit != nullptr) {
|
|
cModemKit = result.modemKit;
|
|
currentAudioSampleRate = cModemKit->audioSampleRate;
|
|
}
|
|
|
|
if (result.bandwidth) {
|
|
currentBandwidth = result.bandwidth;
|
|
}
|
|
|
|
if (result.sampleRate) {
|
|
currentSampleRate = result.sampleRate;
|
|
}
|
|
|
|
if (result.modemName != "") {
|
|
demodType = result.modemName;
|
|
demodTypeChanged.store(false);
|
|
}
|
|
|
|
shiftFrequency = inp->frequency-1;
|
|
initialized.store(cModem != nullptr);
|
|
break;
|
|
default:
|
|
break;
|
|
}
|
|
} //end while
|
|
|
|
if ((cModem != nullptr) && modemSettingsChanged.load()) {
|
|
cModem->writeSettings(modemSettingsBuffered);
|
|
modemSettingsBuffered.clear();
|
|
modemSettingsChanged.store(false);
|
|
}
|
|
} //end while stopping
|
|
|
|
DemodulatorThreadPostIQData *tmp;
|
|
while (iqOutputQueue->try_pop(tmp)) {
|
|
|
|
tmp->decRefCount();
|
|
}
|
|
buffers.purge();
|
|
}
|
|
|
|
void DemodulatorPreThread::setDemodType(std::string demodType) {
|
|
newDemodType = demodType;
|
|
demodTypeChanged.store(true);
|
|
}
|
|
|
|
std::string DemodulatorPreThread::getDemodType() {
|
|
if (demodTypeChanged.load()) {
|
|
return newDemodType;
|
|
}
|
|
return demodType;
|
|
}
|
|
|
|
void DemodulatorPreThread::setFrequency(long long freq) {
|
|
frequencyChanged.store(true);
|
|
newFrequency = freq;
|
|
}
|
|
|
|
long long DemodulatorPreThread::getFrequency() {
|
|
if (frequencyChanged.load()) {
|
|
return newFrequency;
|
|
}
|
|
return currentFrequency;
|
|
}
|
|
|
|
void DemodulatorPreThread::setSampleRate(long long sampleRate) {
|
|
sampleRateChanged.store(true);
|
|
newSampleRate = sampleRate;
|
|
}
|
|
|
|
long long DemodulatorPreThread::getSampleRate() {
|
|
if (sampleRateChanged.load()) {
|
|
return newSampleRate;
|
|
}
|
|
return currentSampleRate;
|
|
}
|
|
|
|
void DemodulatorPreThread::setBandwidth(int bandwidth) {
|
|
bandwidthChanged.store(true);
|
|
newBandwidth = bandwidth;
|
|
}
|
|
|
|
int DemodulatorPreThread::getBandwidth() {
|
|
return currentBandwidth;
|
|
}
|
|
|
|
void DemodulatorPreThread::setAudioSampleRate(int rate) {
|
|
audioSampleRateChanged.store(true);
|
|
newAudioSampleRate = rate;
|
|
}
|
|
|
|
int DemodulatorPreThread::getAudioSampleRate() {
|
|
if (audioSampleRateChanged.load()) {
|
|
return newAudioSampleRate;
|
|
}
|
|
return currentAudioSampleRate;
|
|
}
|
|
|
|
void DemodulatorPreThread::terminate() {
|
|
IOThread::terminate();
|
|
DemodulatorThreadIQData *inp = new DemodulatorThreadIQData; // push dummy to nudge queue
|
|
if (!iqInputQueue->push(inp)) {
|
|
delete inp;
|
|
}
|
|
|
|
DemodulatorWorkerThreadCommand command(DemodulatorWorkerThreadCommand::DEMOD_WORKER_THREAD_CMD_NULL);
|
|
workerQueue->push(command);
|
|
|
|
workerThread->terminate();
|
|
workerThread->isTerminated(1000);
|
|
|
|
t_Worker->join();
|
|
delete t_Worker;
|
|
t_Worker = nullptr;
|
|
|
|
delete workerThread;
|
|
workerThread = nullptr;
|
|
|
|
delete workerResults;
|
|
workerResults = nullptr;
|
|
|
|
delete workerQueue;
|
|
workerQueue = nullptr;
|
|
}
|
|
|
|
Modem *DemodulatorPreThread::getModem() {
|
|
return cModem;
|
|
}
|
|
|
|
ModemKit *DemodulatorPreThread::getModemKit() {
|
|
return cModemKit;
|
|
}
|
|
|
|
|
|
std::string DemodulatorPreThread::readModemSetting(std::string setting) {
|
|
if (cModem) {
|
|
return cModem->readSetting(setting);
|
|
} else if (modemSettingsBuffered.find(setting) != modemSettingsBuffered.end()) {
|
|
return modemSettingsBuffered[setting];
|
|
}
|
|
return "";
|
|
}
|
|
|
|
void DemodulatorPreThread::writeModemSetting(std::string setting, std::string value) {
|
|
modemSettingsBuffered[setting] = value;
|
|
modemSettingsChanged.store(true);
|
|
}
|
|
|
|
ModemSettings DemodulatorPreThread::readModemSettings() {
|
|
if (cModem) {
|
|
return cModem->readSettings();
|
|
} else {
|
|
return modemSettingsBuffered;
|
|
}
|
|
}
|
|
|
|
void DemodulatorPreThread::writeModemSettings(ModemSettings settings) {
|
|
modemSettingsBuffered = settings;
|
|
modemSettingsChanged.store(true);
|
|
}
|