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) :
|
2014-12-26 22:20:50 -05:00
|
|
|
postInputQueue(pQueue), visOutQueue(NULL), audioInputQueue(NULL), agc(NULL), stereo(false), 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
|
2014-12-16 20:33:44 -05:00
|
|
|
#ifdef __APPLE__
|
2014-12-18 20:11:25 -05:00
|
|
|
pthread_t tID = pthread_self(); // ID of this thread
|
|
|
|
int priority = sched_get_priority_max( SCHED_FIFO )-1;
|
2014-12-18 21:39:32 -05:00
|
|
|
sched_param prio = {priority}; // scheduling priority of thread
|
2014-12-18 20:11:25 -05:00
|
|
|
pthread_setschedparam(tID, SCHED_FIFO, &prio);
|
2014-12-16 20:33:44 -05:00
|
|
|
#endif
|
|
|
|
|
2014-12-21 17:37:41 -05:00
|
|
|
msresamp_rrrf audio_resampler = NULL;
|
2014-12-26 21:55:13 -05:00
|
|
|
msresamp_rrrf stereo_resampler = NULL;
|
2014-12-16 21:30:03 -05:00
|
|
|
msresamp_crcf resampler = NULL;
|
2014-12-16 18:27:32 -05:00
|
|
|
|
2014-12-27 12:45:21 -05:00
|
|
|
unsigned int m = 5; // filter semi-length
|
|
|
|
float slsl = 60.0f; // filter sidelobe suppression level
|
2014-12-27 12:23:09 -05:00
|
|
|
liquid_float_complex x, y;
|
|
|
|
|
2014-12-27 12:45:21 -05:00
|
|
|
firhilbf firR2C = firhilbf_create(m, slsl);
|
|
|
|
firhilbf firC2R = firhilbf_create(m, slsl);
|
2014-12-27 12:23:09 -05:00
|
|
|
|
|
|
|
nco_crcf nco_shift = nco_crcf_create(LIQUID_NCO);
|
|
|
|
float shift_freq = 0;
|
|
|
|
|
2014-12-21 16:08: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;
|
|
|
|
|
2014-12-24 03:03:34 -05:00
|
|
|
std::vector<liquid_float_complex> resampled_data;
|
|
|
|
std::vector<liquid_float_complex> agc_data;
|
|
|
|
std::vector<float> demod_output;
|
2014-12-26 20:58:42 -05:00
|
|
|
std::vector<float> demod_output_stereo;
|
2014-12-24 03:03:34 -05:00
|
|
|
std::vector<float> resampled_audio_output;
|
2014-12-26 20:58:42 -05:00
|
|
|
std::vector<float> resampled_audio_output_stereo;
|
|
|
|
|
|
|
|
double freq_index = 0;
|
2014-12-24 03:03:34 -05:00
|
|
|
|
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;
|
2014-12-26 21:55:13 -05:00
|
|
|
stereo_resampler = inp->stereo_resampler;
|
2014-12-23 01:12:14 -05:00
|
|
|
} else if (resampler != inp->resampler) {
|
2014-12-16 21:30:03 -05:00
|
|
|
msresamp_crcf_destroy(resampler);
|
2014-12-21 17:37:41 -05:00
|
|
|
msresamp_rrrf_destroy(audio_resampler);
|
2014-12-26 21:55:13 -05:00
|
|
|
msresamp_rrrf_destroy(stereo_resampler);
|
2014-12-23 01:12:14 -05:00
|
|
|
resampler = inp->resampler;
|
|
|
|
audio_resampler = inp->audio_resampler;
|
2014-12-26 21:55:13 -05:00
|
|
|
stereo_resampler = inp->stereo_resampler;
|
2014-12-16 21:30:03 -05:00
|
|
|
}
|
2014-12-16 20:33:44 -05:00
|
|
|
|
2014-12-23 01:12:14 -05:00
|
|
|
int out_size = ceil((float) (bufSize) * inp->resample_ratio);
|
2014-12-24 03:03:34 -05:00
|
|
|
|
|
|
|
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
|
|
|
|
2014-12-16 20:33:44 -05:00
|
|
|
unsigned int num_written;
|
2014-12-24 03:03:34 -05:00
|
|
|
msresamp_crcf_execute(resampler, &(inp->data[0]), bufSize, &resampled_data[0], &num_written);
|
2014-12-22 19:43:56 -05:00
|
|
|
|
2014-12-24 03:03:34 -05:00
|
|
|
agc_crcf_execute_block(agc, &resampled_data[0], num_written, &agc_data[0]);
|
2014-12-21 16:08:32 -05:00
|
|
|
|
2014-12-23 01:12:14 -05:00
|
|
|
float audio_resample_ratio = inp->audio_resample_ratio;
|
2014-12-16 18:27:32 -05:00
|
|
|
|
2014-12-24 03:03:34 -05:00
|
|
|
if (demod_output.size() != num_written) {
|
|
|
|
if (demod_output.capacity() < num_written) {
|
|
|
|
demod_output.reserve(num_written);
|
|
|
|
}
|
|
|
|
demod_output.resize(num_written);
|
|
|
|
}
|
|
|
|
|
2014-12-27 12:45:21 -05:00
|
|
|
int audio_out_size = ceil((float) (num_written) * audio_resample_ratio);
|
|
|
|
|
2014-12-24 03:03:34 -05:00
|
|
|
freqdem_demodulate_block(fdem, &agc_data[0], num_written, &demod_output[0]);
|
2014-12-16 18:27:32 -05:00
|
|
|
|
2014-12-27 12:45:21 -05:00
|
|
|
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);
|
|
|
|
}
|
|
|
|
|
|
|
|
unsigned int num_audio_written;
|
|
|
|
msresamp_rrrf_execute(audio_resampler, &demod_output[0], num_written, &resampled_audio_output[0], &num_audio_written);
|
|
|
|
|
2014-12-26 20:58:42 -05:00
|
|
|
if (stereo) {
|
2014-12-27 12:45:21 -05:00
|
|
|
if (demod_output_stereo.size() != num_written) {
|
|
|
|
if (demod_output_stereo.capacity() < num_written) {
|
|
|
|
demod_output_stereo.reserve(num_written);
|
|
|
|
}
|
|
|
|
demod_output_stereo.resize(num_written);
|
|
|
|
}
|
2014-12-26 20:58:42 -05:00
|
|
|
|
2014-12-27 12:23:09 -05:00
|
|
|
double freq = (2.0 * M_PI) * (((float) abs(38000)) / ((float) inp->bandwidth));
|
2014-12-26 20:58:42 -05:00
|
|
|
|
2014-12-27 12:23:09 -05:00
|
|
|
if (shift_freq != freq) {
|
|
|
|
nco_crcf_set_frequency(nco_shift, freq);
|
|
|
|
shift_freq = freq;
|
|
|
|
}
|
|
|
|
|
|
|
|
for (int i = 0; i < num_written; i++) {
|
2014-12-27 12:45:21 -05:00
|
|
|
firhilbf_r2c_execute(firR2C, demod_output[i], &x);
|
2014-12-27 12:23:09 -05:00
|
|
|
nco_crcf_mix_down(nco_shift, x, &y);
|
|
|
|
nco_crcf_step(nco_shift);
|
2014-12-27 12:45:21 -05:00
|
|
|
firhilbf_c2r_execute(firR2C, y, &demod_output_stereo[i]);
|
2014-12-26 20:58:42 -05:00
|
|
|
}
|
2014-12-24 03:03:34 -05:00
|
|
|
|
2014-12-27 12:45:21 -05:00
|
|
|
if (audio_out_size != resampled_audio_output_stereo.size()) {
|
|
|
|
if (resampled_audio_output_stereo.capacity() < audio_out_size) {
|
|
|
|
resampled_audio_output_stereo.reserve(audio_out_size);
|
|
|
|
}
|
|
|
|
resampled_audio_output_stereo.resize(audio_out_size);
|
2014-12-24 03:03:34 -05:00
|
|
|
}
|
2014-12-16 18:27:32 -05:00
|
|
|
|
2014-12-26 21:55:13 -05:00
|
|
|
msresamp_rrrf_execute(stereo_resampler, &demod_output_stereo[0], num_written, &resampled_audio_output_stereo[0], &num_audio_written);
|
2014-12-26 20:58:42 -05:00
|
|
|
}
|
|
|
|
|
2014-12-16 21:30:03 -05:00
|
|
|
if (audioInputQueue != NULL) {
|
2014-12-21 16:08:32 -05:00
|
|
|
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);
|
2014-12-26 20:58:42 -05:00
|
|
|
|
|
|
|
if (stereo) {
|
|
|
|
ati->channels = 2;
|
2014-12-27 12:45:21 -05:00
|
|
|
if (ati->data.capacity() < (num_audio_written * 2)) {
|
|
|
|
ati->data.reserve(num_audio_written * 2);
|
|
|
|
}
|
2014-12-26 22:20:50 -05:00
|
|
|
ati->data.resize(num_audio_written * 2);
|
2014-12-26 20:58:42 -05:00
|
|
|
for (int i = 0; i < num_audio_written; i++) {
|
2014-12-26 22:20:50 -05:00
|
|
|
ati->data[i * 2] = (resampled_audio_output[i] - (resampled_audio_output_stereo[i]));
|
|
|
|
ati->data[i * 2 + 1] = (resampled_audio_output[i] + (resampled_audio_output_stereo[i]));
|
2014-12-26 20:58:42 -05:00
|
|
|
}
|
|
|
|
} else {
|
|
|
|
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
|
|
|
|
2014-12-21 16:08:32 -05:00
|
|
|
audioInputQueue->push(ati);
|
|
|
|
}
|
2014-12-16 21:30:03 -05:00
|
|
|
}
|
2014-12-16 18:27:32 -05:00
|
|
|
|
2014-12-18 20:11:25 -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;
|
2014-12-21 17:37:41 -05:00
|
|
|
|
2014-12-21 16:08:32 -05:00
|
|
|
int num_vis = DEMOD_VIS_SIZE;
|
2014-12-26 22:20:50 -05:00
|
|
|
if (stereo) {
|
|
|
|
|
|
|
|
int stereoSize = resampled_audio_output.size();
|
|
|
|
if (stereoSize > DEMOD_VIS_SIZE) {
|
|
|
|
stereoSize = DEMOD_VIS_SIZE;
|
|
|
|
}
|
|
|
|
ati_vis->data.resize(stereoSize);
|
2014-12-27 12:45:21 -05:00
|
|
|
ati_vis->channels = stereo ? 2 : 1;
|
2014-12-26 22:20:50 -05:00
|
|
|
|
|
|
|
for (int i = 0; i < stereoSize / 2; i++) {
|
|
|
|
ati_vis->data[i] = (resampled_audio_output[i] - (resampled_audio_output_stereo[i]));
|
|
|
|
ati_vis->data[i + stereoSize / 2] = (resampled_audio_output[i] + (resampled_audio_output_stereo[i]));
|
2014-12-21 16:08:32 -05:00
|
|
|
}
|
2014-12-18 21:39:32 -05:00
|
|
|
} else {
|
2014-12-26 22:20:50 -05:00
|
|
|
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);
|
2014-12-21 16:08:32 -05:00
|
|
|
}
|
2014-12-26 22:20:50 -05:00
|
|
|
|
|
|
|
// std::cout << "Signal: " << agc_crcf_get_signal_level(agc) << " -- " << agc_crcf_get_rssi(agc) << "dB " << std::endl;
|
2014-12-21 16:08:32 -05:00
|
|
|
}
|
2014-12-21 17:37:41 -05:00
|
|
|
|
2014-12-21 16:08:32 -05:00
|
|
|
visOutQueue->push(ati_vis);
|
|
|
|
}
|
|
|
|
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;
|
2014-12-21 16:08:32 -05:00
|
|
|
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-18 21:39:32 -05:00
|
|
|
}
|
2014-12-16 21:30:03 -05:00
|
|
|
}
|
2014-12-21 16:08:32 -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) {
|
2014-12-21 17:37:41 -05:00
|
|
|
msresamp_rrrf_destroy(audio_resampler);
|
2014-12-16 21:30:03 -05:00
|
|
|
}
|
2014-12-26 21:55:13 -05:00
|
|
|
if (stereo_resampler != NULL) {
|
|
|
|
msresamp_rrrf_destroy(stereo_resampler);
|
|
|
|
}
|
2014-12-16 20:33:44 -05:00
|
|
|
|
2014-12-21 16:08:32 -05:00
|
|
|
agc_crcf_destroy(agc);
|
2014-12-27 12:23:09 -05:00
|
|
|
firhilbf_destroy(firR2C);
|
|
|
|
firhilbf_destroy(firC2R);
|
|
|
|
nco_crcf_destroy(nco_shift);
|
2014-12-21 16:08:32 -05:00
|
|
|
|
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
|
|
|
}
|
2014-12-26 20:58:42 -05:00
|
|
|
|
|
|
|
void DemodulatorThread::setStereo(bool state) {
|
|
|
|
stereo = state;
|
2014-12-26 22:20:50 -05:00
|
|
|
std::cout << "Stereo " << (state ? "Enabled" : "Disabled") << std::endl;
|
2014-12-26 20:58:42 -05:00
|
|
|
}
|
|
|
|
|
|
|
|
bool DemodulatorThread::isStereo() {
|
|
|
|
return stereo;
|
|
|
|
}
|