CubicSDR/src/demod/DemodulatorThread.cpp

207 lines
7.1 KiB
C++
Raw Normal View History

2014-12-16 18:27:32 -05:00
#include "DemodulatorThread.h"
#include "CubicSDRDefs.h"
#include <vector>
#ifdef __APPLE__
#include <pthread.h>
#endif
2014-12-24 01:28:33 -05:00
DemodulatorThread::DemodulatorThread(DemodulatorThreadPostInputQueue* pQueue, DemodulatorThreadControlCommandQueue *threadQueueControl,
DemodulatorThreadCommandQueue* threadQueueNotify) :
postInputQueue(pQueue), visOutQueue(NULL), audioInputQueue(NULL), agc(NULL), terminated(false), threadQueueNotify(threadQueueNotify), threadQueueControl(
threadQueueControl), squelch_level(0), squelch_tolerance(0), squelch_enabled(false) {
2014-12-16 18:27:32 -05:00
2014-12-16 21:30:03 -05:00
float kf = 0.5; // modulation factor
fdem = freqdem_create(kf);
2014-12-16 18:27:32 -05:00
// freqdem_print(fdem);
}
DemodulatorThread::~DemodulatorThread() {
}
#ifdef __APPLE__
void *DemodulatorThread::threadMain() {
#else
2014-12-16 21:30:03 -05:00
void DemodulatorThread::threadMain() {
2014-12-16 18:27:32 -05:00
#endif
#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
msresamp_rrrf audio_resampler = NULL;
2014-12-16 21:30:03 -05:00
msresamp_crcf resampler = NULL;
2014-12-16 18:27:32 -05:00
agc = agc_crcf_create();
agc_crcf_set_bandwidth(agc, 1e-3f);
2014-12-16 21:30:03 -05:00
std::cout << "Demodulator thread started.." << std::endl;
2014-12-24 01:28:33 -05:00
std::deque<AudioThreadInput *> buffers;
std::deque<AudioThreadInput *>::iterator buffers_i;
std::vector<liquid_float_complex> resampled_data;
std::vector<liquid_float_complex> agc_data;
std::vector<float> demod_output;
std::vector<float> resampled_audio_output;
2014-12-16 21:30:03 -05:00
while (!terminated) {
2014-12-23 01:12:14 -05:00
DemodulatorThreadPostIQData *inp;
2014-12-16 21:30:03 -05:00
postInputQueue->pop(inp);
2014-12-24 01:28:33 -05:00
std::lock_guard < std::mutex > lock(inp->m_mutex);
2014-12-16 18:27:32 -05:00
2014-12-23 01:12:14 -05:00
int bufSize = inp->data.size();
2014-12-16 18:27:32 -05:00
2014-12-16 21:30:03 -05:00
if (!bufSize) {
2014-12-24 01:28:33 -05:00
inp->decRefCount();
2014-12-16 21:30:03 -05:00
continue;
}
2014-12-16 18:27:32 -05:00
2014-12-16 21:30:03 -05:00
if (resampler == NULL) {
2014-12-23 01:12:14 -05:00
resampler = inp->resampler;
audio_resampler = inp->audio_resampler;
} else if (resampler != inp->resampler) {
2014-12-16 21:30:03 -05:00
msresamp_crcf_destroy(resampler);
msresamp_rrrf_destroy(audio_resampler);
2014-12-23 01:12:14 -05:00
resampler = inp->resampler;
audio_resampler = inp->audio_resampler;
2014-12-16 21:30:03 -05:00
}
2014-12-23 01:12:14 -05:00
int out_size = ceil((float) (bufSize) * inp->resample_ratio);
if (agc_data.size() != out_size) {
if (agc_data.capacity() < out_size) {
agc_data.reserve(out_size);
resampled_data.reserve(out_size);
}
agc_data.resize(out_size);
resampled_data.resize(out_size);
}
2014-12-16 21:30:03 -05:00
unsigned int num_written;
msresamp_crcf_execute(resampler, &(inp->data[0]), bufSize, &resampled_data[0], &num_written);
agc_crcf_execute_block(agc, &resampled_data[0], num_written, &agc_data[0]);
2014-12-23 01:12:14 -05:00
float audio_resample_ratio = inp->audio_resample_ratio;
2014-12-16 18:27:32 -05:00
if (demod_output.size() != num_written) {
if (demod_output.capacity() < num_written) {
demod_output.reserve(num_written);
}
demod_output.resize(num_written);
}
freqdem_demodulate_block(fdem, &agc_data[0], num_written, &demod_output[0]);
2014-12-16 18:27:32 -05:00
2014-12-16 21:30:03 -05:00
int audio_out_size = ceil((float) (num_written) * audio_resample_ratio);
if (audio_out_size != resampled_audio_output.size()) {
if (resampled_audio_output.capacity() < audio_out_size) {
resampled_audio_output.reserve(audio_out_size);
}
resampled_audio_output.resize(audio_out_size);
}
2014-12-16 18:27:32 -05:00
2014-12-16 21:30:03 -05:00
unsigned int num_audio_written;
msresamp_rrrf_execute(audio_resampler, &demod_output[0], num_written, &resampled_audio_output[0], &num_audio_written);
2014-12-16 18:27:32 -05:00
2014-12-16 21:30:03 -05:00
if (audioInputQueue != NULL) {
if (!squelch_enabled || ((agc_crcf_get_signal_level(agc)) >= 0.1)) {
2014-12-24 01:28:33 -05:00
AudioThreadInput *ati = NULL;
for (buffers_i = buffers.begin(); buffers_i != buffers.end(); buffers_i++) {
if ((*buffers_i)->getRefCount() <= 0) {
ati = (*buffers_i);
break;
}
}
if (ati == NULL) {
ati = new AudioThreadInput;
buffers.push_back(ati);
}
ati->setRefCount(1);
ati->channels = 1;
ati->data.assign(resampled_audio_output.begin(), resampled_audio_output.begin() + num_audio_written);
2014-12-24 01:28:33 -05:00
audioInputQueue->push(ati);
}
2014-12-16 21:30:03 -05:00
}
2014-12-16 18:27:32 -05:00
if (visOutQueue != NULL && visOutQueue->empty()) {
2014-12-23 01:59:03 -05:00
AudioThreadInput *ati_vis = new AudioThreadInput;
2014-12-24 01:28:33 -05:00
ati_vis->channels = 1;
int num_vis = DEMOD_VIS_SIZE;
if (num_audio_written > num_written) {
if (num_vis > num_audio_written) {
num_vis = num_audio_written;
}
ati_vis->data.assign(resampled_audio_output.begin(), resampled_audio_output.begin() + num_vis);
} else {
if (num_vis > num_written) {
num_vis = num_written;
}
ati_vis->data.assign(demod_output.begin(), demod_output.begin() + num_vis);
}
visOutQueue->push(ati_vis);
// std::cout << "Signal: " << agc_crcf_get_signal_level(agc) << " -- " << agc_crcf_get_rssi(agc) << "dB " << std::endl;
}
if (!threadQueueControl->empty()) {
while (!threadQueueControl->empty()) {
DemodulatorThreadControlCommand command;
threadQueueControl->pop(command);
switch (command.cmd) {
case DemodulatorThreadControlCommand::DEMOD_THREAD_CMD_CTL_SQUELCH_AUTO:
squelch_level = agc_crcf_get_signal_level(agc);
2014-12-24 01:28:33 -05:00
squelch_tolerance = agc_crcf_get_signal_level(agc) / 2.0;
squelch_enabled = true;
break;
case DemodulatorThreadControlCommand::DEMOD_THREAD_CMD_CTL_SQUELCH_OFF:
squelch_level = 0;
squelch_tolerance = 1;
squelch_enabled = false;
break;
default:
break;
}
}
2014-12-16 21:30:03 -05:00
}
2014-12-24 01:28:33 -05:00
inp->decRefCount();
2014-12-16 21:30:03 -05:00
}
2014-12-16 18:27:32 -05:00
2014-12-16 21:30:03 -05:00
if (resampler != NULL) {
msresamp_crcf_destroy(resampler);
}
if (audio_resampler != NULL) {
msresamp_rrrf_destroy(audio_resampler);
2014-12-16 21:30:03 -05:00
}
agc_crcf_destroy(agc);
2014-12-24 01:28:33 -05:00
while (!buffers.empty()) {
AudioThreadInput *audioDataDel = buffers.front();
buffers.pop_front();
std::lock_guard < std::mutex > lock(audioDataDel->m_mutex);
delete audioDataDel;
}
2014-12-16 21:30:03 -05:00
std::cout << "Demodulator thread done." << std::endl;
DemodulatorThreadCommand tCmd(DemodulatorThreadCommand::DEMOD_THREAD_CMD_DEMOD_TERMINATED);
tCmd.context = this;
threadQueueNotify->push(tCmd);
2014-12-16 18:27:32 -05:00
}
void DemodulatorThread::terminate() {
2014-12-16 21:30:03 -05:00
terminated = true;
2014-12-23 01:12:14 -05:00
DemodulatorThreadPostIQData *inp = new DemodulatorThreadPostIQData; // push dummy to nudge queue
2014-12-16 21:30:03 -05:00
postInputQueue->push(inp);
2014-12-16 18:27:32 -05:00
}