ModemKit *ModemFMStereo::buildKit(long long sampleRate, int audioSampleRate) {
ModemKitFMStereo *kit = new ModemKitFMStereo;
kit->audioResampleRatio = double(audioSampleRate) / double(sampleRate);
kit->sampleRate = sampleRate;
kit->audioSampleRate = audioSampleRate;
float As = 60.0f; // stop-band attenuation [dB]
kit->audioResampler = msresamp_rrrf_create(kit->audioResampleRatio, As);
kit->stereoResampler = msresamp_rrrf_create(kit->audioResampleRatio, As);
// Stereo filters / shifters
double firStereoCutoff = 16000.0 / double(audioSampleRate);
// filter transition
float ft = 1000.0f / double(audioSampleRate);
// fractional timing offset
float mu = 0.0f;
if (firStereoCutoff < 0) {
firStereoCutoff = 0;
}
if (firStereoCutoff > 0.5) {
firStereoCutoff = 0.5;
}
unsigned int h_len = estimate_req_filter_len(ft, As);
float *h = new float[h_len];
liquid_firdes_kaiser(h_len, firStereoCutoff, As, mu, h);
kit->firStereoLeft = firfilt_rrrf_create(h, h_len);
kit->firStereoRight = firfilt_rrrf_create(h, h_len);
// stereo pilot filter
float bw = float(sampleRate);
if (bw < 100000.0) {
bw = 100000.0;
}
unsigned int order = 5; // filter order
float f0 = ((float) 19000 / bw);
float fc = ((float) 19500 / bw);
float Ap = 1.0f;
kit->iirStereoPilot = iirfilt_crcf_create_prototype(LIQUID_IIRDES_CHEBY2, LIQUID_IIRDES_BANDPASS, LIQUID_IIRDES_SOS, order, fc, f0, Ap, As);
kit->firStereoR2C = firhilbf_create(5, 60.0f);
kit->firStereoC2R = firhilbf_create(5, 60.0f);
kit->stereoPilot = nco_crcf_create(LIQUID_VCO);
nco_crcf_reset(kit->stereoPilot);
nco_crcf_pll_set_bandwidth(kit->stereoPilot, 0.25f);
return kit;
}
void DemodulatorPreThread::initialize() {
initialized = false;
iqResampleRatio = (double) (params.bandwidth) / (double) params.sampleRate;
audioResampleRatio = (double) (params.audioSampleRate) / (double) params.bandwidth;
float As = 120.0f; // stop-band attenuation [dB]
iqResampler = msresamp_crcf_create(iqResampleRatio, As);
audioResampler = msresamp_rrrf_create(audioResampleRatio, As);
stereoResampler = msresamp_rrrf_create(audioResampleRatio, As);
// Stereo filters / shifters
double firStereoCutoff = ((double) 16000 / (double) params.audioSampleRate);
float ft = ((double) 1000 / (double) params.audioSampleRate); // filter transition
float mu = 0.0f; // fractional timing offset
if (firStereoCutoff < 0) {
firStereoCutoff = 0;
}
if (firStereoCutoff > 0.5) {
firStereoCutoff = 0.5;
}
unsigned int h_len = estimate_req_filter_len(ft, As);
float *h = new float[h_len];
liquid_firdes_kaiser(h_len, firStereoCutoff, As, mu, h);
firStereoLeft = firfilt_rrrf_create(h, h_len);
firStereoRight = firfilt_rrrf_create(h, h_len);
// stereo pilot filter
float bw = params.bandwidth;
if (bw < 100000.0) {
bw = 100000.0;
}
unsigned int order = 5; // filter order
float f0 = ((double) 19000 / bw);
float fc = ((double) 19500 / bw);
float Ap = 1.0f;
As = 60.0f;
iirStereoPilot = iirfilt_crcf_create_prototype(LIQUID_IIRDES_CHEBY2, LIQUID_IIRDES_BANDPASS, LIQUID_IIRDES_SOS, order, fc, f0, Ap, As);
initialized = true;
lastParams = params;
}
// create multi-stage half-band resampler
// _type : resampler type (e.g. LIQUID_RESAMP_DECIM)
// _num_stages : number of resampling stages
// _fc : filter cut-off frequency 0 < _fc < 0.5
// _f0 : filter center frequency
// _As : stop-band attenuation [dB]
MSRESAMP2() MSRESAMP2(_create)(int _type,
unsigned int _num_stages,
float _fc,
float _f0,
float _As)
{
// validate input
if (_num_stages > 10) {
fprintf(stderr,"error: msresamp2_%s_create(), number of stages should not exceed 10\n", EXTENSION_FULL);
exit(1);
}
// ensure cut-off frequency is valid
if ( _fc <= 0.0f || _fc >= 0.5f ) {
fprintf(stderr,"error: msresamp2_%s_create(), cut-off frequency must be in (0,0.5)\n", EXTENSION_FULL);
exit(1);
} else if ( _fc > 0.45f ) {
fprintf(stderr,"warning: msresamp2_%s_create(), cut-off frequency greater than 0.45\n", EXTENSION_FULL);
fprintf(stderr," >> truncating to 0.45\n");
_fc = 0.45f;
}
// check center frequency
if ( _f0 != 0. ) {
fprintf(stderr,"warning: msresamp2_%s_create(), non-zero center frequency not yet supported\n", EXTENSION_FULL);
_f0 = 0.;
}
unsigned int i;
// create object
MSRESAMP2() q = (MSRESAMP2()) malloc(sizeof(struct MSRESAMP2(_s)));
// set internal properties
q->type = _type == LIQUID_RESAMP_INTERP ? LIQUID_RESAMP_INTERP : LIQUID_RESAMP_DECIM;
q->num_stages = _num_stages;
q->fc = _fc;
q->f0 = _f0;
q->As = _As;
// derived values
q->M = 1 << q->num_stages;
q->zeta = 1.0f / (float)(q->M);
// allocate memory for buffers
q->buffer0 = (T*) malloc( q->M * sizeof(T) );
q->buffer1 = (T*) malloc( q->M * sizeof(T) );
// allocate arrays for half-band resampler parameters
q->fc_stage = (float*) malloc(q->num_stages*sizeof(float) );
q->f0_stage = (float*) malloc(q->num_stages*sizeof(float) );
q->As_stage = (float*) malloc(q->num_stages*sizeof(float) );
q->m_stage = (unsigned int*) malloc(q->num_stages*sizeof(unsigned int));
// determine half-band resampler parameters
float fc = q->fc;
float f0 = q->f0;
for (i=0; i<q->num_stages; i++) {
// compute parameters based on filter requirements;
f0 = 0.5f*f0; // update center frequency
fc = 0.5f*fc; // update cutoff frequency
// estiamte required filter length
float ft = (0.5f - fc)/2.0f;
unsigned int h_len = estimate_req_filter_len(ft, q->As);
unsigned int m = ceilf( (float)(h_len-1) / 4.0f );
q->fc_stage[i] = fc; // filter cut-of
q->f0_stage[i] = f0; // filter center frequency
q->As_stage[i] = q->As; // filter stop-band attenuation
q->m_stage[i] = m < 3 ? 3 : m; // minimum 2
}
// create half-band resampler objects
q->resamp2 = (RESAMP2()*) malloc(q->num_stages*sizeof(RESAMP2()));
for (i=0; i<q->num_stages; i++) {
// create half-band resampler
q->resamp2[i] = RESAMP2(_create)(q->m_stage[i],
q->f0_stage[i],
q->As_stage[i]);
}
// reset object
MSRESAMP2(_reset)(q);
// return main object
return q;
}
void DemodulatorWorkerThread::threadMain() {
std::cout << "Demodulator worker thread started.." << std::endl;
while (!terminated) {
bool filterChanged = false;
DemodulatorWorkerThreadCommand filterCommand;
DemodulatorWorkerThreadCommand command;
bool done = false;
while (!done) {
commandQueue->pop(command);
switch (command.cmd) {
case DemodulatorWorkerThreadCommand::DEMOD_WORKER_THREAD_CMD_BUILD_FILTERS:
filterChanged = true;
filterCommand = command;
break;
default:
break;
}
done = commandQueue->empty();
}
if (filterChanged && !terminated) {
DemodulatorWorkerThreadResult result(DemodulatorWorkerThreadResult::DEMOD_WORKER_THREAD_RESULT_FILTERS);
float As = 60.0f; // stop-band attenuation [dB]
if (filterCommand.sampleRate && filterCommand.bandwidth) {
result.iqResampleRatio = (double) (filterCommand.bandwidth) / (double) filterCommand.sampleRate;
result.iqResampler = msresamp_crcf_create(result.iqResampleRatio, As);
}
if (filterCommand.bandwidth && filterCommand.audioSampleRate) {
result.audioResamplerRatio = (double) (filterCommand.audioSampleRate) / (double) filterCommand.bandwidth;
result.audioResampler = msresamp_rrrf_create(result.audioResamplerRatio, As);
result.stereoResampler = msresamp_rrrf_create(result.audioResamplerRatio, As);
result.audioSampleRate = filterCommand.audioSampleRate;
// Stereo filters / shifters
double firStereoCutoff = ((double) 16000 / (double) filterCommand.audioSampleRate);
float ft = ((double) 1000 / (double) filterCommand.audioSampleRate); // filter transition
float mu = 0.0f; // fractional timing offset
if (firStereoCutoff < 0) {
firStereoCutoff = 0;
}
if (firStereoCutoff > 0.5) {
firStereoCutoff = 0.5;
}
unsigned int h_len = estimate_req_filter_len(ft, As);
float *h = new float[h_len];
liquid_firdes_kaiser(h_len, firStereoCutoff, As, mu, h);
result.firStereoLeft = firfilt_rrrf_create(h, h_len);
result.firStereoRight = firfilt_rrrf_create(h, h_len);
float bw = filterCommand.bandwidth;
if (bw < 100000.0) {
bw = 100000.0;
}
// stereo pilot filter
unsigned int order = 5; // filter order
float f0 = ((double) 19000 / bw);
float fc = ((double) 19500 / bw);
float Ap = 1.0f;
As = 60.0f;
result.iirStereoPilot = iirfilt_crcf_create_prototype(LIQUID_IIRDES_CHEBY2, LIQUID_IIRDES_BANDPASS, LIQUID_IIRDES_SOS, order, fc, f0, Ap, As);
}
if (filterCommand.bandwidth) {
result.bandwidth = filterCommand.bandwidth;
}
if (filterCommand.sampleRate) {
result.sampleRate = filterCommand.sampleRate;
}
resultQueue->push(result);
}
}
std::cout << "Demodulator worker thread done." << std::endl;
}
// create dds object
// _num_stages : number of halfband stages
// _fc : input carrier
// _bw : input signal bandwidth
// _As : stop-band attenuation
DDS() DDS(_create)(unsigned int _num_stages,
float _fc,
float _bw,
float _As)
{
// create object
DDS() q = (DDS()) malloc(sizeof(struct DDS(_s)));
q->num_stages = _num_stages;
q->rate = 1<<(q->num_stages);
q->fc0 = _fc;
q->bw0 = _bw;
q->As0 = _As;
// error checking
if (q->fc0 > 0.5f || q->fc0 < -0.5f) {
fprintf(stderr,"error: dds_xxxf_create(), frequency %12.4e is out of range [-0.5,0.5]\n", q->fc0);
exit(1);
}
// allocate memory for filter properties
q->fc = (float*) malloc((q->num_stages)*sizeof(float));
q->ft = (float*) malloc((q->num_stages)*sizeof(float));
q->As = (float*) malloc((q->num_stages)*sizeof(float));
q->h_len = (unsigned int*) malloc((q->num_stages)*sizeof(unsigned int));
unsigned int i;
float fc, bw;
fc = 0.5*(1<<q->num_stages)*q->fc0; // filter center frequency
bw = q->bw0; // signal bandwidth
// TODO : compute/set filter bandwidths, lengths appropriately
for (i=0; i<q->num_stages; i++) {
q->fc[i] = fc;
while (q->fc[i] > 0.5f) q->fc[i] -= 1.0f;
while (q->fc[i] < -0.5f) q->fc[i] += 1.0f;
// compute transition bandwidth
q->ft[i] = 0.5f*(1.0f - bw);
if (q->ft[i] > 0.45) q->ft[i] = 0.45f; // set maximum bandwidth
q->As[i] = q->As0;
// compute (estimate) required filter length
//q->h_len[i] = i==0 ? 37 : q->h_len[i-1]*0.7;
q->h_len[i] = estimate_req_filter_len(q->ft[i], q->As[i]);
if ((q->h_len[i] % 2) == 0) q->h_len[i]++;
// ensure h_len[i] is of form 4*m+1
unsigned int m = (q->h_len[i]-1)/4;
if (m < 1) m = 1;
q->h_len[i] = 4*m+1;
// update carrier, bandwidth parameters
fc *= 0.5f;
bw *= 0.5f;
}
// allocate memory for buffering
q->buffer_len = q->rate;
q->buffer0 = (T*) malloc((q->buffer_len)*sizeof(T));
q->buffer1 = (T*) malloc((q->buffer_len)*sizeof(T));
// allocate memory for resampler pointers and create objects
q->halfband_resamp = (RESAMP2()*) malloc((q->num_stages)*sizeof(RESAMP()*));
for (i=0; i<q->num_stages; i++) {
q->halfband_resamp[i] = RESAMP2(_create)(q->h_len[i],
q->fc[i],
q->As[i]);
}
// set down-converter scaling factor
q->zeta = 1.0f / ((float)(q->rate));
// create NCO and set frequency
q->ncox = NCO(_create)(LIQUID_VCO);
// TODO : ensure range is in [-pi,pi]
NCO(_set_frequency)(q->ncox, 2*M_PI*(q->rate)*(q->fc0));
return q;
}
