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=5; // Hilbert filter semi-length float As=60.0f; // stop-band attenuation [dB] float fc=0.37f; // signal center frequency unsigned int num_samples=64; // number of samples // derived values unsigned int h_len = 4*m+1; unsigned int N = num_samples + h_len; // create Hilbert transform object firhilbf q = firhilbf_create(m,As); firhilbf_print(q); // data arrays float x[N]; // real input float complex y[N]; // complex output float z[N]; // real output // initialize input array unsigned int i; for (i=0; i<N; i++) { x[i] = cosf(2*M_PI*fc*i); x[i] *= (i < num_samples) ? 1.855f*hamming(i,num_samples) : 0.0f; } for (i=0; i<N; i++) { // execute real-to-complex conversion firhilbf_r2c_execute(q, x[i], &y[i]); // execute complex-to-real conversion firhilbf_c2r_execute(q, y[i], &z[i]); //printf("y(%3u) = %12.8f + j*%12.8f;\n", i+1, crealf(y[i]), cimagf(y[i])); } // destroy Hilbert transform object firhilbf_destroy(q); // // export 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"); fprintf(fid,"h_len=%u;\n", 4*m+1); fprintf(fid,"num_samples=%u;\n", num_samples); fprintf(fid,"N=%u;\n", N); fprintf(fid,"t = 0:(N-1);\n"); for (i=0; i<N; i++) { // print results fprintf(fid,"x(%3u) = %12.4e;\n", i+1, x[i]); fprintf(fid,"y(%3u) = %12.4e + j*%12.4e;\n", i+1, crealf(y[i]), cimagf(y[i])); fprintf(fid,"z(%3u) = %12.4e;\n", i+1, z[i]); } fprintf(fid,"figure;\n"); fprintf(fid," subplot(3,1,1); plot(t,x);\n"); fprintf(fid," subplot(3,1,2); plot(t,real(y), t,imag(y));\n"); fprintf(fid," subplot(3,1,3); plot(t,z);\n"); // plot results fprintf(fid,"nfft=512;\n"); fprintf(fid,"%% compute normalized windowing functions\n"); fprintf(fid,"X=20*log10(abs(fftshift(fft(x/num_samples,nfft))));\n"); fprintf(fid,"Y=20*log10(abs(fftshift(fft(y/num_samples,nfft))));\n"); fprintf(fid,"Z=20*log10(abs(fftshift(fft(z/num_samples,nfft))));\n"); fprintf(fid,"f =[0:(nfft-1)]/nfft-0.5;\n"); fprintf(fid,"figure; plot(f,X,'LineWidth',1,'Color',[0.50 0.50 0.50],...\n"); fprintf(fid," f,Y,'LineWidth',2,'Color',[0.00 0.50 0.25],...\n"); fprintf(fid," f,Z,'LineWidth',1,'Color',[0.00 0.25 0.50]);\n"); fprintf(fid,"grid on;\n"); fprintf(fid,"axis([-0.5 0.5 -80 20]);\n"); fprintf(fid,"xlabel('normalized frequency');\n"); fprintf(fid,"ylabel('PSD [dB]');\n"); fprintf(fid,"legend('original/real','transformed/complex','regenerated/real',1);"); fclose(fid); printf("results written to %s\n", OUTPUT_FILENAME); printf("done.\n"); return 0; }