void autotest_nco_crcf_mix_block_up()
{
// options
unsigned int buf_len = 4096;
float phase = 0.7123f;
float freq = 0; //0.1324f;
float tol = 1e-2f;
// create object
nco_crcf nco = nco_crcf_create(LIQUID_NCO);
nco_crcf_set_phase (nco, phase);
nco_crcf_set_frequency(nco, freq);
// generate signal
float complex buf_0[buf_len];
float complex buf_1[buf_len];
unsigned int i;
for (i=0; i<buf_len; i++)
buf_0[i] = cexpf(_Complex_I*2*M_PI*randf());
// mix signal
nco_crcf_mix_block_up(nco, buf_0, buf_1, buf_len);
// compare result to expected
float phi = phase;
for (i=0; i<buf_len; i++) {
float complex v = buf_0[i] * cexpf(_Complex_I*phi);
CONTEND_DELTA( crealf(buf_1[i]), crealf(v), tol);
CONTEND_DELTA( cimagf(buf_1[i]), cimagf(v), tol);
phi += freq;
}
// destroy object
nco_crcf_destroy(nco);
}
// modulate sample
// _q : frequency modulator object
// _s : input symbol
// _y : output sample array [size: _k x 1]
void fskmod_modulate(fskmod _q,
unsigned int _s,
float complex * _y)
{
// validate input
if (_s >= _q->M) {
fprintf(stderr,"warning: fskmod_modulate(), input symbol (%u) exceeds maximum (%u)\n",
_s, _q->M);
_s = 0;
}
// compute appropriate frequency
float dphi = ((float)_s - _q->M2) * 2 * M_PI * _q->bandwidth / _q->M2;
// set frequency appropriately
nco_crcf_set_frequency(_q->oscillator, dphi);
// generate output tone
unsigned int i;
for (i=0; i<_q->k; i++) {
// compute complex output
nco_crcf_cexpf(_q->oscillator, &_y[i]);
// step oscillator
nco_crcf_step(_q->oscillator);
}
}
//
// test floating point precision nco
//
void autotest_nco_basic() {
nco_crcf p = nco_crcf_create(LIQUID_NCO);
unsigned int i; // loop index
float s, c; // sine/cosine result
float tol=1e-4f; // error tolerance
float f=0.0f; // frequency to test
nco_crcf_set_phase( p, 0.0f);
CONTEND_DELTA( nco_crcf_cos(p), 1.0f, tol );
CONTEND_DELTA( nco_crcf_sin(p), 0.0f, tol );
nco_crcf_sincos(p, &s, &c);
CONTEND_DELTA( s, 0.0f, tol );
CONTEND_DELTA( c, 1.0f, tol );
nco_crcf_set_phase(p, M_PI/2);
CONTEND_DELTA( nco_crcf_cos(p), 0.0f, tol );
CONTEND_DELTA( nco_crcf_sin(p), 1.0f, tol );
nco_crcf_sincos(p, &s, &c);
CONTEND_DELTA( s, 1.0f, tol );
CONTEND_DELTA( c, 0.0f, tol );
// cycle through one full period in 64 steps
nco_crcf_set_phase(p, 0.0f);
f = 2.0f * M_PI / 64.0f;
nco_crcf_set_frequency(p, f);
for (i=0; i<128; i++) {
nco_crcf_sincos(p, &s, &c);
CONTEND_DELTA( s, sinf(i*f), tol );
CONTEND_DELTA( c, cosf(i*f), tol );
nco_crcf_step(p);
}
// double frequency: cycle through one full period in 32 steps
nco_crcf_set_phase(p, 0.0f);
f = 2.0f * M_PI / 32.0f;
nco_crcf_set_frequency(p, f);
for (i=0; i<128; i++) {
nco_crcf_sincos(p, &s, &c);
CONTEND_DELTA( s, sinf(i*f), tol );
CONTEND_DELTA( c, cosf(i*f), tol );
nco_crcf_step(p);
}
// destroy NCO object
nco_crcf_destroy(p);
}
// frame detection
void wlanframesync_execute_rxshort1(wlanframesync _q)
{
_q->timer++;
if (_q->timer < 16)
return;
// reset timer
_q->timer = 0;
// read contents of input buffer
float complex * rc;
windowcf_read(_q->input_buffer, &rc);
// estimate S0 gain
wlanframesync_estimate_gain_S0(_q, &rc[16], _q->G0b);
float complex s_hat;
wlanframesync_S0_metrics(_q, _q->G0b, &s_hat);
//float g = agc_crcf_get_gain(_q->agc_rx);
s_hat *= _q->g0;
// save second 'short' symbol statistic
_q->s0b_hat = s_hat;
#if DEBUG_WLANFRAMESYNC_PRINT
float tau_hat = cargf(s_hat) * 16.0f / (2*M_PI);
printf("********** S0[b] received ************\n");
printf(" s_hat : %12.8f <%12.8f>\n", cabsf(s_hat), cargf(s_hat));
printf(" tau_hat : %12.8f\n", tau_hat);
// new timing offset estimate
tau_hat = cargf(_q->s0a_hat + _q->s0b_hat) * 16.0f / (2*M_PI);
printf(" tau_hat * : %12.8f\n", tau_hat);
#endif
#if 0
// compute carrier frequency offset estimate using ML method
float complex t0 = 0.0f;
for (i=0; i<48; i++) {
t0 += conjf(rc[i]) * wlanframe_s0[i] *
rc[i+16] * conjf(wlanframe_s0[i+16]);
}
float nu_hat = cargf(t0) / (float)(_q->M2);
#else
// compute carrier frequency offset estimate using freq. domain method
float nu_hat = wlanframesync_estimate_cfo_S0(_q->G0a, _q->G0b);
#endif
// set NCO frequency
nco_crcf_set_frequency(_q->nco_rx, nu_hat);
#if DEBUG_WLANFRAMESYNC_PRINT
printf(" nu_hat[0]: %12.8f\n", nu_hat);
#endif
// set state
_q->state = WLANFRAMESYNC_STATE_RXLONG0;
}
void ofdmoqamframe64sync_execute_plcpshort(ofdmoqamframe64sync _q, float complex _x)
{
// run AGC, clip output
float complex y;
agc_crcf_execute(_q->sigdet, _x, &y);
//if (agc_crcf_get_signal_level(_q->sigdet) < -15.0f)
// return;
if (cabsf(y) > 2.0f)
y = 2.0f*liquid_cexpjf(cargf(y));
// auto-correlators
//autocorr_cccf_push(_q->autocorr, _x);
autocorr_cccf_push(_q->autocorr, y);
autocorr_cccf_execute(_q->autocorr, &_q->rxx);
#if DEBUG_OFDMOQAMFRAME64SYNC
windowcf_push(_q->debug_rxx, _q->rxx);
windowf_push(_q->debug_rssi, agc_crcf_get_signal_level(_q->sigdet));
#endif
float rxx_mag = cabsf(_q->rxx);
float threshold = (_q->rxx_thresh)*(_q->autocorr_length);
if (rxx_mag > threshold) {
// wait for auto-correlation to peak before changing state
if (rxx_mag > _q->rxx_mag_max) {
_q->rxx_mag_max = rxx_mag;
return;
}
// estimate CFO
_q->nu_hat = cargf(_q->rxx);
if (_q->nu_hat > M_PI/2.0f) _q->nu_hat -= M_PI;
if (_q->nu_hat < -M_PI/2.0f) _q->nu_hat += M_PI;
_q->nu_hat *= 4.0f / (float)(_q->num_subcarriers);
#if DEBUG_OFDMOQAMFRAME64SYNC_PRINT
printf("rxx = |%12.8f| arg{%12.8f}\n", cabsf(_q->rxx),cargf(_q->rxx));
printf("nu_hat = %12.8f\n", _q->nu_hat);
#endif
nco_crcf_set_frequency(_q->nco_rx, _q->nu_hat);
_q->state = OFDMOQAMFRAME64SYNC_STATE_PLCPLONG0;
_q->g = agc_crcf_get_gain(_q->sigdet);
#if DEBUG_OFDMOQAMFRAME64SYNC_PRINT
printf("gain : %f\n", _q->g);
#endif
_q->timer=0;
}
}
// push buffered p/n sequence through synchronizer
void gmskframesync_pushpn(gmskframesync _q)
{
unsigned int i;
// reset filterbanks
firpfb_rrrf_reset(_q->mf);
firpfb_rrrf_reset(_q->dmf);
// read buffer
float complex * rc;
windowcf_read(_q->buffer, &rc);
// compute delay and filterbank index
// tau_hat < 0 : delay = 2*k*m-1, index = round( tau_hat *npfb), flag = 0
// tau_hat > 0 : delay = 2*k*m-2, index = round((1-tau_hat)*npfb), flag = 0
assert(_q->tau_hat < 0.5f && _q->tau_hat > -0.5f);
unsigned int delay = 2*_q->k*_q->m - 1; // samples to buffer before computing output
_q->pfb_soft = -_q->tau_hat*_q->npfb;
_q->pfb_index = (int) roundf(_q->pfb_soft);
while (_q->pfb_index < 0) {
delay -= 1;
_q->pfb_index += _q->npfb;
_q->pfb_soft += _q->npfb;
}
_q->pfb_timer = 0;
// set coarse carrier frequency offset
nco_crcf_set_frequency(_q->nco_coarse, _q->dphi_hat);
unsigned int buffer_len = (_q->preamble_len + _q->m) * _q->k;
for (i=0; i<buffer_len; i++) {
if (i < delay) {
float complex y;
nco_crcf_mix_down(_q->nco_coarse, rc[i], &y);
nco_crcf_step(_q->nco_coarse);
// update instantanenous frequency estimate
gmskframesync_update_fi(_q, y);
// push initial samples into filterbanks
firpfb_rrrf_push(_q->mf, _q->fi_hat);
firpfb_rrrf_push(_q->dmf, _q->fi_hat);
} else {
// run remaining samples through p/n sequence recovery
gmskframesync_execute_rxpreamble(_q, rc[i]);
}
}
// set state (still need a few more samples before entire p/n
// sequence has been received)
_q->state = STATE_RXPREAMBLE;
}
quiet_beacon_encoder *quiet_beacon_encoder_create(float sample_rate, float target_freq, float gain) {
quiet_beacon_encoder *e = calloc(1, sizeof(quiet_beacon_encoder));
float omega = (target_freq/sample_rate) * 2 * M_PI;
e->nco = nco_crcf_create(LIQUID_NCO);
nco_crcf_set_phase(e->nco, 0.0f);
nco_crcf_set_frequency(e->nco, omega);
e->gain = gain;
return e;
}
//
// test nco block mixing
//
void autotest_nco_block_mixing()
{
// frequency, phase
float f = 0.1f;
float phi = M_PI;
// error tolerance (high for NCO)
float tol = 0.05f;
unsigned int i;
// number of samples
unsigned int num_samples = 1024;
// store samples
float complex * x = (float complex*)malloc(num_samples*sizeof(float complex));
float complex * y = (float complex*)malloc(num_samples*sizeof(float complex));
// generate complex sin/cos
for (i=0; i<num_samples; i++)
x[i] = cexpf(_Complex_I*(f*i + phi));
// initialize nco object
nco_crcf p = nco_crcf_create(LIQUID_NCO);
nco_crcf_set_frequency(p, f);
nco_crcf_set_phase(p, phi);
// mix signal back to zero phase (in pieces)
unsigned int num_remaining = num_samples;
i = 0;
while (num_remaining > 0) {
unsigned int n = 7 < num_remaining ? 7 : num_remaining;
nco_crcf_mix_block_down(p, &x[i], &y[i], n);
i += n;
num_remaining -= n;
}
// assert mixer output is correct
for (i=0; i<num_samples; i++) {
CONTEND_DELTA( crealf(y[i]), 1.0f, tol );
CONTEND_DELTA( cimagf(y[i]), 0.0f, tol );
}
// free those buffers
free(x);
free(y);
// destroy NCO object
nco_crcf_destroy(p);
}
// execute synchronizer, seeking p/n sequence
// _q : frame synchronizer object
// _x : input sample
// _sym : demodulated symbol
void flexframesync_execute_seekpn(flexframesync _q,
float complex _x)
{
// push through pre-demod synchronizer
float complex * v = qdetector_cccf_execute(_q->detector, _x);
// check if frame has been detected
if (v != NULL) {
// get estimates
_q->tau_hat = qdetector_cccf_get_tau (_q->detector);
_q->gamma_hat = qdetector_cccf_get_gamma(_q->detector);
_q->dphi_hat = qdetector_cccf_get_dphi (_q->detector);
_q->phi_hat = qdetector_cccf_get_phi (_q->detector);
#if DEBUG_FLEXFRAMESYNC_PRINT
printf("***** frame detected! tau-hat:%8.4f, dphi-hat:%8.4f, gamma:%8.2f dB\n",
_q->tau_hat, _q->dphi_hat, 20*log10f(_q->gamma_hat));
#endif
// set appropriate filterbank index
if (_q->tau_hat > 0) {
_q->pfb_index = (unsigned int)( _q->tau_hat * _q->npfb) % _q->npfb;
_q->mf_counter = 0;
} else {
_q->pfb_index = (unsigned int)((1.0f+_q->tau_hat) * _q->npfb) % _q->npfb;
_q->mf_counter = 1;
}
// output filter scale (gain estimate, scaled by 1/2 for k=2 samples/symbol)
firpfb_crcf_set_scale(_q->mf, 0.5f / _q->gamma_hat);
// set frequency/phase of mixer
nco_crcf_set_frequency(_q->mixer, _q->dphi_hat);
nco_crcf_set_phase (_q->mixer, _q->phi_hat );
// update state
_q->state = FLEXFRAMESYNC_STATE_RXPREAMBLE;
#if DEBUG_FLEXFRAMESYNC
// the debug_qdetector_flush prevents samples from being written twice
_q->debug_qdetector_flush = 1;
#endif
// run buffered samples through synchronizer
unsigned int buf_len = qdetector_cccf_get_buf_len(_q->detector);
flexframesync_execute(_q, v, buf_len);
#if DEBUG_FLEXFRAMESYNC
_q->debug_qdetector_flush = 0;
#endif
}
}
//
// test phase-locked loop
// _type : NCO type (e.g. LIQUID_NCO)
// _phase_offset : initial phase offset
// _freq_offset : initial frequency offset
// _pll_bandwidth : bandwidth of phase-locked loop
// _num_iterations : number of iterations to run
// _tol : error tolerance
void nco_crcf_pll_test(int _type,
float _phase_offset,
float _freq_offset,
float _pll_bandwidth,
unsigned int _num_iterations,
float _tol)
{
// objects
nco_crcf nco_tx = nco_crcf_create(_type);
nco_crcf nco_rx = nco_crcf_create(_type);
// initialize objects
nco_crcf_set_phase(nco_tx, _phase_offset);
nco_crcf_set_frequency(nco_tx, _freq_offset);
nco_crcf_pll_set_bandwidth(nco_rx, _pll_bandwidth);
// run loop
unsigned int i;
float phase_error;
float complex r, v;
for (i=0; i<_num_iterations; i++) {
// received complex signal
nco_crcf_cexpf(nco_tx,&r);
nco_crcf_cexpf(nco_rx,&v);
// error estimation
phase_error = cargf(r*conjf(v));
// update pll
nco_crcf_pll_step(nco_rx, phase_error);
// update nco objects
nco_crcf_step(nco_tx);
nco_crcf_step(nco_rx);
}
// ensure phase of oscillators is locked
float nco_tx_phase = nco_crcf_get_phase(nco_tx);
float nco_rx_phase = nco_crcf_get_phase(nco_rx);
CONTEND_DELTA(nco_tx_phase, nco_rx_phase, _tol);
// ensure frequency of oscillators is locked
float nco_tx_freq = nco_crcf_get_frequency(nco_tx);
float nco_rx_freq = nco_crcf_get_frequency(nco_rx);
CONTEND_DELTA(nco_tx_freq, nco_rx_freq, _tol);
// clean it up
nco_crcf_destroy(nco_tx);
nco_crcf_destroy(nco_rx);
}
void PfbSynthesizerComponent::initialize()
{
// design custom filterbank channelizer
unsigned int m = 7; // prototype filter delay
float As = 60.0f; // stop-band attenuation
channelizer = firpfbch_crcf_create_kaiser(LIQUID_SYNTHESIZER, nChans_x, m, As);
// create NCO to center spectrum
float offset = -0.5f*(float)(nChans_x-1) / (float)nChans_x * 2 * M_PI;
nco = nco_crcf_create(LIQUID_VCO);
nco_crcf_set_frequency(nco, offset);
//Set up our input and output buffers
buf.resize(nChans_x);
bufIt = buf.begin();
outBuf.resize(nChans_x);
}
// once p/n symbols are buffered, estimate residual carrier
// frequency and phase offsets, push through fine-tuned NCO
void flexframesync_syncpn(flexframesync _q)
{
unsigned int i;
// estimate residual carrier frequency offset from p/n symbols
float complex dphi_metric = 0.0f;
float complex r0 = 0.0f;
float complex r1 = 0.0f;
for (i=0; i<64; i++) {
r0 = r1;
r1 = _q->preamble_rx[i]*_q->preamble_pn[i];
dphi_metric += r1 * conjf(r0);
}
float dphi_hat = cargf(dphi_metric);
// estimate carrier phase offset from p/n symbols
float complex theta_metric = 0.0f;
for (i=0; i<64; i++)
theta_metric += _q->preamble_rx[i]*liquid_cexpjf(-dphi_hat*i)*_q->preamble_pn[i];
float theta_hat = cargf(theta_metric);
// TODO: compute gain correction factor
// initialize fine-tuned nco
nco_crcf_set_frequency(_q->nco_fine, dphi_hat);
nco_crcf_set_phase( _q->nco_fine, theta_hat);
// correct for carrier offset, pushing through phase-locked loop
for (i=0; i<64; i++) {
// mix signal down
nco_crcf_mix_down(_q->nco_fine, _q->preamble_rx[i], &_q->preamble_rx[i]);
// push through phase-locked loop
float phase_error = cimagf(_q->preamble_rx[i]*_q->preamble_pn[i]);
nco_crcf_pll_step(_q->nco_fine, phase_error);
#if DEBUG_FLEXFRAMESYNC
//_q->debug_nco_phase[i] = nco_crcf_get_phase(_q->nco_fine);
#endif
// update nco phase
nco_crcf_step(_q->nco_fine);
}
}
int main() {
// create the NCO object
nco_crcf p = nco_crcf_create(LIQUID_NCO);
nco_crcf_set_phase(p, 0.0f);
nco_crcf_set_frequency(p, M_PI/10);
unsigned int i;
float s, c;
for (i=0; i<11; i++) {
nco_crcf_sincos(p, &s, &c);
printf(" %3u : %8.5f + j %8.5f\n",
i, c, s);
nco_crcf_step(p);
}
// clean up allocated memory
nco_crcf_destroy(p);
printf("done.\n");
return 0;
}
// create ampmodem object
// _m : modulation index
// _fc : carrier frequency
// _type : AM type (e.g. LIQUID_AMPMODEM_DSB)
// _suppressed_carrier : carrier suppression flag
ampmodem ampmodem_create(float _m,
float _fc,
liquid_ampmodem_type _type,
int _suppressed_carrier)
{
ampmodem q = (ampmodem) malloc(sizeof(struct ampmodem_s));
q->type = _type;
q->m = _m;
q->fc = _fc;
q->suppressed_carrier = (_suppressed_carrier == 0) ? 0 : 1;
// create nco, pll objects
q->oscillator = nco_crcf_create(LIQUID_NCO);
nco_crcf_set_frequency(q->oscillator, 2*M_PI*q->fc);
nco_crcf_pll_set_bandwidth(q->oscillator,0.05f);
// suppressed carrier
q->ssb_alpha = 0.01f;
q->ssb_q_hat = 0.0f;
// single side-band
q->hilbert = firhilbf_create(9, 60.0f);
// double side-band
ampmodem_reset(q);
// debugging
#if DEBUG_AMPMODEM
q->debug_x = windowcf_create(DEBUG_AMPMODEM_BUFFER_LEN);
q->debug_phase_error = windowf_create(DEBUG_AMPMODEM_BUFFER_LEN);
q->debug_freq_error = windowf_create(DEBUG_AMPMODEM_BUFFER_LEN);
#endif
return q;
}
demodulator *demodulator_create(const demodulator_options *opt) {
if (!opt) {
return NULL;
}
demodulator *d = malloc(sizeof(demodulator));
d->opt = *opt;
d->nco = nco_crcf_create(LIQUID_NCO);
nco_crcf_set_phase(d->nco, 0.0f);
nco_crcf_set_frequency(d->nco, opt->center_rads);
if (opt->samples_per_symbol > 1) {
d->decim = firdecim_crcf_create_prototype(opt->shape, opt->samples_per_symbol,
opt->symbol_delay, opt->excess_bw, 0);
} else {
d->opt.samples_per_symbol = 1;
d->opt.symbol_delay = 0;
d->decim = NULL;
}
return d;
}
//
// test nco mixing
//
void autotest_nco_mixing() {
// frequency, phase
float f = 0.1f;
float phi = M_PI;
// error tolerance (high for NCO)
float tol = 0.05f;
// initialize nco object
nco_crcf p = nco_crcf_create(LIQUID_NCO);
nco_crcf_set_frequency(p, f);
nco_crcf_set_phase(p, phi);
unsigned int i;
float nco_i, nco_q;
for (i=0; i<64; i++) {
// generate sin/cos
nco_crcf_sincos(p, &nco_q, &nco_i);
// mix back to zero phase
complex float nco_cplx_in = nco_i + _Complex_I*nco_q;
complex float nco_cplx_out;
nco_crcf_mix_down(p, nco_cplx_in, &nco_cplx_out);
// assert mixer output is correct
CONTEND_DELTA(crealf(nco_cplx_out), 1.0f, tol);
CONTEND_DELTA(cimagf(nco_cplx_out), 0.0f, tol);
//printf("%3u : %12.8f + j*%12.8f\n", i, crealf(nco_cplx_out), cimagf(nco_cplx_out));
// step nco
nco_crcf_step(p);
}
// destroy NCO object
nco_crcf_destroy(p);
}
void SpectrumVisualProcessor::process() {
if (!isOutputEmpty()) {
return;
}
if (!input || input->empty()) {
return;
}
if (fftSizeChanged.load()) {
setup(newFFTSize);
fftSizeChanged.store(false);
}
DemodulatorThreadIQData *iqData;
input->pop(iqData);
if (!iqData) {
return;
}
//Start by locking concurrent access to iqData
std::lock_guard < std::recursive_mutex > lock(iqData->getMonitor());
//then get the busy_lock
std::lock_guard < std::mutex > busy_lock(busy_run);
bool doPeak = peakHold.load() && (peakReset.load() == 0);
if (fft_result.size() != fftSizeInternal) {
if (fft_result.capacity() < fftSizeInternal) {
fft_result.reserve(fftSizeInternal);
fft_result_ma.reserve(fftSizeInternal);
fft_result_maa.reserve(fftSizeInternal);
fft_result_peak.reserve(fftSizeInternal);
}
fft_result.resize(fftSizeInternal);
fft_result_ma.resize(fftSizeInternal);
fft_result_maa.resize(fftSizeInternal);
fft_result_temp.resize(fftSizeInternal);
fft_result_peak.resize(fftSizeInternal);
}
if (peakReset.load() != 0) {
peakReset--;
if (peakReset.load() == 0) {
for (unsigned int i = 0, iMax = fftSizeInternal; i < iMax; i++) {
fft_result_peak[i] = fft_floor_maa;
}
fft_ceil_peak = fft_floor_maa;
fft_floor_peak = fft_ceil_maa;
}
}
std::vector<liquid_float_complex> *data = &iqData->data;
if (data && data->size()) {
unsigned int num_written;
long resampleBw = iqData->sampleRate;
bool newResampler = false;
int bwDiff;
if (is_view.load()) {
if (!iqData->sampleRate) {
iqData->decRefCount();
return;
}
while (resampleBw / SPECTRUM_VZM >= bandwidth) {
resampleBw /= SPECTRUM_VZM;
}
resamplerRatio = (double) (resampleBw) / (double) iqData->sampleRate;
size_t desired_input_size = fftSizeInternal / resamplerRatio;
this->desiredInputSize.store(desired_input_size);
if (iqData->data.size() < desired_input_size) {
// std::cout << "fft underflow, desired: " << desired_input_size << " actual:" << input->data.size() << std::endl;
desired_input_size = iqData->data.size();
}
if (centerFreq != iqData->frequency) {
if ((centerFreq - iqData->frequency) != shiftFrequency || lastInputBandwidth != iqData->sampleRate) {
if (abs(iqData->frequency - centerFreq) < (wxGetApp().getSampleRate() / 2)) {
long lastShiftFrequency = shiftFrequency;
shiftFrequency = centerFreq - iqData->frequency;
nco_crcf_set_frequency(freqShifter, (2.0 * M_PI) * (((double) abs(shiftFrequency)) / ((double) iqData->sampleRate)));
if (is_view.load()) {
long freqDiff = shiftFrequency - lastShiftFrequency;
if (lastBandwidth!=0) {
double binPerHz = double(lastBandwidth) / double(fftSizeInternal);
unsigned int numShift = floor(double(abs(freqDiff)) / binPerHz);
if (numShift < fftSizeInternal/2 && numShift) {
if (freqDiff > 0) {
memmove(&fft_result_ma[0], &fft_result_ma[numShift], (fftSizeInternal-numShift) * sizeof(double));
memmove(&fft_result_maa[0], &fft_result_maa[numShift], (fftSizeInternal-numShift) * sizeof(double));
// memmove(&fft_result_peak[0], &fft_result_peak[numShift], (fftSizeInternal-numShift) * sizeof(double));
// memset(&fft_result_peak[fftSizeInternal-numShift], 0, numShift * sizeof(double));
} else {
memmove(&fft_result_ma[numShift], &fft_result_ma[0], (fftSizeInternal-numShift) * sizeof(double));
memmove(&fft_result_maa[numShift], &fft_result_maa[0], (fftSizeInternal-numShift) * sizeof(double));
// memmove(&fft_result_peak[numShift], &fft_result_peak[0], (fftSizeInternal-numShift) * sizeof(double));
// memset(&fft_result_peak[0], 0, numShift * sizeof(double));
}
}
}
}
}
peakReset.store(PEAK_RESET_COUNT);
}
if (shiftBuffer.size() != desired_input_size) {
if (shiftBuffer.capacity() < desired_input_size) {
shiftBuffer.reserve(desired_input_size);
}
shiftBuffer.resize(desired_input_size);
}
if (shiftFrequency < 0) {
nco_crcf_mix_block_up(freqShifter, &iqData->data[0], &shiftBuffer[0], desired_input_size);
} else {
nco_crcf_mix_block_down(freqShifter, &iqData->data[0], &shiftBuffer[0], desired_input_size);
}
} else {
shiftBuffer.assign(iqData->data.begin(), iqData->data.begin()+desired_input_size);
}
if (!resampler || resampleBw != lastBandwidth || lastInputBandwidth != iqData->sampleRate) {
float As = 60.0f;
if (resampler) {
msresamp_crcf_destroy(resampler);
}
resampler = msresamp_crcf_create(resamplerRatio, As);
bwDiff = resampleBw-lastBandwidth;
lastBandwidth = resampleBw;
lastInputBandwidth = iqData->sampleRate;
newResampler = true;
peakReset.store(PEAK_RESET_COUNT);
}
unsigned int out_size = ceil((double) (desired_input_size) * resamplerRatio) + 512;
if (resampleBuffer.size() != out_size) {
if (resampleBuffer.capacity() < out_size) {
resampleBuffer.reserve(out_size);
}
resampleBuffer.resize(out_size);
}
msresamp_crcf_execute(resampler, &shiftBuffer[0], desired_input_size, &resampleBuffer[0], &num_written);
if (num_written < fftSizeInternal) {
memcpy(fftInData, resampleBuffer.data(), num_written * sizeof(liquid_float_complex));
memset(&(fftInData[num_written]), 0, (fftSizeInternal-num_written) * sizeof(liquid_float_complex));
} else {
memcpy(fftInData, resampleBuffer.data(), fftSizeInternal * sizeof(liquid_float_complex));
}
} else {
this->desiredInputSize.store(fftSizeInternal);
num_written = data->size();
if (data->size() < fftSizeInternal) {
memcpy(fftInData, data->data(), data->size() * sizeof(liquid_float_complex));
memset(&fftInData[data->size()], 0, (fftSizeInternal - data->size()) * sizeof(liquid_float_complex));
} else {
memcpy(fftInData, data->data(), fftSizeInternal * sizeof(liquid_float_complex));
}
}
bool execute = false;
if (num_written >= fftSizeInternal) {
execute = true;
memcpy(fftInput, fftInData, fftSizeInternal * sizeof(liquid_float_complex));
memcpy(fftLastData, fftInput, fftSizeInternal * sizeof(liquid_float_complex));
} else {
if (lastDataSize + num_written < fftSizeInternal) { // priming
unsigned int num_copy = fftSizeInternal - lastDataSize;
if (num_written > num_copy) {
num_copy = num_written;
}
memcpy(fftLastData, fftInData, num_copy * sizeof(liquid_float_complex));
lastDataSize += num_copy;
} else {
unsigned int num_last = (fftSizeInternal - num_written);
memcpy(fftInput, fftLastData + (lastDataSize - num_last), num_last * sizeof(liquid_float_complex));
memcpy(fftInput + num_last, fftInData, num_written * sizeof(liquid_float_complex));
memcpy(fftLastData, fftInput, fftSizeInternal * sizeof(liquid_float_complex));
execute = true;
}
}
if (execute) {
SpectrumVisualData *output = outputBuffers.getBuffer();
if (output->spectrum_points.size() != fftSize * 2) {
output->spectrum_points.resize(fftSize * 2);
}
if (doPeak) {
if (output->spectrum_hold_points.size() != fftSize * 2) {
output->spectrum_hold_points.resize(fftSize * 2);
}
} else {
output->spectrum_hold_points.resize(0);
}
float fft_ceil = 0, fft_floor = 1;
fft_execute(fftPlan);
for (int i = 0, iMax = fftSizeInternal / 2; i < iMax; i++) {
float a = fftOutput[i].real;
float b = fftOutput[i].imag;
float c = sqrt(a * a + b * b);
float x = fftOutput[fftSizeInternal / 2 + i].real;
float y = fftOutput[fftSizeInternal / 2 + i].imag;
float z = sqrt(x * x + y * y);
fft_result[i] = (z);
fft_result[fftSizeInternal / 2 + i] = (c);
}
if (newResampler && lastView) {
if (bwDiff < 0) {
for (unsigned int i = 0, iMax = fftSizeInternal; i < iMax; i++) {
fft_result_temp[i] = fft_result_ma[(fftSizeInternal/4) + (i/2)];
}
for (unsigned int i = 0, iMax = fftSizeInternal; i < iMax; i++) {
fft_result_ma[i] = fft_result_temp[i];
fft_result_temp[i] = fft_result_maa[(fftSizeInternal/4) + (i/2)];
}
for (unsigned int i = 0, iMax = fftSizeInternal; i < iMax; i++) {
fft_result_maa[i] = fft_result_temp[i];
}
} else {
for (size_t i = 0, iMax = fftSizeInternal; i < iMax; i++) {
if (i < fftSizeInternal/4) {
fft_result_temp[i] = 0; // fft_result_ma[fftSizeInternal/4];
} else if (i >= fftSizeInternal - fftSizeInternal/4) {
fft_result_temp[i] = 0; // fft_result_ma[fftSizeInternal - fftSizeInternal/4-1];
} else {
fft_result_temp[i] = fft_result_ma[(i-fftSizeInternal/4)*2];
}
}
for (unsigned int i = 0, iMax = fftSizeInternal; i < iMax; i++) {
fft_result_ma[i] = fft_result_temp[i];
if (i < fftSizeInternal/4) {
fft_result_temp[i] = 0; //fft_result_maa[fftSizeInternal/4];
} else if (i >= fftSizeInternal - fftSizeInternal/4) {
fft_result_temp[i] = 0; // fft_result_maa[fftSizeInternal - fftSizeInternal/4-1];
} else {
fft_result_temp[i] = fft_result_maa[(i-fftSizeInternal/4)*2];
}
}
for (unsigned int i = 0, iMax = fftSizeInternal; i < iMax; i++) {
fft_result_maa[i] = fft_result_temp[i];
}
}
}
for (int i = 0, iMax = fftSizeInternal; i < iMax; i++) {
if (fft_result_maa[i] != fft_result_maa[i]) fft_result_maa[i] = fft_result[i];
fft_result_maa[i] += (fft_result_ma[i] - fft_result_maa[i]) * fft_average_rate;
if (fft_result_ma[i] != fft_result_ma[i]) fft_result_ma[i] = fft_result[i];
fft_result_ma[i] += (fft_result[i] - fft_result_ma[i]) * fft_average_rate;
if (fft_result_maa[i] > fft_ceil || fft_ceil != fft_ceil) {
fft_ceil = fft_result_maa[i];
}
if (fft_result_maa[i] < fft_floor || fft_floor != fft_floor) {
fft_floor = fft_result_maa[i];
}
if (doPeak) {
if (fft_result_maa[i] > fft_result_peak[i]) {
fft_result_peak[i] = fft_result_maa[i];
}
}
}
if (fft_ceil_ma != fft_ceil_ma) fft_ceil_ma = fft_ceil;
fft_ceil_ma = fft_ceil_ma + (fft_ceil - fft_ceil_ma) * 0.05;
if (fft_ceil_maa != fft_ceil_maa) fft_ceil_maa = fft_ceil;
fft_ceil_maa = fft_ceil_maa + (fft_ceil_ma - fft_ceil_maa) * 0.05;
if (fft_floor_ma != fft_floor_ma) fft_floor_ma = fft_floor;
fft_floor_ma = fft_floor_ma + (fft_floor - fft_floor_ma) * 0.05;
if (fft_floor_maa != fft_floor_maa) fft_floor_maa = fft_floor;
fft_floor_maa = fft_floor_maa + (fft_floor_ma - fft_floor_maa) * 0.05;
if (doPeak) {
if (fft_ceil_maa > fft_ceil_peak) {
fft_ceil_peak = fft_ceil_maa;
}
if (fft_floor_maa < fft_floor_peak) {
fft_floor_peak = fft_floor_maa;
}
}
float sf = scaleFactor.load();
double visualRatio = (double(bandwidth) / double(resampleBw));
double visualStart = (double(fftSizeInternal) / 2.0) - (double(fftSizeInternal) * (visualRatio / 2.0));
double visualAccum = 0;
double peak_acc = 0, acc = 0, accCount = 0, i = 0;
double point_ceil = doPeak?fft_ceil_peak:fft_ceil_maa;
double point_floor = doPeak?fft_floor_peak:fft_floor_maa;
for (int x = 0, xMax = output->spectrum_points.size() / 2; x < xMax; x++) {
visualAccum += visualRatio * double(SPECTRUM_VZM);
while (visualAccum >= 1.0) {
unsigned int idx = round(visualStart+i);
if (idx > 0 && idx < fftSizeInternal) {
acc += fft_result_maa[idx];
if (doPeak) {
peak_acc += fft_result_peak[idx];
}
} else {
acc += fft_floor_maa;
if (doPeak) {
peak_acc += fft_floor_maa;
}
}
accCount += 1.0;
visualAccum -= 1.0;
i++;
}
output->spectrum_points[x * 2] = ((float) x / (float) xMax);
if (doPeak) {
output->spectrum_hold_points[x * 2] = ((float) x / (float) xMax);
}
if (accCount) {
output->spectrum_points[x * 2 + 1] = ((log10((acc/accCount)+0.25 - (point_floor-0.75)) / log10((point_ceil+0.25) - (point_floor-0.75))))*sf;
acc = 0.0;
if (doPeak) {
output->spectrum_hold_points[x * 2 + 1] = ((log10((peak_acc/accCount)+0.25 - (point_floor-0.75)) / log10((point_ceil+0.25) - (point_floor-0.75))))*sf;
peak_acc = 0.0;
}
accCount = 0.0;
}
}
if (hideDC.load()) { // DC-spike removal
long long freqMin = centerFreq-(bandwidth/2);
long long freqMax = centerFreq+(bandwidth/2);
long long zeroPt = (iqData->frequency-freqMin);
if (freqMin < iqData->frequency && freqMax > iqData->frequency) {
int freqRange = int(freqMax-freqMin);
int freqStep = freqRange/fftSize;
int fftStart = (zeroPt/freqStep)-(2000/freqStep);
int fftEnd = (zeroPt/freqStep)+(2000/freqStep);
// std::cout << "range:" << freqRange << ", step: " << freqStep << ", start: " << fftStart << ", end: " << fftEnd << std::endl;
if (fftEnd-fftStart < 2) {
fftEnd++;
fftStart--;
}
int numSteps = (fftEnd-fftStart);
int halfWay = fftStart+(numSteps/2);
if ((fftEnd+numSteps/2+1 < (long long) fftSize) && (fftStart-numSteps/2-1 >= 0) && (fftEnd > fftStart)) {
int n = 1;
for (int i = fftStart; i < halfWay; i++) {
output->spectrum_points[i * 2 + 1] = output->spectrum_points[(fftStart - n) * 2 + 1];
n++;
}
n = 1;
for (int i = halfWay; i < fftEnd; i++) {
output->spectrum_points[i * 2 + 1] = output->spectrum_points[(fftEnd + n) * 2 + 1];
n++;
}
if (doPeak) {
int n = 1;
for (int i = fftStart; i < halfWay; i++) {
output->spectrum_hold_points[i * 2 + 1] = output->spectrum_hold_points[(fftStart - n) * 2 + 1];
n++;
}
n = 1;
for (int i = halfWay; i < fftEnd; i++) {
output->spectrum_hold_points[i * 2 + 1] = output->spectrum_hold_points[(fftEnd + n) * 2 + 1];
n++;
}
}
}
}
}
output->fft_ceiling = point_ceil/sf;
output->fft_floor = point_floor;
output->centerFreq = centerFreq;
output->bandwidth = bandwidth;
distribute(output);
}
}
iqData->decRefCount();
lastView = is_view.load();
}
void SpectrumVisualProcessor::process() {
if (!isOutputEmpty()) {
return;
}
if (!input || input->empty()) {
return;
}
DemodulatorThreadIQData *iqData;
input->pop(iqData);
if (!iqData) {
return;
}
iqData->busy_rw.lock();
busy_run.lock();
std::vector<liquid_float_complex> *data = &iqData->data;
if (data && data->size()) {
SpectrumVisualData *output = outputBuffers.getBuffer();
if (output->spectrum_points.size() < fftSize * 2) {
output->spectrum_points.resize(fftSize * 2);
}
unsigned int num_written;
if (is_view.load()) {
if (!iqData->frequency || !iqData->sampleRate) {
iqData->decRefCount();
iqData->busy_rw.unlock();
busy_run.unlock();
return;
}
resamplerRatio = (double) (bandwidth) / (double) iqData->sampleRate;
int desired_input_size = fftSize / resamplerRatio;
this->desiredInputSize.store(desired_input_size);
if (iqData->data.size() < desired_input_size) {
// std::cout << "fft underflow, desired: " << desired_input_size << " actual:" << input->data.size() << std::endl;
desired_input_size = iqData->data.size();
}
if (centerFreq != iqData->frequency) {
if ((centerFreq - iqData->frequency) != shiftFrequency || lastInputBandwidth != iqData->sampleRate) {
if (abs(iqData->frequency - centerFreq) < (wxGetApp().getSampleRate() / 2)) {
shiftFrequency = centerFreq - iqData->frequency;
nco_crcf_reset(freqShifter);
nco_crcf_set_frequency(freqShifter, (2.0 * M_PI) * (((double) abs(shiftFrequency)) / ((double) iqData->sampleRate)));
}
}
if (shiftBuffer.size() != desired_input_size) {
if (shiftBuffer.capacity() < desired_input_size) {
shiftBuffer.reserve(desired_input_size);
}
shiftBuffer.resize(desired_input_size);
}
if (shiftFrequency < 0) {
nco_crcf_mix_block_up(freqShifter, &iqData->data[0], &shiftBuffer[0], desired_input_size);
} else {
nco_crcf_mix_block_down(freqShifter, &iqData->data[0], &shiftBuffer[0], desired_input_size);
}
} else {
shiftBuffer.assign(iqData->data.begin(), iqData->data.end());
}
if (!resampler || bandwidth != lastBandwidth || lastInputBandwidth != iqData->sampleRate) {
float As = 60.0f;
if (resampler) {
msresamp_crcf_destroy(resampler);
}
resampler = msresamp_crcf_create(resamplerRatio, As);
lastBandwidth = bandwidth;
lastInputBandwidth = iqData->sampleRate;
}
int out_size = ceil((double) (desired_input_size) * resamplerRatio) + 512;
if (resampleBuffer.size() != out_size) {
if (resampleBuffer.capacity() < out_size) {
resampleBuffer.reserve(out_size);
}
resampleBuffer.resize(out_size);
}
msresamp_crcf_execute(resampler, &shiftBuffer[0], desired_input_size, &resampleBuffer[0], &num_written);
resampleBuffer.resize(fftSize);
if (num_written < fftSize) {
for (int i = 0; i < num_written; i++) {
fftInData[i][0] = resampleBuffer[i].real;
fftInData[i][1] = resampleBuffer[i].imag;
}
for (int i = num_written; i < fftSize; i++) {
fftInData[i][0] = 0;
fftInData[i][1] = 0;
}
} else {
for (int i = 0; i < fftSize; i++) {
fftInData[i][0] = resampleBuffer[i].real;
fftInData[i][1] = resampleBuffer[i].imag;
}
}
} else {
num_written = data->size();
if (data->size() < fftSize) {
for (int i = 0, iMax = data->size(); i < iMax; i++) {
fftInData[i][0] = (*data)[i].real;
fftInData[i][1] = (*data)[i].imag;
}
for (int i = data->size(); i < fftSize; i++) {
fftInData[i][0] = 0;
fftInData[i][1] = 0;
}
} else {
for (int i = 0; i < fftSize; i++) {
fftInData[i][0] = (*data)[i].real;
fftInData[i][1] = (*data)[i].imag;
}
}
}
bool execute = false;
if (num_written >= fftSize) {
execute = true;
memcpy(fftwInput, fftInData, fftSize * sizeof(fftwf_complex));
memcpy(fftLastData, fftwInput, fftSize * sizeof(fftwf_complex));
} else {
if (lastDataSize + num_written < fftSize) { // priming
unsigned int num_copy = fftSize - lastDataSize;
if (num_written > num_copy) {
num_copy = num_written;
}
memcpy(fftLastData, fftInData, num_copy * sizeof(fftwf_complex));
lastDataSize += num_copy;
} else {
unsigned int num_last = (fftSize - num_written);
memcpy(fftwInput, fftLastData + (lastDataSize - num_last), num_last * sizeof(fftwf_complex));
memcpy(fftwInput + num_last, fftInData, num_written * sizeof(fftwf_complex));
memcpy(fftLastData, fftwInput, fftSize * sizeof(fftwf_complex));
execute = true;
}
}
if (execute) {
fftwf_execute(fftw_plan);
float fft_ceil = 0, fft_floor = 1;
if (fft_result.size() < fftSize) {
fft_result.resize(fftSize);
fft_result_ma.resize(fftSize);
fft_result_maa.resize(fftSize);
}
for (int i = 0, iMax = fftSize / 2; i < iMax; i++) {
float a = fftwOutput[i][0];
float b = fftwOutput[i][1];
float c = sqrt(a * a + b * b);
float x = fftwOutput[fftSize / 2 + i][0];
float y = fftwOutput[fftSize / 2 + i][1];
float z = sqrt(x * x + y * y);
fft_result[i] = (z);
fft_result[fftSize / 2 + i] = (c);
}
for (int i = 0, iMax = fftSize; i < iMax; i++) {
if (is_view.load()) {
fft_result_maa[i] += (fft_result_ma[i] - fft_result_maa[i]) * fft_average_rate;
fft_result_ma[i] += (fft_result[i] - fft_result_ma[i]) * fft_average_rate;
} else {
fft_result_maa[i] += (fft_result_ma[i] - fft_result_maa[i]) * fft_average_rate;
fft_result_ma[i] += (fft_result[i] - fft_result_ma[i]) * fft_average_rate;
}
if (fft_result_maa[i] > fft_ceil) {
fft_ceil = fft_result_maa[i];
}
if (fft_result_maa[i] < fft_floor) {
fft_floor = fft_result_maa[i];
}
}
fft_ceil_ma = fft_ceil_ma + (fft_ceil - fft_ceil_ma) * 0.05;
fft_ceil_maa = fft_ceil_maa + (fft_ceil_ma - fft_ceil_maa) * 0.05;
fft_floor_ma = fft_floor_ma + (fft_floor - fft_floor_ma) * 0.05;
fft_floor_maa = fft_floor_maa + (fft_floor_ma - fft_floor_maa) * 0.05;
float sf = scaleFactor.load();
for (int i = 0, iMax = fftSize; i < iMax; i++) {
float v = (log10(fft_result_maa[i]+0.25 - (fft_floor_maa-0.75)) / log10((fft_ceil_maa+0.25) - (fft_floor_maa-0.75)));
output->spectrum_points[i * 2] = ((float) i / (float) iMax);
output->spectrum_points[i * 2 + 1] = v*sf;
}
if (hideDC.load()) { // DC-spike removal
long long freqMin = centerFreq-(bandwidth/2);
long long freqMax = centerFreq+(bandwidth/2);
long long zeroPt = (iqData->frequency-freqMin);
if (freqMin < iqData->frequency && freqMax > iqData->frequency) {
int freqRange = int(freqMax-freqMin);
int freqStep = freqRange/fftSize;
int fftStart = (zeroPt/freqStep)-(2000/freqStep);
int fftEnd = (zeroPt/freqStep)+(2000/freqStep);
// std::cout << "range:" << freqRange << ", step: " << freqStep << ", start: " << fftStart << ", end: " << fftEnd << std::endl;
if (fftEnd-fftStart < 2) {
fftEnd++;
fftStart--;
}
int numSteps = (fftEnd-fftStart);
int halfWay = fftStart+(numSteps/2);
if ((fftEnd+numSteps/2+1 < fftSize) && (fftStart-numSteps/2-1 >= 0) && (fftEnd > fftStart)) {
int n = 1;
for (int i = fftStart; i < halfWay; i++) {
output->spectrum_points[i * 2 + 1] = output->spectrum_points[(fftStart - n) * 2 + 1];
n++;
}
n = 1;
for (int i = halfWay; i < fftEnd; i++) {
output->spectrum_points[i * 2 + 1] = output->spectrum_points[(fftEnd + n) * 2 + 1];
n++;
}
}
}
}
output->fft_ceiling = fft_ceil_maa/sf;
output->fft_floor = fft_floor_maa;
}
distribute(output);
}
iqData->decRefCount();
iqData->busy_rw.unlock();
busy_run.unlock();
}
int main(int argc, char*argv[]) {
srand( time(NULL) );
// parameters
float phase_offset = M_PI/10;
float frequency_offset = 0.001f;
float SNRdB = 30.0f;
float pll_bandwidth = 0.02f;
modulation_scheme ms = LIQUID_MODEM_QPSK;
unsigned int n=256; // number of iterations
int dopt;
while ((dopt = getopt(argc,argv,"uhs:b:n:P:F:m:")) != EOF) {
switch (dopt) {
case 'u':
case 'h': usage(); return 0;
case 's': SNRdB = atof(optarg); break;
case 'b': pll_bandwidth = atof(optarg); break;
case 'n': n = atoi(optarg); break;
case 'P': phase_offset = atof(optarg); break;
case 'F': frequency_offset= atof(optarg); break;
case 'm':
ms = liquid_getopt_str2mod(optarg);
if (ms == LIQUID_MODEM_UNKNOWN) {
fprintf(stderr,"error: %s, unknown/unsupported modulation scheme \"%s\"\n", argv[0], optarg);
return 1;
}
break;
default:
exit(1);
}
}
unsigned int d=n/32; // print every "d" lines
FILE * fid = fopen(OUTPUT_FILENAME,"w");
fprintf(fid, "%% %s : auto-generated file\n", OUTPUT_FILENAME);
fprintf(fid, "clear all;\n");
fprintf(fid, "phi=zeros(1,%u);\n",n);
fprintf(fid, "r=zeros(1,%u);\n",n);
// objects
nco_crcf nco_tx = nco_crcf_create(LIQUID_VCO);
nco_crcf nco_rx = nco_crcf_create(LIQUID_VCO);
modem mod = modem_create(ms);
modem demod = modem_create(ms);
unsigned int bps = modem_get_bps(mod);
// initialize objects
nco_crcf_set_phase(nco_tx, phase_offset);
nco_crcf_set_frequency(nco_tx, frequency_offset);
nco_crcf_pll_set_bandwidth(nco_rx, pll_bandwidth);
float noise_power = powf(10.0f, -SNRdB/20.0f);
// print parameters
printf("PLL example :\n");
printf("modem : %u-%s\n", 1<<bps, modulation_types[ms].name);
printf("frequency offset: %6.3f, phase offset: %6.3f, SNR: %6.2fdB, pll b/w: %6.3f\n",
frequency_offset, phase_offset, SNRdB, pll_bandwidth);
// run loop
unsigned int i, M=1<<bps, sym_in, sym_out, num_errors=0;
float phase_error;
float complex x, r, v, noise;
for (i=0; i<n; i++) {
// generate random symbol
sym_in = rand() % M;
// modulate
modem_modulate(mod, sym_in, &x);
// channel
//r = nco_crcf_cexpf(nco_tx);
nco_crcf_mix_up(nco_tx, x, &r);
// add complex white noise
crandnf(&noise);
r += noise * noise_power;
//
//v = nco_crcf_cexpf(nco_rx);
nco_crcf_mix_down(nco_rx, r, &v);
// demodulate
modem_demodulate(demod, v, &sym_out);
num_errors += count_bit_errors(sym_in, sym_out);
// error estimation
//phase_error = cargf(r*conjf(v));
phase_error = modem_get_demodulator_phase_error(demod);
// perfect error estimation
//phase_error = nco_tx->theta - nco_rx->theta;
// print every line in a format that octave can read
fprintf(fid, "phi(%u) = %10.6E;\n", i+1, phase_error);
fprintf(fid, "r(%u) = %10.6E + j*%10.6E;\n",
i+1, crealf(v), cimagf(v));
if ((i+1)%d == 0 || i==n-1) {
printf(" %4u: e_hat : %6.3f, phase error : %6.3f, freq error : %6.3f\n",
i+1, // iteration
phase_error, // estimated phase error
nco_crcf_get_phase(nco_tx) - nco_crcf_get_phase(nco_rx),// true phase error
nco_crcf_get_frequency(nco_tx) - nco_crcf_get_frequency(nco_rx)// true frequency error
);
}
// update tx nco object
nco_crcf_step(nco_tx);
// update pll
nco_crcf_pll_step(nco_rx, phase_error);
// update rx nco object
nco_crcf_step(nco_rx);
}
fprintf(fid, "figure;\n");
fprintf(fid, "plot(1:length(phi),phi,'LineWidth',2,'Color',[0 0.25 0.5]);\n");
fprintf(fid, "xlabel('Symbol Index');\n");
fprintf(fid, "ylabel('Phase Error [radians]');\n");
fprintf(fid, "grid on;\n");
fprintf(fid, "t0 = round(0.25*length(r));\n");
fprintf(fid, "figure;\n");
fprintf(fid, "plot(r(1:t0),'x','Color',[0.6 0.6 0.6],r(t0:end),'x','Color',[0 0.25 0.5]);\n");
fprintf(fid, "grid on;\n");
fprintf(fid, "axis([-1.5 1.5 -1.5 1.5]);\n");
fprintf(fid, "axis('square');\n");
fprintf(fid, "xlabel('In-Phase');\n");
fprintf(fid, "ylabel('Quadrature');\n");
fprintf(fid, "legend(['first 25%%'],['last 75%%'],1);\n");
fclose(fid);
printf("results written to %s.\n",OUTPUT_FILENAME);
nco_crcf_destroy(nco_tx);
nco_crcf_destroy(nco_rx);
modem_destroy(mod);
modem_destroy(demod);
printf("bit errors: %u / %u\n", num_errors, bps*n);
printf("done.\n");
return 0;
}
int main() {
// spectral periodogram options
unsigned int nfft=256; // spectral periodogram FFT size
unsigned int num_samples = 2001; // number of samples
float beta = 10.0f; // Kaiser-Bessel window parameter
// allocate memory for data arrays
float complex x[num_samples]; // input signal
float complex X[nfft]; // output spectrum
float psd[nfft]; // power spectral density
unsigned int ramp = num_samples/20 < 10 ? 10 : num_samples/20;
// create spectral periodogram
unsigned int window_size = nfft/2; // spgram window size
unsigned int delay = nfft/8; // samples between transforms
spgram q = spgram_create_kaiser(nfft, window_size, beta);
unsigned int i;
// generate signal
nco_crcf nco = nco_crcf_create(LIQUID_VCO);
for (i=0; i<num_samples; i++) {
nco_crcf_set_frequency(nco, 0.1f*(1.2f+sinf(0.007f*i)) );
nco_crcf_cexpf(nco, &x[i]);
nco_crcf_step(nco);
}
nco_crcf_destroy(nco);
// add soft ramping functions
for (i=0; i<ramp; i++) {
x[i] *= 0.5f - 0.5f*cosf(M_PI*(float)i / (float)ramp);
x[num_samples-ramp+i-1] *= 0.5f - 0.5f*cosf(M_PI*(float)(ramp-i-1) / (float)ramp);
}
//
// export output file(s)
//
FILE * fid;
//
// export time-doman data
//
fid = fopen(OUTPUT_FILENAME_TIME,"w");
fprintf(fid,"# %s : auto-generated file\n", OUTPUT_FILENAME_TIME);
for (i=0; i<num_samples; i++)
fprintf(fid,"%12u %12.8f %12.8f\n", i, crealf(x[i]), cimagf(x[i]));
fclose(fid);
printf("results written to %s.\n", OUTPUT_FILENAME_TIME);
//
// export freq-doman data
//
fid = fopen(OUTPUT_FILENAME_FREQ,"w");
fprintf(fid,"# %s : auto-generated file\n", OUTPUT_FILENAME_FREQ);
unsigned int t=0;
for (i=0; i<num_samples; i++) {
// push sample into periodogram
spgram_push(q, &x[i], 1);
if ( ((i+1)%delay)==0 ) {
// compute spectral periodogram output
spgram_execute(q, X);
unsigned int k;
// compute PSD and FFT shift
for (k=0; k<nfft; k++)
psd[k] = 20*log10f( cabsf(X[(k+nfft/2)%nfft]) );
#if 1
for (k=0; k<nfft; k++)
fprintf(fid,"%12u %12.8f %12.8f\n", t, (float)k/(float)nfft - 0.5f, psd[k]);
#else
for (k=0; k<nfft; k++)
fprintf(fid,"%12.8f ", psd[k]);
#endif
fprintf(fid,"\n");
t++;
}
}
// destroy spectral periodogram object
spgram_destroy(q);
fclose(fid);
printf("results written to %s.\n", OUTPUT_FILENAME_FREQ);
printf("done.\n");
return 0;
}
int main() {
// options
unsigned int num_channels=8; // number of channels
unsigned int m=2; // filter delay
float As=-60; // stop-band attenuation
unsigned int num_frames=25; // number of frames
// create objects
firpfbch_crcf c = firpfbch_crcf_create_kaiser(LIQUID_ANALYZER, num_channels, m, As);
//firpfbch_crcf_print(c);
FILE*fid = fopen(OUTPUT_FILENAME,"w");
fprintf(fid,"%% %s: auto-generated file\n\n", OUTPUT_FILENAME);
fprintf(fid,"clear all;\nclose all;\n\n");
fprintf(fid,"num_channels=%u;\n", num_channels);
fprintf(fid,"num_frames=%u;\n", num_frames);
fprintf(fid,"x = zeros(1,%u);\n", num_channels * num_frames);
fprintf(fid,"y = zeros(%u,%u);\n", num_channels, num_frames);
unsigned int i, j, n=0;
float complex x[num_channels]; // time-domain input
float complex y[num_channels]; // channelized output
// create nco: sweeps entire range of frequencies over the evaluation interval
nco_crcf nco_tx = nco_crcf_create(LIQUID_VCO);
nco_crcf_set_frequency(nco_tx, 0.0f);
float df = 2*M_PI/(num_channels*num_frames);
printf("fr/ch:");
for (j=0; j<num_channels; j++) printf("%3u",j);
printf("\n");
for (i=0; i<num_frames; i++) {
// generate frame of data
for (j=0; j<num_channels; j++) {
nco_crcf_cexpf(nco_tx, &x[j]);
nco_crcf_adjust_frequency(nco_tx, df);
nco_crcf_step(nco_tx);
}
// execute analysis filter bank
firpfbch_crcf_analyzer_execute(c, x, y);
printf("%4u : ", i);
for (j=0; j<num_channels; j++) {
if (cabsf(y[j]) > num_channels / 4)
printf(" x ");
else
printf(" . ");
}
printf("\n");
// write output to file
for (j=0; j<num_channels; j++) {
// frequency data
fprintf(fid,"y(%4u,%4u) = %12.4e + j*%12.4e;\n", j+1, i+1, crealf(y[j]), cimagf(y[j]));
// time data
fprintf(fid,"x(%4u) = %12.4e + j*%12.4e;\n", n+1, crealf(x[j]), cimag(x[j]));
n++;
}
}
// destroy objects
nco_crcf_destroy(nco_tx);
firpfbch_crcf_destroy(c);
// plot results
fprintf(fid,"\n\n");
fprintf(fid,"figure;\n");
fprintf(fid,"subplot(2,1,1);\n");
fprintf(fid," plot(1:length(x),real(x),1:length(x),imag(x));\n");
fprintf(fid," xlabel('time');\n");
fprintf(fid," ylabel('signal');\n");
fprintf(fid,"subplot(2,1,2);\n");
fprintf(fid," plot(20*log10(abs(y.')/num_channels));\n");
fprintf(fid," xlabel('time (decimated)');\n");
fprintf(fid," ylabel('channelized energy [dB]');\n");
fprintf(fid,"n=min(num_channels,8);\n");
fprintf(fid,"figure;\n");
fprintf(fid,"for i=1:n\n");
fprintf(fid," subplot(n,1,i);\n");
fprintf(fid," plot(1:num_frames,real(y(i,:)),1:num_frames,imag(y(i,:)));\n");
fprintf(fid," axis off;\n");
fprintf(fid," ylabel(num2str(i));\n");
fprintf(fid,"end;\n");
fclose(fid);
printf("results written to %s\n", OUTPUT_FILENAME);
printf("done.\n");
return 0;
}
int main(int argc, char*argv[])
{
// options
unsigned int sequence_len = 80; // number of sync symbols
unsigned int k = 2; // samples/symbol
unsigned int m = 7; // filter delay [symbols]
float beta = 0.3f; // excess bandwidth factor
int ftype = LIQUID_FIRFILT_ARKAISER;
float gamma = 10.0f; // channel gain
float tau = -0.3f; // fractional sample timing offset
float dphi = -0.01f; // carrier frequency offset
float phi = 0.5f; // carrier phase offset
float SNRdB = 20.0f; // signal-to-noise ratio [dB]
float threshold = 0.5f; // detection threshold
int dopt;
while ((dopt = getopt(argc,argv,"hn:k:m:b:F:T:S:t:")) != EOF) {
switch (dopt) {
case 'h': usage(); return 0;
case 'n': sequence_len = atoi(optarg); break;
case 'k': k = atoi(optarg); break;
case 'm': m = atoi(optarg); break;
case 'b': beta = atof(optarg); break;
case 'F': dphi = atof(optarg); break;
case 'T': tau = atof(optarg); break;
case 'S': SNRdB = atof(optarg); break;
case 't': threshold = atof(optarg); break;
default:
exit(1);
}
}
unsigned int i;
// validate input
if (tau < -0.5f || tau > 0.5f) {
fprintf(stderr,"error: %s, fractional sample offset must be in [-0.5,0.5]\n", argv[0]);
exit(1);
}
// generate synchronization sequence (QPSK symbols)
float complex sequence[sequence_len];
for (i=0; i<sequence_len; i++) {
sequence[i] = (rand() % 2 ? 1.0f : -1.0f) * M_SQRT1_2 +
(rand() % 2 ? 1.0f : -1.0f) * M_SQRT1_2 * _Complex_I;
}
//
float tau_hat = 0.0f;
float gamma_hat = 0.0f;
float dphi_hat = 0.0f;
float phi_hat = 0.0f;
int frame_detected = 0;
// create detector
qdetector_cccf q = qdetector_cccf_create_linear(sequence, sequence_len, ftype, k, m, beta);
qdetector_cccf_set_threshold(q, threshold);
qdetector_cccf_print(q);
//
unsigned int seq_len = qdetector_cccf_get_seq_len(q);
unsigned int buf_len = qdetector_cccf_get_buf_len(q);
unsigned int num_samples = 2*buf_len; // double buffer length to ensure detection
unsigned int num_symbols = buf_len;
// arrays
float complex y[num_samples]; // received signal
float complex syms_rx[num_symbols]; // recovered symbols
// get pointer to sequence and generate full sequence
float complex * v = (float complex*) qdetector_cccf_get_sequence(q);
unsigned int filter_delay = 15;
firfilt_crcf filter = firfilt_crcf_create_kaiser(2*filter_delay+1, 0.4f, 60.0f, -tau);
float nstd = 0.1f;
for (i=0; i<num_samples; i++) {
// add delay
firfilt_crcf_push(filter, i < seq_len ? v[i] : 0);
firfilt_crcf_execute(filter, &y[i]);
// channel gain
y[i] *= gamma;
// carrier offset
y[i] *= cexpf(_Complex_I*(dphi*i + phi));
// noise
y[i] += nstd*(randnf() + _Complex_I*randnf())*M_SQRT1_2;
}
firfilt_crcf_destroy(filter);
// run detection on sequence
for (i=0; i<num_samples; i++) {
v = qdetector_cccf_execute(q,y[i]);
if (v != NULL) {
printf("\nframe detected!\n");
frame_detected = 1;
// get statistics
tau_hat = qdetector_cccf_get_tau(q);
gamma_hat = qdetector_cccf_get_gamma(q);
dphi_hat = qdetector_cccf_get_dphi(q);
phi_hat = qdetector_cccf_get_phi(q);
break;
}
}
unsigned int num_syms_rx = 0; // output symbol counter
unsigned int counter = 0; // decimation counter
if (frame_detected) {
// recover symbols
firfilt_crcf mf = firfilt_crcf_create_rnyquist(ftype, k, m, beta, tau_hat);
firfilt_crcf_set_scale(mf, 1.0f / (float)(k*gamma_hat));
nco_crcf nco = nco_crcf_create(LIQUID_VCO);
nco_crcf_set_frequency(nco, dphi_hat);
nco_crcf_set_phase (nco, phi_hat);
for (i=0; i<buf_len; i++) {
//
float complex sample;
nco_crcf_mix_down(nco, v[i], &sample);
nco_crcf_step(nco);
// apply decimator
firfilt_crcf_push(mf, sample);
counter++;
if (counter == k-1)
firfilt_crcf_execute(mf, &syms_rx[num_syms_rx++]);
counter %= k;
}
nco_crcf_destroy(nco);
firfilt_crcf_destroy(mf);
}
// destroy objects
qdetector_cccf_destroy(q);
// print results
printf("\n");
printf("frame detected : %s\n", frame_detected ? "yes" : "no");
printf(" gamma hat : %8.3f, actual=%8.3f (error=%8.3f)\n", gamma_hat, gamma, gamma_hat - gamma);
printf(" tau hat : %8.3f, actual=%8.3f (error=%8.3f) samples\n", tau_hat, tau, tau_hat - tau );
printf(" dphi hat : %8.5f, actual=%8.5f (error=%8.5f) rad/sample\n", dphi_hat, dphi, dphi_hat - dphi );
printf(" phi hat : %8.5f, actual=%8.5f (error=%8.5f) radians\n", phi_hat, phi, phi_hat - phi );
printf(" symbols rx : %u\n", num_syms_rx);
printf("\n");
//
// export results
//
FILE * fid = fopen(OUTPUT_FILENAME,"w");
fprintf(fid,"%% %s : auto-generated file\n", OUTPUT_FILENAME);
fprintf(fid,"clear all\n");
fprintf(fid,"close all\n");
fprintf(fid,"sequence_len= %u;\n", sequence_len);
fprintf(fid,"num_samples = %u;\n", num_samples);
fprintf(fid,"y = zeros(1,num_samples);\n");
for (i=0; i<num_samples; i++)
fprintf(fid,"y(%4u) = %12.8f + j*%12.8f;\n", i+1, crealf(y[i]), cimagf(y[i]));
fprintf(fid,"num_syms_rx = %u;\n", num_syms_rx);
fprintf(fid,"syms_rx = zeros(1,num_syms_rx);\n");
for (i=0; i<num_syms_rx; i++)
fprintf(fid,"syms_rx(%4u) = %12.8f + j*%12.8f;\n", i+1, crealf(syms_rx[i]), cimagf(syms_rx[i]));
fprintf(fid,"t=[0:(num_samples-1)];\n");
fprintf(fid,"figure;\n");
fprintf(fid,"subplot(4,1,1);\n");
fprintf(fid," plot(t,real(y), t,imag(y));\n");
fprintf(fid," grid on;\n");
fprintf(fid," xlabel('time');\n");
fprintf(fid," ylabel('received signal');\n");
fprintf(fid,"subplot(4,1,2:4);\n");
fprintf(fid," plot(real(syms_rx), imag(syms_rx), 'x');\n");
fprintf(fid," axis([-1 1 -1 1]*1.5);\n");
fprintf(fid," axis square;\n");
fprintf(fid," grid on;\n");
fprintf(fid," xlabel('real');\n");
fprintf(fid," ylabel('imag');\n");
fclose(fid);
printf("results written to '%s'\n", OUTPUT_FILENAME);
return 0;
}
// decode header
void flexframesync_decode_header(flexframesync _q)
{
// recover data symbols from pilots
qpilotsync_execute(_q->header_pilotsync, _q->header_sym, _q->header_mod);
// decode payload
_q->header_valid = qpacketmodem_decode(_q->header_decoder,
_q->header_mod,
_q->header_dec);
if (!_q->header_valid)
return;
// set fine carrier frequency and phase
float dphi_hat = qpilotsync_get_dphi(_q->header_pilotsync);
float phi_hat = qpilotsync_get_phi (_q->header_pilotsync);
//printf("residual offset: dphi=%12.8f, phi=%12.8f\n", dphi_hat, phi_hat);
nco_crcf_set_frequency(_q->pll, dphi_hat);
nco_crcf_set_phase (_q->pll, phi_hat + dphi_hat * _q->header_sym_len);
// first several bytes of header are user-defined
unsigned int n = FLEXFRAME_H_USER;
// first byte is for expansion/version validation
unsigned int protocol = _q->header_dec[n+0];
if (protocol != FLEXFRAME_PROTOCOL) {
fprintf(stderr,"warning: flexframesync_decode_header(), invalid framing protocol %u (expected %u)\n", protocol, FLEXFRAME_PROTOCOL);
_q->header_valid = 0;
return;
}
// strip off payload length
unsigned int payload_dec_len = (_q->header_dec[n+1] << 8) | (_q->header_dec[n+2]);
_q->payload_dec_len = payload_dec_len;
// strip off modulation scheme/depth
unsigned int mod_scheme = _q->header_dec[n+3];
// strip off CRC, forward error-correction schemes
// CRC : most-significant 3 bits of [n+4]
// fec0 : least-significant 5 bits of [n+4]
// fec1 : least-significant 5 bits of [n+5]
unsigned int check = (_q->header_dec[n+4] >> 5 ) & 0x07;
unsigned int fec0 = (_q->header_dec[n+4] ) & 0x1f;
unsigned int fec1 = (_q->header_dec[n+5] ) & 0x1f;
// validate properties
if (mod_scheme == 0 || mod_scheme >= LIQUID_MODEM_NUM_SCHEMES) {
fprintf(stderr,"warning: flexframesync_decode_header(), invalid modulation scheme\n");
_q->header_valid = 0;
return;
} else if (check == LIQUID_CRC_UNKNOWN || check >= LIQUID_CRC_NUM_SCHEMES) {
fprintf(stderr,"warning: flexframesync_decode_header(), decoded CRC exceeds available\n");
_q->header_valid = 0;
return;
} else if (fec0 == LIQUID_FEC_UNKNOWN || fec0 >= LIQUID_FEC_NUM_SCHEMES) {
fprintf(stderr,"warning: flexframesync_decode_header(), decoded FEC (inner) exceeds available\n");
_q->header_valid = 0;
return;
} else if (fec1 == LIQUID_FEC_UNKNOWN || fec1 >= LIQUID_FEC_NUM_SCHEMES) {
fprintf(stderr,"warning: flexframesync_decode_header(), decoded FEC (outer) exceeds available\n");
_q->header_valid = 0;
return;
}
// re-create payload demodulator for phase-locked loop
_q->payload_demod = modem_recreate(_q->payload_demod, mod_scheme);
// reconfigure payload demodulator/decoder
qpacketmodem_configure(_q->payload_decoder,
payload_dec_len, check, fec0, fec1, mod_scheme);
// set length appropriately
_q->payload_sym_len = qpacketmodem_get_frame_len(_q->payload_decoder);
// re-allocate buffers accordingly
_q->payload_sym = (float complex*) realloc(_q->payload_sym, (_q->payload_sym_len)*sizeof(float complex));
_q->payload_dec = (unsigned char*) realloc(_q->payload_dec, (_q->payload_dec_len)*sizeof(unsigned char));
if (_q->payload_sym == NULL || _q->payload_dec == NULL) {
fprintf(stderr,"error: flexframesync_decode_header(), could not re-allocate payload arrays\n");
_q->header_valid = 0;
return;
}
#if DEBUG_FLEXFRAMESYNC_PRINT
// print results
printf("flexframesync_decode_header():\n");
printf(" header crc : %s\n", _q->header_valid ? "pass" : "FAIL");
printf(" check : %s\n", crc_scheme_str[check][1]);
printf(" fec (inner) : %s\n", fec_scheme_str[fec0][1]);
printf(" fec (outer) : %s\n", fec_scheme_str[fec1][1]);
printf(" mod scheme : %s\n", modulation_types[mod_scheme].name);
printf(" payload sym len : %u\n", _q->payload_sym_len);
printf(" payload dec len : %u\n", _q->payload_dec_len);
printf(" user data :");
unsigned int i;
for (i=0; i<FLEXFRAME_H_USER; i++)
printf(" %.2x", _q->header_dec[i]);
printf("\n");
#endif
}
void do_track(){
if (local_track_freq != shm_settings.track_frequency_target) {
if (ff.sosMap.count(shm_settings.track_frequency_target)) { //frequency key exists
if (track_filterA != NULL)
iirfilt_crcf_destroy(track_filterA);
if (track_filterB != NULL)
iirfilt_crcf_destroy(track_filterB);
if (track_filterC != NULL)
iirfilt_crcf_destroy(track_filterC);
float *b = &((ff.sosMap[shm_settings.track_frequency_target]->b)[0]);
float *a = &((ff.sosMap[shm_settings.track_frequency_target]->a)[0]);
track_filterA = iirfilt_crcf_create_sos(b,a,NUMBER_OF_SECTIONS);
track_filterB = iirfilt_crcf_create_sos(b,a,NUMBER_OF_SECTIONS);
track_filterC = iirfilt_crcf_create_sos(b,a,NUMBER_OF_SECTIONS);
shm_results_track.track_state = 0;
local_track_freq = shm_settings.track_frequency_target;
float normalized_frequency = (float) shm_settings.track_frequency_target / (float) SAMPLING_FREQUENCY;
nco_crcf_set_frequency(nco,2*M_PI*normalized_frequency);
}
else { //frequency key does not exist. Trigger error state
shm_results_track.track_state = -1;
std::cerr << shm_settings.track_frequency_target << " WARN TRACK FREQ NOT IN SOS TABLE" << std::endl;
}
shm_setg(hydrophones_results_track, shm_results_track);
}
std::complex<float> *chA_ptr, *chA_filtered_ptr;
std::complex<float> *chB_ptr, *chB_filtered_ptr;
std::complex<float> *chC_ptr, *chC_filtered_ptr;
float *mag_ptr;
windowcf_read(wchA,&chA_ptr);
windowcf_read(wchB,&chB_ptr);
windowcf_read(wchC,&chC_ptr);
std::complex<float> filtOutA, filtOutB, filtOutC;
for (int i = 0; i < SAMPLING_DEPTH; i++) {
iirfilt_crcf_execute(track_filterA,chA_ptr[i],&filtOutA);
iirfilt_crcf_execute(track_filterB,chB_ptr[i],&filtOutB);
iirfilt_crcf_execute(track_filterC,chC_ptr[i],&filtOutC);
windowcf_read(wchA_filtered,&chA_filtered_ptr);
windowcf_read(wchB_filtered,&chB_filtered_ptr);
windowcf_read(wchC_filtered,&chC_filtered_ptr);
windowcf_push(wchA_filtered,filtOutA);
windowcf_push(wchB_filtered,filtOutB);
windowcf_push(wchC_filtered,filtOutC);
float b_sqrd = std::pow(std::real(filtOutB),2);
pwr_sum += b_sqrd;
windowf_read(wmag_buffer,&mag_ptr);
pwr_sum -= mag_ptr[0];
windowf_push(wmag_buffer,b_sqrd);
double normalized_pwr = pwr_sum / (double)TRACK_LENGTH;
if (normalized_pwr > shm_settings.track_magnitude_threshold &&
(track_sample_idx - shm_results_track.tracked_ping_time) >= shm_settings.track_cooldown_samples) {
std::complex<float> dtft_coeff_A = goertzelNonInteger(chA_filtered_ptr,TRACK_LENGTH,shm_settings.track_frequency_target,SAMPLING_FREQUENCY);
std::complex<float> dtft_coeff_B = goertzelNonInteger(chB_filtered_ptr,TRACK_LENGTH,shm_settings.track_frequency_target,SAMPLING_FREQUENCY);
std::complex<float> dtft_coeff_C = goertzelNonInteger(chC_filtered_ptr,TRACK_LENGTH,shm_settings.track_frequency_target,SAMPLING_FREQUENCY);
float phaseA = std::arg(dtft_coeff_A);
float phaseB = std::arg(dtft_coeff_B);
float phaseC = std::arg(dtft_coeff_C);
shm_results_track.diff_phase_y = phase_difference(phaseC,phaseB);
shm_results_track.diff_phase_x = phase_difference(phaseA,phaseB);
float kx = SOUND_SPEED * shm_results_track.diff_phase_x / (NIPPLE_DISTANCE * 2 * M_PI * shm_settings.track_frequency_target);
float ky = SOUND_SPEED * shm_results_track.diff_phase_y / (NIPPLE_DISTANCE * 2 * M_PI * shm_settings.track_frequency_target);
float kz_2 = 1 - kx * kx - ky * ky;
if (kz_2 < 0) {
std::cerr << "WARNING: z mag is negative! " << kz_2 << std::endl;
kz_2 = 0;
}
shm_results_track.tracked_ping_heading_radians = std::atan2(ky, kx);
shm_results_track.tracked_ping_elevation_radians = std::acos(std::sqrt(kz_2));
shm_results_track.tracked_ping_count++;
shm_results_track.tracked_ping_frequency = shm_results_spectrum.most_recent_ping_frequency;
shm_results_track.tracked_ping_time = track_sample_idx;
std::cout << "PING DETECTED @ n=" << track_sample_idx << " w/ pwr="<< normalized_pwr<< std::endl;
std::cout << "@ HEADING=" << shm_results_track.tracked_ping_heading_radians*(180.0f/M_PI) << std::endl;
shm_setg(hydrophones_results_track, shm_results_track);
}
track_sample_idx++;
}
}
int main(int argc, char*argv[])
{
// set random seed
srand( time(NULL) );
// parameters
float phase_offset = 0.0f; // initial phase offset
float frequency_offset = 0.40f; // initial frequency offset
float pll_bandwidth = 0.003f; // phase-locked loop bandwidth
unsigned int n = 512; // number of iterations
int dopt;
while ((dopt = getopt(argc,argv,"uhb:n:p:f:")) != EOF) {
switch (dopt) {
case 'u':
case 'h': usage(); return 0;
case 'b': pll_bandwidth = atof(optarg); break;
case 'n': n = atoi(optarg); break;
case 'p': phase_offset = atof(optarg); break;
case 'f': frequency_offset= atof(optarg); break;
default:
exit(1);
}
}
// objects
nco_crcf nco_tx = nco_crcf_create(LIQUID_VCO);
nco_crcf nco_rx = nco_crcf_create(LIQUID_VCO);
// initialize objects
nco_crcf_set_phase(nco_tx, phase_offset);
nco_crcf_set_frequency(nco_tx, frequency_offset);
nco_crcf_pll_set_bandwidth(nco_rx, pll_bandwidth);
// generate input
float complex x[n];
float complex y[n];
float phase_error[n];
unsigned int i;
for (i=0; i<n; i++) {
// generate complex sinusoid
nco_crcf_cexpf(nco_tx, &x[i]);
// update nco
nco_crcf_step(nco_tx);
}
// run loop
for (i=0; i<n; i++) {
#if 0
// test resetting bandwidth in middle of acquisition
if (i == 100) nco_pll_set_bandwidth(nco_rx, pll_bandwidth*0.2f);
#endif
// generate input
nco_crcf_cexpf(nco_rx, &y[i]);
// update rx nco object
nco_crcf_step(nco_rx);
// compute phase error
phase_error[i] = cargf(x[i]*conjf(y[i]));
// update pll
nco_crcf_pll_step(nco_rx, phase_error[i]);
// print phase error
if ( (i+1)%50 == 0 || i==n-1 || i==0)
printf("%4u : phase error = %12.8f\n", i+1, phase_error[i]);
}
nco_crcf_destroy(nco_tx);
nco_crcf_destroy(nco_rx);
// write output file
FILE * fid = fopen(OUTPUT_FILENAME,"w");
fprintf(fid,"%% %s : auto-generated file\n", OUTPUT_FILENAME);
fprintf(fid,"clear all;\n");
fprintf(fid,"close all;\n");
fprintf(fid,"n = %u;\n", n);
fprintf(fid,"x = zeros(1,n);\n");
fprintf(fid,"y = zeros(1,n);\n");
for (i=0; i<n; i++) {
fprintf(fid,"x(%4u) = %12.4e + j*%12.4e;\n", i+1, crealf(x[i]), cimagf(x[i]));
fprintf(fid,"y(%4u) = %12.4e + j*%12.4e;\n", i+1, crealf(y[i]), cimagf(y[i]));
fprintf(fid,"e(%4u) = %12.4e;\n", i+1, phase_error[i]);
}
fprintf(fid,"t=0:(n-1);\n");
fprintf(fid,"figure;\n");
fprintf(fid,"subplot(3,1,1);\n");
fprintf(fid," hold on;\n");
fprintf(fid," plot(t,real(x),'Color',[1 1 1]*0.8);\n");
fprintf(fid," plot(t,real(y),'Color',[0 0.2 0.5]);\n");
fprintf(fid," hold off;\n");
fprintf(fid," xlabel('time');\n");
fprintf(fid," ylabel('real');\n");
fprintf(fid," axis([0 n -1.2 1.2]);\n");
fprintf(fid," grid on;\n");
fprintf(fid,"subplot(3,1,2);\n");
fprintf(fid," hold on;\n");
fprintf(fid," plot(t,imag(x),'Color',[1 1 1]*0.8);\n");
fprintf(fid," plot(t,imag(y),'Color',[0 0.5 0.2]);\n");
fprintf(fid," hold off;\n");
fprintf(fid," xlabel('time');\n");
fprintf(fid," ylabel('imag');\n");
fprintf(fid," axis([0 n -1.2 1.2]);\n");
fprintf(fid," grid on;\n");
fprintf(fid,"subplot(3,1,3);\n");
fprintf(fid," plot(t,e,'Color',[0.5 0 0]);\n");
fprintf(fid," xlabel('time');\n");
fprintf(fid," ylabel('phase error');\n");
fprintf(fid," axis([0 n -pi pi]);\n");
fprintf(fid," grid on;\n");
fclose(fid);
printf("results written to %s.\n",OUTPUT_FILENAME);
printf("done.\n");
return 0;
}
int main()
{
// spectral periodogram options
unsigned int nfft = 1200; // spectral periodogram FFT size
unsigned int num_samples = 64000; // number of samples
float fc = 0.20f; // carrier (relative to sampling rate)
// create objects
iirfilt_crcf filter_tx = iirfilt_crcf_create_lowpass(15, 0.05);
nco_crcf mixer_tx = nco_crcf_create(LIQUID_VCO);
nco_crcf mixer_rx = nco_crcf_create(LIQUID_VCO);
iirfilt_crcf filter_rx = iirfilt_crcf_create_lowpass(7, 0.2);
// set carrier frequencies
nco_crcf_set_frequency(mixer_tx, fc * 2*M_PI);
nco_crcf_set_frequency(mixer_rx, fc * 2*M_PI);
// create objects for measuring power spectral density
spgramcf spgram_tx = spgramcf_create_default(nfft);
spgramf spgram_dac = spgramf_create_default(nfft);
spgramcf spgram_rx = spgramcf_create_default(nfft);
// run through loop one step at a time
unsigned int i;
for (i=0; i<num_samples; i++) {
// STEP 1: generate input signal (filtered noise with offset tone)
float complex v1 = (randnf() + randnf()*_Complex_I) + 3.0f*cexpf(-_Complex_I*0.2f*i);
iirfilt_crcf_execute(filter_tx, v1, &v1);
// save spectrum
spgramcf_push(spgram_tx, v1);
// STEP 2: mix signal up and save real part (DAC output)
nco_crcf_mix_up(mixer_tx, v1, &v1);
float v2 = crealf(v1);
nco_crcf_step(mixer_tx);
// save spectrum
spgramf_push(spgram_dac, v2);
// STEP 3: mix signal down and filter off image
float complex v3;
nco_crcf_mix_down(mixer_rx, v2, &v3);
iirfilt_crcf_execute(filter_rx, v3, &v3);
nco_crcf_step(mixer_rx);
// save spectrum
spgramcf_push(spgram_rx, v3);
}
// compute power spectral density output
float psd_tx [nfft];
float psd_dac [nfft];
float psd_rx [nfft];
spgramcf_get_psd(spgram_tx, psd_tx);
spgramf_get_psd( spgram_dac, psd_dac);
spgramcf_get_psd(spgram_rx, psd_rx);
// destroy objects
spgramcf_destroy(spgram_tx);
spgramf_destroy(spgram_dac);
spgramcf_destroy(spgram_rx);
iirfilt_crcf_destroy(filter_tx);
nco_crcf_destroy(mixer_tx);
nco_crcf_destroy(mixer_rx);
iirfilt_crcf_destroy(filter_rx);
//
// export output file
//
FILE * fid = fopen(OUTPUT_FILENAME,"w");
fprintf(fid,"%% %s : auto-generated file\n", OUTPUT_FILENAME);
fprintf(fid,"clear all;\n");
fprintf(fid,"close all;\n\n");
fprintf(fid,"nfft = %u;\n", nfft);
fprintf(fid,"f = [0:(nfft-1)]/nfft - 0.5;\n");
fprintf(fid,"psd_tx = zeros(1,nfft);\n");
fprintf(fid,"psd_dac= zeros(1,nfft);\n");
fprintf(fid,"psd_rx = zeros(1,nfft);\n");
for (i=0; i<nfft; i++) {
fprintf(fid,"psd_tx (%6u) = %12.4e;\n", i+1, psd_tx [i]);
fprintf(fid,"psd_dac(%6u) = %12.4e;\n", i+1, psd_dac[i]);
fprintf(fid,"psd_rx (%6u) = %12.4e;\n", i+1, psd_rx [i]);
}
fprintf(fid,"figure;\n");
fprintf(fid,"hold on;\n");
fprintf(fid," plot(f, psd_tx, '-', 'LineWidth',1.5,'Color',[0.7 0.7 0.7]);\n");
fprintf(fid," plot(f, psd_dac, '-', 'LineWidth',1.5,'Color',[0.0 0.5 0.3]);\n");
fprintf(fid," plot(f, psd_rx, '-', 'LineWidth',1.5,'Color',[0.0 0.3 0.5]);\n");
fprintf(fid,"hold off;\n");
fprintf(fid,"xlabel('Normalized Frequency [f/F_s]');\n");
fprintf(fid,"ylabel('Power Spectral Density [dB]');\n");
fprintf(fid,"grid on;\n");
fprintf(fid,"axis([-0.5 0.5 -100 60]);\n");
fprintf(fid,"legend('transmit (complex)','DAC output (real)','receive (complex)','location','northeast');\n");
fclose(fid);
printf("results written to %s.\n", OUTPUT_FILENAME);
printf("done.\n");
return 0;
}
// frame detection
void ofdmframesync_execute_S0b(ofdmframesync _q)
{
//printf("t = %u\n", _q->timer);
_q->timer++;
if (_q->timer < _q->M2)
return;
// reset timer
_q->timer = _q->M + _q->cp_len - _q->backoff;
//
float complex * rc;
windowcf_read(_q->input_buffer, &rc);
// estimate S0 gain
ofdmframesync_estimate_gain_S0(_q, &rc[_q->cp_len], _q->G1);
float complex s_hat;
ofdmframesync_S0_metrics(_q, _q->G1, &s_hat);
s_hat *= _q->g0;
_q->s_hat_1 = s_hat;
#if DEBUG_OFDMFRAMESYNC_PRINT
float tau_hat = cargf(s_hat) * (float)(_q->M2) / (2*M_PI);
printf("********** S0[1] received ************\n");
printf(" s_hat : %12.8f <%12.8f>\n", cabsf(s_hat), cargf(s_hat));
printf(" tau_hat : %12.8f\n", tau_hat);
// new timing offset estimate
tau_hat = cargf(_q->s_hat_0 + _q->s_hat_1) * (float)(_q->M2) / (2*M_PI);
printf(" tau_hat * : %12.8f\n", tau_hat);
printf("**********\n");
#endif
// re-adjust timer accordingly
float tau_prime = cargf(_q->s_hat_0 + _q->s_hat_1) * (float)(_q->M2) / (2*M_PI);
_q->timer -= (int)roundf(tau_prime);
#if 0
if (cabsf(s_hat) < 0.3f) {
#if DEBUG_OFDMFRAMESYNC_PRINT
printf("false alarm S0[1]\n");
#endif
// false alarm
ofdmframesync_reset(_q);
return;
}
#endif
float complex g_hat = 0.0f;
unsigned int i;
for (i=0; i<_q->M; i++)
g_hat += _q->G1[i] * conjf(_q->G0[i]);
#if 0
// compute carrier frequency offset estimate using freq. domain method
float nu_hat = 2.0f * cargf(g_hat) / (float)(_q->M);
#else
// compute carrier frequency offset estimate using ML method
float complex t0 = 0.0f;
for (i=0; i<_q->M2; i++) {
t0 += conjf(rc[i]) * _q->s0[i] *
rc[i+_q->M2] * conjf(_q->s0[i+_q->M2]);
}
float nu_hat = cargf(t0) / (float)(_q->M2);
#endif
#if DEBUG_OFDMFRAMESYNC_PRINT
printf(" nu_hat : %12.8f\n", nu_hat);
#endif
// set NCO frequency
nco_crcf_set_frequency(_q->nco_rx, nu_hat);
_q->state = OFDMFRAMESYNC_STATE_PLCPLONG;
}
int main(int argc, char*argv[])
{
srand(time(NULL));
// options
unsigned int M = 64; // number of subcarriers
unsigned int cp_len = 16; // cyclic prefix length
unsigned int taper_len = 4; // taper length
unsigned int payload_len = 120; // length of payload (bytes)
modulation_scheme ms = LIQUID_MODEM_QPSK;
fec_scheme fec0 = LIQUID_FEC_NONE;
fec_scheme fec1 = LIQUID_FEC_HAMMING128;
crc_scheme check = LIQUID_CRC_32;
float noise_floor = -30.0f; // noise floor [dB]
float SNRdB = 20.0f; // signal-to-noise ratio [dB]
float dphi = 0.02f; // carrier frequency offset
int debug_enabled = 0; // enable debugging?
// get options
int dopt;
while((dopt = getopt(argc,argv,"uhds:F:M:C:n:m:v:c:k:")) != EOF){
switch (dopt) {
case 'u':
case 'h': usage(); return 0;
case 'd': debug_enabled = 1; break;
case 's': SNRdB = atof(optarg); break;
case 'F': dphi = atof(optarg); break;
case 'M': M = atoi(optarg); break;
case 'C': cp_len = atoi(optarg); break;
case 'n': payload_len = atol(optarg); break;
case 'm':
ms = liquid_getopt_str2mod(optarg);
if (ms == LIQUID_MODEM_UNKNOWN) {
fprintf(stderr,"error: %s, unknown/unsupported mod. scheme: %s\n", argv[0], optarg);
exit(-1);
}
break;
case 'v':
// data integrity check
check = liquid_getopt_str2crc(optarg);
if (check == LIQUID_CRC_UNKNOWN) {
fprintf(stderr,"error: unknown/unsupported CRC scheme \"%s\"\n\n",optarg);
exit(1);
}
break;
case 'c':
// inner FEC scheme
fec0 = liquid_getopt_str2fec(optarg);
if (fec0 == LIQUID_FEC_UNKNOWN) {
fprintf(stderr,"error: unknown/unsupported inner FEC scheme \"%s\"\n\n",optarg);
exit(1);
}
break;
case 'k':
// outer FEC scheme
fec1 = liquid_getopt_str2fec(optarg);
if (fec1 == LIQUID_FEC_UNKNOWN) {
fprintf(stderr,"error: unknown/unsupported outer FEC scheme \"%s\"\n\n",optarg);
exit(1);
}
break;
default:
exit(-1);
}
}
unsigned int i;
// TODO : validate options
// derived values
unsigned int frame_len = M + cp_len;
float complex buffer[frame_len]; // time-domain buffer
float nstd = powf(10.0f, noise_floor/20.0f);
float gamma = powf(10.0f, (SNRdB + noise_floor)/20.0f);
// allocate memory for header, payload
unsigned char header[8];
unsigned char payload[payload_len];
// initialize subcarrier allocation
unsigned char p[M];
ofdmframe_init_default_sctype(M, p);
// create frame generator
ofdmflexframegenprops_s fgprops;
ofdmflexframegenprops_init_default(&fgprops);
fgprops.check = check;
fgprops.fec0 = fec0;
fgprops.fec1 = fec1;
fgprops.mod_scheme = ms;
ofdmflexframegen fg = ofdmflexframegen_create(M, cp_len, taper_len, p, &fgprops);
// create frame synchronizer
ofdmflexframesync fs = ofdmflexframesync_create(M, cp_len, taper_len, p, callback, (void*)payload);
if (debug_enabled)
ofdmflexframesync_debug_enable(fs);
// initialize header/payload and assemble frame
for (i=0; i<8; i++)
header[i] = i & 0xff;
for (i=0; i<payload_len; i++)
payload[i] = rand() & 0xff;
ofdmflexframegen_assemble(fg, header, payload, payload_len);
ofdmflexframegen_print(fg);
ofdmflexframesync_print(fs);
// initialize frame synchronizer with noise
for (i=0; i<1000; i++) {
float complex noise = nstd*( randnf() + _Complex_I*randnf())*M_SQRT1_2;
ofdmflexframesync_execute(fs, &noise, 1);
}
// generate frame, push through channel
int last_symbol=0;
nco_crcf nco = nco_crcf_create(LIQUID_VCO);
nco_crcf_set_frequency(nco, dphi);
while (!last_symbol) {
// generate symbol
last_symbol = ofdmflexframegen_writesymbol(fg, buffer);
// apply channel
for (i=0; i<frame_len; i++) {
float complex noise = nstd*( randnf() + _Complex_I*randnf())*M_SQRT1_2;
buffer[i] *= gamma;
buffer[i] += noise;
nco_crcf_mix_up(nco, buffer[i], &buffer[i]);
nco_crcf_step(nco);
}
// receive symbol
ofdmflexframesync_execute(fs, buffer, frame_len);
}
nco_crcf_destroy(nco);
// export debugging file
if (debug_enabled)
ofdmflexframesync_debug_print(fs, "ofdmflexframesync_debug.m");
// destroy objects
ofdmflexframegen_destroy(fg);
ofdmflexframesync_destroy(fs);
printf("done.\n");
return 0;
}
void *DemodulatorPreThread::threadMain() {
#else
void DemodulatorPreThread::threadMain() {
#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
if (!initialized) {
initialize();
}
std::cout << "Demodulator preprocessor thread started.." << std::endl;
std::deque<DemodulatorThreadPostIQData *> buffers;
std::deque<DemodulatorThreadPostIQData *>::iterator buffers_i;
std::vector<liquid_float_complex> in_buf_data;
std::vector<liquid_float_complex> out_buf_data;
// liquid_float_complex carrySample; // Keep the stream count even to simplify some demod operations
// bool carrySampleFlag = false;
terminated = false;
while (!terminated) {
DemodulatorThreadIQData *inp;
iqInputQueue->pop(inp);
bool bandwidthChanged = false;
bool rateChanged = false;
DemodulatorThreadParameters tempParams = params;
if (!commandQueue->empty()) {
while (!commandQueue->empty()) {
DemodulatorThreadCommand command;
commandQueue->pop(command);
switch (command.cmd) {
case DemodulatorThreadCommand::DEMOD_THREAD_CMD_SET_BANDWIDTH:
if (command.llong_value < 1500) {
command.llong_value = 1500;
}
if (command.llong_value > params.sampleRate) {
tempParams.bandwidth = params.sampleRate;
} else {
tempParams.bandwidth = command.llong_value;
}
bandwidthChanged = true;
break;
case DemodulatorThreadCommand::DEMOD_THREAD_CMD_SET_FREQUENCY:
params.frequency = tempParams.frequency = command.llong_value;
break;
case DemodulatorThreadCommand::DEMOD_THREAD_CMD_SET_AUDIO_RATE:
tempParams.audioSampleRate = (int)command.llong_value;
rateChanged = true;
break;
default:
break;
}
}
}
if (inp->sampleRate != tempParams.sampleRate && inp->sampleRate) {
tempParams.sampleRate = inp->sampleRate;
rateChanged = true;
}
if (bandwidthChanged || rateChanged) {
DemodulatorWorkerThreadCommand command(DemodulatorWorkerThreadCommand::DEMOD_WORKER_THREAD_CMD_BUILD_FILTERS);
command.sampleRate = tempParams.sampleRate;
command.audioSampleRate = tempParams.audioSampleRate;
command.bandwidth = tempParams.bandwidth;
command.frequency = tempParams.frequency;
workerQueue->push(command);
}
if (!initialized) {
continue;
}
// Requested frequency is not center, shift it into the center!
if ((params.frequency - inp->frequency) != shiftFrequency || rateChanged) {
shiftFrequency = params.frequency - inp->frequency;
if (abs(shiftFrequency) <= (int) ((double) (wxGetApp().getSampleRate() / 2) * 1.5)) {
nco_crcf_set_frequency(freqShifter, (2.0 * M_PI) * (((double) abs(shiftFrequency)) / ((double) wxGetApp().getSampleRate())));
}
}
if (abs(shiftFrequency) > (int) ((double) (wxGetApp().getSampleRate() / 2) * 1.5)) {
continue;
}
// std::lock_guard < std::mutex > lock(inp->m_mutex);
std::vector<liquid_float_complex> *data = &inp->data;
if (data->size() && (inp->sampleRate == params.sampleRate)) {
int 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 = NULL;
for (buffers_i = buffers.begin(); buffers_i != buffers.end(); buffers_i++) {
if ((*buffers_i)->getRefCount() <= 0) {
resamp = (*buffers_i);
break;
}
}
if (resamp == NULL) {
resamp = new DemodulatorThreadPostIQData;
buffers.push_back(resamp);
}
int 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->setRefCount(1);
resamp->data.assign(resampledData.begin(), resampledData.begin() + numWritten);
// bool uneven = (numWritten % 2 != 0);
// if (!carrySampleFlag && !uneven) {
// resamp->data.assign(resampledData.begin(), resampledData.begin() + numWritten);
// carrySampleFlag = false;
// } else if (!carrySampleFlag && uneven) {
// resamp->data.assign(resampledData.begin(), resampledData.begin() + (numWritten-1));
// carrySample = resampledData.back();
// carrySampleFlag = true;
// } else if (carrySampleFlag && uneven) {
// resamp->data.resize(numWritten+1);
// resamp->data[0] = carrySample;
// memcpy(&resamp->data[1],&resampledData[0],sizeof(liquid_float_complex)*numWritten);
// carrySampleFlag = false;
// } else if (carrySampleFlag && !uneven) {
// resamp->data.resize(numWritten);
// resamp->data[0] = carrySample;
// memcpy(&resamp->data[1],&resampledData[0],sizeof(liquid_float_complex)*(numWritten-1));
// carrySample = resampledData.back();
// carrySampleFlag = true;
// }
resamp->audioResampleRatio = audioResampleRatio;
resamp->audioResampler = audioResampler;
resamp->audioSampleRate = params.audioSampleRate;
resamp->stereoResampler = stereoResampler;
resamp->firStereoLeft = firStereoLeft;
resamp->firStereoRight = firStereoRight;
resamp->iirStereoPilot = iirStereoPilot;
resamp->sampleRate = params.bandwidth;
iqOutputQueue->push(resamp);
}
inp->decRefCount();
if (!workerResults->empty()) {
while (!workerResults->empty()) {
DemodulatorWorkerThreadResult result;
workerResults->pop(result);
switch (result.cmd) {
case DemodulatorWorkerThreadResult::DEMOD_WORKER_THREAD_RESULT_FILTERS:
msresamp_crcf_destroy(iqResampler);
if (result.iqResampler) {
iqResampler = result.iqResampler;
iqResampleRatio = result.iqResampleRatio;
}
if (result.firStereoLeft) {
firStereoLeft = result.firStereoLeft;
}
if (result.firStereoRight) {
firStereoRight = result.firStereoRight;
}
if (result.iirStereoPilot) {
iirStereoPilot = result.iirStereoPilot;
}
if (result.audioResampler) {
audioResampler = result.audioResampler;
audioResampleRatio = result.audioResamplerRatio;
stereoResampler = result.stereoResampler;
}
if (result.audioSampleRate) {
params.audioSampleRate = result.audioSampleRate;
}
if (result.bandwidth) {
params.bandwidth = result.bandwidth;
}
if (result.sampleRate) {
params.sampleRate = result.sampleRate;
}
break;
default:
break;
}
}
}
}
while (!buffers.empty()) {
DemodulatorThreadPostIQData *iqDataDel = buffers.front();
buffers.pop_front();
delete iqDataDel;
}
DemodulatorThreadCommand tCmd(DemodulatorThreadCommand::DEMOD_THREAD_CMD_DEMOD_PREPROCESS_TERMINATED);
tCmd.context = this;
threadQueueNotify->push(tCmd);
std::cout << "Demodulator preprocessor thread done." << std::endl;
#ifdef __APPLE__
return this;
#endif
}
int main() {
unsigned int h_len=20; // filter length
float beta = 0.3f; // stop-band attenuation
unsigned int num_samples=64; // number of samples
// derived values
unsigned int n = num_samples + h_len; // extend length of analysis to
// incorporate filter delay
// create filterbank
qmfb_crcf q = qmfb_crcf_create(h_len, beta, LIQUID_QMFB_ANALYZER);
qmfb_crcf_print(q);
FILE*fid = fopen(OUTPUT_FILENAME,"w");
fprintf(fid,"%% %s : auto-generated file\n", OUTPUT_FILENAME);
fprintf(fid,"clear all;\nclose all;\n\n");
fprintf(fid,"n=%u;\n", n);
fprintf(fid,"x = zeros(1,%u);\n", 2*n);
fprintf(fid,"y = zeros(2,%u);\n", n);
unsigned int i;
float complex x[2*n], y[2][n];
// generate time-domain signal (windowed sinusoidal pulses)
nco_crcf nco_0 = nco_crcf_create(LIQUID_VCO);
nco_crcf nco_1 = nco_crcf_create(LIQUID_VCO);
nco_crcf_set_frequency(nco_0, 0.122*M_PI);
nco_crcf_set_frequency(nco_1, 0.779*M_PI);
float complex x0,x1;
for (i=0; i<2*num_samples; i++) {
nco_crcf_cexpf(nco_0, &x0);
nco_crcf_cexpf(nco_1, &x1);
x[i] = (x0 + x1) * kaiser(i,2*num_samples,10.0f,0.0f);
nco_crcf_step(nco_0);
nco_crcf_step(nco_1);
}
// pad end with zeros
for (i=2*num_samples; i<2*n; i++)
x[i] = 0.0f;
// compute QMF sub-channel output
for (i=0; i<n; i++) {
qmfb_crcf_execute(q, x[2*i+0], x[2*i+1], y[0]+i, y[1]+i);
}
// write results to output file
for (i=0; i<n; i++) {
fprintf(fid,"x(%3u) = %8.4f + j*%8.4f;\n", 2*i+1, crealf(x[2*i+0]), cimagf(x[2*i+0]));
fprintf(fid,"x(%3u) = %8.4f + j*%8.4f;\n", 2*i+2, crealf(x[2*i+1]), cimagf(x[2*i+1]));
fprintf(fid,"y(1,%3u) = %8.4f + j*%8.4f;\n", i+1, crealf(y[0][i]), cimagf(y[0][i]));
fprintf(fid,"y(2,%3u) = %8.4f + j*%8.4f;\n", i+1, crealf(y[1][i]), cimagf(y[1][i]));
}
// plot time-domain results
fprintf(fid,"t0=0:(2*n-1);\n");
fprintf(fid,"t1=0:(n-1);\n");
fprintf(fid,"figure;\n");
fprintf(fid,"subplot(3,1,1); plot(t0,real(x),t0,imag(x)); ylabel('x(t)');\n");
fprintf(fid,"subplot(3,1,2); plot(t1,real(y(1,:)),t1,imag(y(1,:))); ylabel('y_0(t)');\n");
fprintf(fid,"subplot(3,1,3); plot(t1,real(y(2,:)),t1,imag(y(2,:))); ylabel('y_1(t)');\n");
// plot freq-domain results
fprintf(fid,"nfft=512; %% must be even number\n");
fprintf(fid,"X=20*log10(abs(fftshift(fft(x/n,nfft))));\n");
fprintf(fid,"Y0=20*log10(abs(fftshift(fft(y(1,:)/n,nfft))));\n");
fprintf(fid,"Y1=20*log10(abs(fftshift(fft(y(2,:)/n,nfft))));\n");
// Y1 needs to be split into two regions
fprintf(fid,"f=[0:(nfft-1)]/nfft - 0.5;\n");
fprintf(fid,"t0 = [1]:[nfft/2];\n");
fprintf(fid,"t1 = [nfft/2+1]:[nfft];\n");
fprintf(fid,"figure;\n");
fprintf(fid,"hold on;\n");
fprintf(fid," plot(f,X,'Color',[0.5 0.5 0.5]);\n");
fprintf(fid," plot(f/2,Y0,'LineWidth',2,'Color',[0 0.5 0]);\n");
fprintf(fid," plot(f(t0)/2+0.5,Y1(t0),'LineWidth',2,'Color',[0.5 0 0]);\n");
fprintf(fid," plot(f(t1)/2-0.5,Y1(t1),'LineWidth',2,'Color',[0.5 0 0]);\n");
fprintf(fid,"hold off;\n");
fprintf(fid,"grid on;\nxlabel('normalized frequency');\nylabel('PSD [dB]');\n");
fprintf(fid,"legend('original','Y_0','Y_1',1);");
fprintf(fid,"axis([-0.5 0.5 -140 20]);\n");
fclose(fid);
printf("results written to %s\n", OUTPUT_FILENAME);
qmfb_crcf_destroy(q);
nco_crcf_destroy(nco_0);
nco_crcf_destroy(nco_1);
printf("done.\n");
return 0;
}
