CubicSDR/src/DemodulatorThread.cpp
Charles J. Cliffe 739854cd7f Demodulator and Audio threads working
Data now flows from SDR IQ Thread -> Demodulator Thread -> Audio Thread.
Seems to have added a significant audio delay though..
2014-11-16 17:33:54 -05:00

174 lines
7.1 KiB
C++

#include "DemodulatorThread.h"
#include "CubicSDRDefs.h"
#include <vector>
//wxDEFINE_EVENT(wxEVT_COMMAND_DemodulatorThread_INPUT, wxThreadEvent);
DemodulatorThread::DemodulatorThread(DemodulatorThreadQueue* pQueue, int id) :
wxThread(wxTHREAD_DETACHED), m_pQueue(pQueue), m_ID(id) {
bandwidth = 200000;
resample_ratio = (float) (bandwidth) / (float) SRATE;
wbfm_frequency = 100000;
wbfm_resample_ratio = (float) (wbfm_frequency) / (float) bandwidth;
audio_frequency = AUDIO_FREQUENCY;
audio_resample_ratio = (float) (audio_frequency) / (float) wbfm_frequency;
float fc = 0.5f * ((float) bandwidth / (float) SRATE) * 0.75; // filter cutoff frequency
float ft = 0.05f; // filter transition
float As = 60.0f; // stop-band attenuation [dB]
float mu = 0.0f; // fractional timing offset
// estimate required filter length and generate filter
unsigned int h_len = estimate_req_filter_len(ft, As);
float h[h_len];
liquid_firdes_kaiser(h_len, fc, As, mu, h);
fir_filter = firfilt_crcf_create(h, h_len);
h_len = estimate_req_filter_len(ft, As);
liquid_firdes_kaiser(h_len, 32000.0 / (float) wbfm_frequency, As, mu, h);
fir_audio_filter = firfilt_crcf_create(h, h_len);
// create multi-stage arbitrary resampler object
resampler = msresamp_crcf_create(resample_ratio, As);
msresamp_crcf_print(resampler);
wbfm_resampler = msresamp_crcf_create(wbfm_resample_ratio, As);
msresamp_crcf_print(wbfm_resampler);
audio_resampler = msresamp_crcf_create(audio_resample_ratio, As);
msresamp_crcf_print(audio_resampler);
float kf = 0.75; // modulation factor
fdem = freqdem_create(kf);
freqdem_print(fdem);
}
DemodulatorThread::~DemodulatorThread() {
}
wxThread::ExitCode DemodulatorThread::Entry() {
while (!TestDestroy()) {
if (m_pQueue->stackSize()) {
while (m_pQueue->stackSize()) {
DemodulatorThreadTask task = m_pQueue->pop(); // pop a task from the queue. this will block the worker thread if queue is empty
switch (task.m_cmd) {
case DemodulatorThreadTask::DEMOD_THREAD_DATA:
std::vector<signed char> *data = &task.getData();
if (data->size()) {
liquid_float_complex filtered_input[BUF_SIZE / 2];
for (int i = 0; i < BUF_SIZE / 2; i++) {
liquid_float_complex x;
liquid_float_complex y;
x.real = (float) (*data)[i * 2] / 127.0f;
x.imag = (float) (*data)[i * 2 + 1] / 127.0f;
firfilt_crcf_push(fir_filter, x); // push input sample
firfilt_crcf_execute(fir_filter, &y); // compute output
filtered_input[i] = y;
}
int out_size = ceil((float) (BUF_SIZE / 2) * resample_ratio);
liquid_float_complex resampled_output[out_size];
unsigned int num_written; // number of values written to buffer
msresamp_crcf_execute(resampler, filtered_input, (BUF_SIZE / 2), resampled_output, &num_written);
float waveform_ceil = 0, waveform_floor = 0;
float pcm = 0;
for (int i = 0; i < num_written; i++) {
freqdem_demodulate(fdem, resampled_output[i], &pcm);
resampled_output[i].real = (float) pcm;
resampled_output[i].imag = 0;
if (waveform_ceil < resampled_output[i].real) {
waveform_ceil = resampled_output[i].real;
}
if (waveform_floor > resampled_output[i].real) {
waveform_floor = resampled_output[i].real;
}
}
int wbfm_out_size = ceil((float) (num_written) * wbfm_resample_ratio);
liquid_float_complex resampled_wbfm_output[wbfm_out_size];
unsigned int num_wbfm_written;
msresamp_crcf_execute(wbfm_resampler, resampled_output, num_written, resampled_wbfm_output, &num_wbfm_written);
for (int i = 0; i < num_wbfm_written; i++) {
firfilt_crcf_push(fir_audio_filter, resampled_wbfm_output[i]);
firfilt_crcf_execute(fir_audio_filter, &resampled_wbfm_output[i]);
}
int audio_out_size = ceil((float) (num_wbfm_written) * audio_resample_ratio);
liquid_float_complex resampled_audio_output[audio_out_size];
unsigned int num_audio_written;
msresamp_crcf_execute(audio_resampler, resampled_wbfm_output, num_wbfm_written, resampled_audio_output, &num_audio_written);
std::vector<float> *newBuffer = new std::vector<float>;
newBuffer->resize(num_audio_written * 2);
for (int i = 0; i < num_audio_written; i++) {
liquid_float_complex y = resampled_audio_output[i];
(*newBuffer)[i * 2] = y.real;
(*newBuffer)[i * 2 + 1] = y.real;
}
if (!TestDestroy()) {
wxThreadEvent event(wxEVT_THREAD, EVENT_DEMOD_INPUT);
event.SetPayload(newBuffer);
wxQueueEvent(m_pQueue->getHandler(), event.Clone());
} else {
delete newBuffer;
}
// std::vector<float> *newBuffer = new std::vector<float>;
// newBuffer->resize(num_audio_written * 2);
// for (int i = 0; i < num_audio_written; i++) {
// liquid_float_complex y = resampled_audio_output[i];
//
// (*newBuffer)[i * 2] = y.real;
// (*newBuffer)[i * 2 + 1] = y.real;
// }
// if (waveform_points.size() != num_audio_written * 2) {
// waveform_points.resize(num_audio_written * 2);
// }
//
// for (int i = 0, iMax = waveform_points.size() / 2; i < iMax; i++) {
// waveform_points[i * 2 + 1] = resampled_audio_output[i].real * 0.5f;
// waveform_points[i * 2] = ((double) i / (double) iMax);
// }
}
// audio_queue.push(newBuffer);
break;
}
}
}
Sleep(1000);
}
std::cout << std::endl << "Demodulator Thread Done." << std::endl << std::endl;
return (wxThread::ExitCode) 0;
}