// autotest helper function
// _b : filter coefficients (numerator)
// _a : filter coefficients (denominator)
// _h_len : filter coefficients length
// _x : input array
// _x_len : input array length
// _y : output array
// _y_len : output array length
void iirfilt_crcf_test(float * _b,
float * _a,
unsigned int _h_len,
float complex * _x,
unsigned int _x_len,
float complex * _y,
unsigned int _y_len)
{
float tol = 0.001f;
// load filter coefficients externally
iirfilt_crcf q = iirfilt_crcf_create(_b, _h_len, _a, _h_len);
// allocate memory for output
float complex y_test[_y_len];
unsigned int i;
// compute output
for (i=0; i<_x_len; i++) {
iirfilt_crcf_execute(q, _x[i], &y_test[i]);
CONTEND_DELTA( crealf(y_test[i]), crealf(_y[i]), tol );
CONTEND_DELTA( cimagf(y_test[i]), cimagf(_y[i]), tol );
}
// destroy filter object
iirfilt_crcf_destroy(q);
}
// execute frame synchronizer
// _q : frame synchronizer object
// _x : input sample array [size: _n x 1]
// _n : number of input samples
void gmskframesync_execute(gmskframesync _q,
float complex * _x,
unsigned int _n)
{
// push through synchronizer
unsigned int i;
for (i=0; i<_n; i++) {
float complex xf; // input sample
#if GMSKFRAMESYNC_PREFILTER
iirfilt_crcf_execute(_q->prefilter, _x[i], &xf);
#else
xf = _x[i];
#endif
#if DEBUG_GMSKFRAMESYNC
if (_q->debug_enabled)
windowcf_push(_q->debug_x, xf);
#endif
switch (_q->state) {
case STATE_DETECTFRAME:
// look for p/n sequence
gmskframesync_execute_detectframe(_q, xf);
break;
case STATE_RXPREAMBLE:
// receive p/n sequence symbols
gmskframesync_execute_rxpreamble(_q, xf);
break;
case STATE_RXHEADER:
// receive header
gmskframesync_execute_rxheader(_q, xf);
break;
case STATE_RXPAYLOAD:
// receive payload
gmskframesync_execute_rxpayload(_q, xf);
break;
}
}
}
int main() {
// options
unsigned int order=4; // filter order
unsigned int n = order+1;
float b[n], a[n];
// ... initialize filter coefficients ...
// create filter object
iirfilt_crcf q = iirfilt_crcf_create(b,n,a,n);
float complex x; // input sample
float complex y; // output sample
// execute filter (repeat as necessary)
iirfilt_crcf_execute(q,x,&y);
// destroy filter object
iirfilt_crcf_destroy(q);
}
int main() {
// options
unsigned int order=4; // filter order
float fc=0.1f; // cutoff frequency
float f0=0.25f; // center frequency (bandpass|bandstop)
float Ap=1.0f; // pass-band ripple [dB]
float As=40.0f; // stop-band attenuation [dB]
liquid_iirdes_filtertype ftype = LIQUID_IIRDES_ELLIP;
liquid_iirdes_bandtype btype = LIQUID_IIRDES_BANDPASS;
liquid_iirdes_format format = LIQUID_IIRDES_SOS;
// CREATE filter object (and print to stdout)
iirfilt_crcf myfilter;
myfilter = iirfilt_crcf_create_prototype(ftype,
btype,
format,
order,
fc, f0,
Ap, As);
iirfilt_crcf_print(myfilter);
// allocate memory for data arrays
unsigned int n=128; // number of samples
float complex x[n]; // input samples array
float complex y[n]; // output samples array
// run filter
unsigned int i;
for (i=0; i<n; i++) {
// initialize input
x[i] = randnf() + _Complex_I*randnf();
// EXECUTE filter (repeat as many times as desired)
iirfilt_crcf_execute(myfilter, x[i], &y[i]);
}
// DESTROY filter object
iirfilt_crcf_destroy(myfilter);
}
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()
{
// 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;
}
void ModemFMStereo::demodulate(ModemKit *kit, ModemIQData *input, AudioThreadInput *audioOut) {
ModemKitFMStereo *fmkit = (ModemKitFMStereo *)kit;
size_t bufSize = input->data.size();
liquid_float_complex u, v, w, x, y;
double audio_resample_ratio = fmkit->audioResampleRatio;
if (demodOutputData.size() != bufSize) {
if (demodOutputData.capacity() < bufSize) {
demodOutputData.reserve(bufSize);
}
demodOutputData.resize(bufSize);
}
size_t audio_out_size = (size_t)ceil((double) (bufSize) * audio_resample_ratio) + 512;
freqdem_demodulate_block(demodFM, &input->data[0], bufSize, &demodOutputData[0]);
if (resampledOutputData.size() != audio_out_size) {
if (resampledOutputData.capacity() < audio_out_size) {
resampledOutputData.reserve(audio_out_size);
}
resampledOutputData.resize(audio_out_size);
}
unsigned int numAudioWritten;
msresamp_rrrf_execute(fmkit->audioResampler, &demodOutputData[0], bufSize, &resampledOutputData[0], &numAudioWritten);
if (demodStereoData.size() != bufSize) {
if (demodStereoData.capacity() < bufSize) {
demodStereoData.reserve(bufSize);
}
demodStereoData.resize(bufSize);
}
float phase_error = 0;
for (size_t i = 0; i < bufSize; i++) {
// real -> complex
firhilbf_r2c_execute(fmkit->firStereoR2C, demodOutputData[i], &x);
// 19khz pilot band-pass
iirfilt_crcf_execute(fmkit->iirStereoPilot, x, &v);
nco_crcf_cexpf(fmkit->stereoPilot, &w);
w.imag = -w.imag; // conjf(w)
// multiply u = v * conjf(w)
u.real = v.real * w.real - v.imag * w.imag;
u.imag = v.real * w.imag + v.imag * w.real;
// cargf(u)
phase_error = atan2f(u.imag,u.real);
// step pll
nco_crcf_pll_step(fmkit->stereoPilot, phase_error);
nco_crcf_step(fmkit->stereoPilot);
// 38khz down-mix
nco_crcf_mix_down(fmkit->stereoPilot, x, &y);
nco_crcf_mix_down(fmkit->stereoPilot, y, &x);
// complex -> real
firhilbf_c2r_execute(fmkit->firStereoC2R, x, &demodStereoData[i]);
}
// std::cout << "[PLL] phase error: " << phase_error;
// std::cout << " freq:" << (((nco_crcf_get_frequency(stereoPilot) / (2.0 * M_PI)) * inp->sampleRate)) << std::endl;
if (audio_out_size != resampledStereoData.size()) {
if (resampledStereoData.capacity() < audio_out_size) {
resampledStereoData.reserve(audio_out_size);
}
resampledStereoData.resize(audio_out_size);
}
msresamp_rrrf_execute(fmkit->stereoResampler, &demodStereoData[0], bufSize, &resampledStereoData[0], &numAudioWritten);
audioOut->channels = 2;
if (audioOut->data.capacity() < (numAudioWritten * 2)) {
audioOut->data.reserve(numAudioWritten * 2);
}
audioOut->data.resize(numAudioWritten * 2);
for (size_t i = 0; i < numAudioWritten; i++) {
float l, r;
float ld, rd;
if (fmkit->demph) {
iirfilt_rrrf_execute(fmkit->iirDemphL, 0.568f * (resampledOutputData[i] - (resampledStereoData[i])), &ld);
iirfilt_rrrf_execute(fmkit->iirDemphR, 0.568f * (resampledOutputData[i] + (resampledStereoData[i])), &rd);
firfilt_rrrf_push(fmkit->firStereoLeft, ld);
firfilt_rrrf_execute(fmkit->firStereoLeft, &l);
firfilt_rrrf_push(fmkit->firStereoRight, rd);
firfilt_rrrf_execute(fmkit->firStereoRight, &r);
} else {
firfilt_rrrf_push(fmkit->firStereoLeft, 0.568f * (resampledOutputData[i] - (resampledStereoData[i])));
firfilt_rrrf_execute(fmkit->firStereoLeft, &l);
firfilt_rrrf_push(fmkit->firStereoRight, 0.568f * (resampledOutputData[i] + (resampledStereoData[i])));
firfilt_rrrf_execute(fmkit->firStereoRight, &r);
}
audioOut->data[i * 2] = l;
audioOut->data[i * 2 + 1] = r;
}
}
int main() {
unsigned int m = 12; // filter semi-length (actual length: 4*m+1)
float As = 60.0f; // stop-band attenuation [dB]
unsigned int num_samples = 400; // number of input samples
unsigned int i;
// allocate memory for data arrays
float complex x [num_samples]; // input signal
float complex y0[num_samples]; //
float complex y1[num_samples]; //
// generate the two signals
iirfilt_crcf lowpass = iirfilt_crcf_create_lowpass(6,0.02);
for (i=0; i<num_samples; i++) {
// signal at negative frequency: tone
float complex x_neg = cexpf(-_Complex_I*2*M_PI*0.059f*i);
// signal at positive frequency: filtered noise
float complex v;
iirfilt_crcf_execute(lowpass, 4*randnf(), &v);
float complex x_pos = v * cexpf(_Complex_I*2*M_PI*0.073f*i);
// compsite
x[i] = (x_neg + x_pos) * hamming(i,num_samples);
}
iirfilt_crcf_destroy(lowpass);
// create/print the half-band resampler, with a specified
// stopband attenuation level
resamp2_cccf q = resamp2_cccf_create(m,-0.25f,As);
resamp2_cccf_print(q);
// run filter
for (i=0; i<num_samples; i++)
resamp2_cccf_filter_execute(q,x[i],&y0[i],&y1[i]);
//
// print results to 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,"num_samples=%u;\n", num_samples);
// output results
for (i=0; i<num_samples; i++) {
fprintf(fid,"x( %3u) = %12.4e + j*%12.4e;\n", i+1, crealf( x[i]), cimagf( x[i]));
fprintf(fid,"y0(%3u) = %12.4e + j*%12.4e;\n", i+1, crealf(y0[i]), cimagf(y0[i]));
fprintf(fid,"y1(%3u) = %12.4e + j*%12.4e;\n", i+1, crealf(y1[i]), cimagf(y1[i]));
}
// plot temporal results
fprintf(fid,"\n\n");
fprintf(fid,"t = 0:(num_samples-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.7,'LineWidth',1);\n");
fprintf(fid," plot(t,imag(x),'Color',[1 1 1]*0.5,'LineWidth',2);\n");
fprintf(fid," hold off;\n");
fprintf(fid," grid on;\n");
fprintf(fid," legend('real','imag','location','northeast');\n");
fprintf(fid," axis([0 num_samples -2 2]);\n");
fprintf(fid," ylabel('original');\n");
fprintf(fid,"subplot(3,1,2);\n");
fprintf(fid," hold on;\n");
fprintf(fid," plot(t,real(y0),'Color',[1 1 1]*0.7,'LineWidth',1);\n");
fprintf(fid," plot(t,imag(y0),'Color',[0 0.5 0.2],'LineWidth',2);\n");
fprintf(fid," hold off;\n");
fprintf(fid," grid on;\n");
fprintf(fid," legend('real','imag','location','northeast');\n");
fprintf(fid," axis([0 num_samples -2 2]);\n");
fprintf(fid," ylabel('negative band');\n");
fprintf(fid,"subplot(3,1,3);\n");
fprintf(fid," hold on;\n");
fprintf(fid," plot(t,real(y1),'Color',[1 1 1]*0.7,'LineWidth',1);\n");
fprintf(fid," plot(t,imag(y1),'Color',[0 0.2 0.5],'LineWidth',2);\n");
fprintf(fid," hold off;\n");
fprintf(fid," grid on;\n");
fprintf(fid," legend('real','imag','location','northeast');\n");
fprintf(fid," axis([0 num_samples -2 2]);\n");
fprintf(fid," ylabel('positive band');\n");
fprintf(fid," xlabel('sample index');\n");
// plot spectrum results
fprintf(fid,"\n\n");
fprintf(fid,"nfft=2400;\n");
fprintf(fid,"f = [0:(nfft-1)]/nfft - 0.5;\n");
fprintf(fid,"w = hamming(num_samples)' / num_samples;\n");
fprintf(fid,"X = 20*log10(abs(fftshift(fft( x.*w, nfft))));\n");
fprintf(fid,"Y0 = 20*log10(abs(fftshift(fft(y0.*w, nfft))));\n");
fprintf(fid,"Y1 = 20*log10(abs(fftshift(fft(y1.*w, nfft))));\n");
fprintf(fid,"figure('Color','white');\n");
fprintf(fid," hold on;\n");
fprintf(fid," plot(f,X, 'Color',[1 1 1]*0.7,'LineWidth',2);\n");
fprintf(fid," plot(f,Y0,'Color',[0 0.2 0.5]);\n");
fprintf(fid," plot(f,Y1,'Color',[0 0.5 0.2]);\n");
fprintf(fid," hold off;\n");
fprintf(fid,"legend('original','negative','positive','location','northeast');\n");
fprintf(fid,"grid on;\n");
fprintf(fid,"axis([-0.5 0.5 -120 20]);\n");
fprintf(fid,"xlabel('Normalized Frequency [f/F_s]');\n");
fprintf(fid,"ylabel('Power Spectral Density [dB]');\n");
fclose(fid);
printf("results written to %s\n",OUTPUT_FILENAME);
printf("done.\n");
return 0;
}
