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;
}
