1 // Copyright (c) <2012> <Leif Asbrink>
2 //
3 // Permission is hereby granted, free of charge, to any person
4 // obtaining a copy of this software and associated documentation
5 // files (the "Software"), to deal in the Software without restriction,
6 // including without limitation the rights to use, copy, modify,
7 // merge, publish, distribute, sublicense, and/or sell copies of
8 // the Software, and to permit persons to whom the Software is
9 // furnished to do so, subject to the following conditions:
10 //
11 // The above copyright notice and this permission notice shall be
12 // included in all copies or substantial portions of the Software.
13 //
14 // THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
15 // EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES
16 // OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
17 // NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT
18 // HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY,
19 // WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
20 // FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE
21 // OR OTHER DEALINGS IN THE SOFTWARE.
22
23
24 #include "globdef.h"
25 #include "uidef.h"
26 #include "fft1def.h"
27 #include "fft3def.h"
28 #include "sigdef.h"
29 #include "seldef.h"
30 #include "screendef.h"
31 #include "options.h"
32
33
34 float fmpow[32];
35 float fmbuf[64];
36
37 int iniflag=0;
38
39 float fm_phase;
40 float fm_phasediff_avg;
41
42 float fm_lock_factor=0.9;
43
fmfix(int p0,float * z,float * pha)44 float fmfix(int p0, float *z, float *pha)
45 {
46 int i, nn;
47 int i0, i1, i2;
48 float t1,t2;
49 t2=pha[0];
50 if(fm_n==0)
51 {
52 t1=atan2(z[2*p0+1],z[2*p0]);
53 }
54 else
55 {
56 nn=(p0-fm_size+baseband_size)&baseband_mask;
57 for(i=0; i<fm_size; i++)
58 {
59 fmbuf[2*i ]=fm_win[i]*z[2*nn ];
60 fmbuf[2*i+1]=fm_win[i]*z[2*nn+1];
61 nn=(nn+1)&baseband_mask;
62 }
63 fftforward(fm_size, fm_n, fmbuf, fm_tab, fm_permute, FALSE);
64 t1=0;
65 i1=0;
66 for(i=0; i<fm_size; i++)
67 {
68 fmpow[i]=fmbuf[2*i]*fmbuf[2*i]+fmbuf[2*i+1]*fmbuf[2*i+1];
69 if(t1 < fmpow[i])
70 {
71 t1=fmpow[i];
72 i1=i;
73 }
74 }
75 i0=(i1-1+fm_size)&(fm_size-1);
76 i2=(i1+1)&(fm_size-1);
77 t1=atan2( -fmpow[i0]*fmbuf[2*i0+1]+
78 fmpow[i0]*fmbuf[2*i1+1]-
79 fmpow[i0]*fmbuf[2*i2+1],
80 -fmpow[i0]*fmbuf[2*i0 ]+
81 fmpow[i0]*fmbuf[2*i1 ]-
82 fmpow[i0]*fmbuf[2*i2 ]);
83 t1+=(i0&1)*PI_L;
84 }
85 pha[0]=t1;
86 t1-=t2;
87 if(t1 > PI_L)t1-=2*PI_L;
88 if(t1 < -PI_L)t1+=2*PI_L;
89 return t1;
90 }
91
detect_fm(void)92 void detect_fm(void)
93 {
94 float *zr;
95 float t1, t2, t3;
96 float pil2_sin, pil2_cos, pil2_phstep_sin, pil2_phstep_cos;
97 int p0, p1, last_point;
98 int use_audio_filter;
99 int i, mm, nn;
100 #if NET_BASEBRAW_MODE == BASEBAND_IQ
101 // We do absolutely nothing when the user specified
102 // that he wants the raw baseband signal.
103 if( (ui.network_flag&NET_RXOUT_BASEBRAW) != 0)return;
104 #endif
105 if(genparm[DA_OUTPUT_SPEED]/2 > 1000*bg.fm_audio_bw)
106 {
107 use_audio_filter=TRUE;
108 }
109 else
110 {
111 use_audio_filter=FALSE;
112 }
113 if(bg_coherent == 0)
114 {
115 zr=baseb_raw_orthog;
116 }
117 else
118 {
119 zr=baseb_carrier;
120 }
121 // ************************ Step 1 ***********************
122 // Make an FM detector by computing the phase with atan2.
123 // then take the derivative to get the instantaneous
124 // frequency.
125 // The phase can also be determined from an FFT in fmfix()
126 // Return immediately if bg.fm_audio_bw equals 10 since the
127 // corresponding filter would not reduce the bandwidth.
128 // ********************************************************
129 p0=baseb_pa;
130 last_point=(baseb_pa+mix2.new_points)&baseband_mask;
131 if(baseb_channels == 1)
132 {
133 if(use_audio_filter)
134 {
135 while(p0 != last_point)
136 {
137 baseb_fm_demod[p0 ]=fmfix(p0,baseb_raw,&fm0_ph1);
138 if(bg.fm_subtract != 0)
139 {
140 baseb_fm_phase[p0]=fm0_ph1;
141 }
142 p0=(p0+1)&baseband_mask;
143 }
144 }
145 else
146 {
147 while(p0 != last_point)
148 {
149 baseb_out[2*p0 ]=fmfix(p0,baseb_raw,&fm0_ph1);
150 baseb_out[2*p0+1]=fmfix(p0,baseb_raw,&fm0_ph1);
151 p0=(p0+1)&baseband_mask;
152 }
153 return;
154 }
155 }
156 else
157 {
158 if(use_audio_filter)
159 {
160 while(p0 != last_point)
161 {
162 baseb_fm_demod[2*p0 ]=fmfix(p0,baseb_raw,&fm0_ph1);
163 baseb_fm_demod[2*p0+1]=fmfix(p0,zr,&fm0_ph2);
164 p0=(p0+1)&baseband_mask;
165 }
166 }
167 else
168 {
169 while(p0 != last_point)
170 {
171 baseb_out[4*p0 ]=fmfix(p0,baseb_raw,&fm0_ph1);
172 baseb_out[4*p0+1]=0;
173 baseb_out[4*p0+2]=fmfix(p0,zr,&fm0_ph2);
174 baseb_out[4*p0+3]=0;
175 p0=(p0+1)&baseband_mask;
176 }
177 return;
178 }
179 }
180 #if NET_BASEBRAW_MODE == WFM_FM_FULL_BANDWIDTH || \
181 NET_BASEBRAW_MODE == WFM_AM_FULL_BANDWIDTH
182 if( (ui.network_flag & NET_RXOUT_BASEBRAW) != 0)return;
183 #endif
184 // **********************************************************
185 if(bg.fm_subtract == 2 ||
186 fm_pilot_size == 0 ||
187 bg_coherent != 0 ||
188 baseb_channels != 1)goto nbfm;
189 if(bg.fm_subtract == 1 && (ui.network_flag & NET_RXOUT_BASEBRAW) != 0)
190 {
191 // We need the amplitude and the frequency of the dominating signal
192 // without any contribution from the intermodulation products at 100 kHz
193 // which would come from stations on neighbouring channels.
194 // A good enough filter directly at the baseband frequency is very
195 // CPU intensive since we want filters with steep fall-off.
196 // We apply the filters in steps. First apply a filter that cuts at
197 // about 70 kHz.
198 #if NET_BASEBRAW_MODE == WFM_FM_FIRST_LOWPASS || \
199 NET_BASEBRAW_MODE == WFM_AM_FIRST_LOWPASS || \
200 NET_BASEBRAW_MODE == WFM_SYNTHETIZED_FROM_FIRST_LOWPASS || \
201 NET_BASEBRAW_MODE == WFM_SUBTRACT_FROM_FIRST_LOWPASS
202 // we would normally resample here, but if the user wants to analyze
203 // the first low pass filter, or to run fm subtraction without resamplers
204 // here is an option that applies a low pass filter at the full sampling
205 // rate.
206 p0=baseb_pa;
207 p1=(baseb_pa-fmfil70_points/2+baseband_mask)&baseband_mask;
208 while(p0 != last_point)
209 {
210 nn=p1;
211 t1=fmfil70_fir[fmfil70_points/2]*baseb_fm_demod[nn];
212 t2=fmfil70_fir[fmfil70_points/2]*baseb_totpwr[nn];
213 mm=nn;
214 for(i=fmfil70_points/2-1; i>=0; i--)
215 {
216 nn=(nn+baseband_mask)&baseband_mask;
217 mm=(mm+1)&baseband_mask;
218 t1+=fmfil70_fir[i]*(baseb_fm_demod[nn]+baseb_fm_demod[mm]);
219 t2+=fmfil70_fir[i]*(baseb_totpwr[nn]+baseb_totpwr[mm]);
220 }
221 baseb_fm_demod_low[p0]=t1;
222 baseb_carrier_ampl[p0]=t2;
223 p1=(p1+1)&baseband_mask;
224 p0=(p0+1)&baseband_mask;
225 }
226 #if NET_BASEBRAW_MODE == WFM_FM_FIRST_LOWPASS || \
227 NET_BASEBRAW_MODE == WFM_AM_FIRST_LOWPASS
228 if( (ui.network_flag & NET_RXOUT_BASEBRAW) != 0)return;
229 #endif
230 p1=(baseb_pa-fmfil70_points/2+baseband_mask)&baseband_mask;
231 p0=baseb_pa;
232 while(p0 != last_point)
233 {
234 fm_phase+=baseb_fm_demod_low[p0];
235 if(fm_phase > PI_L)fm_phase-=2*PI_L;
236 if(fm_phase < -PI_L)fm_phase+=2*PI_L;
237 t1=fm_phase-baseb_fm_phase[p1];
238 if(t1 > PI_L)t1-=2*PI_L;
239 if(t1 < -PI_L)t1+=2*PI_L;
240 fm_phasediff_avg=fm_lock_factor*fm_phasediff_avg+(1-fm_lock_factor)*t1;
241 if(fabs(t1) > PI_L/2)
242 {
243 fm_phase=baseb_fm_phase[p1];
244 }
245 fm_phase-=fm_phasediff_avg*(1-fm_lock_factor);
246 t1=sqrt(baseb_carrier_ampl[p1]);
247 baseb_carrier[2*p0 ]=-t1*cos(fm_phase);
248 baseb_carrier[2*p0+1]=-t1*sin(fm_phase);
249 #if NET_BASEBRAW_MODE == WFM_SUBTRACT_FROM_FIRST_LOWPASS
250 baseb_carrier[2*p0]+=baseb_raw[2*p1];
251 baseb_carrier[2*p0+1]+=baseb_raw[2*p1+1];
252 #endif
253 p1=(p1+1)&baseband_mask;
254 p0=(p0+1)&baseband_mask;
255 }
256 return;
257 #endif
258
259 /*
260 Various code pieces for future implementation.
261
262 // Wi will eventually subtract the dominating signal, but here
263 // we detect it properly to improve S/N of the modulation and
264 // to allow fine tuning of the different modulation components.
265 // Here we resample after the low pass filters.
266 // The cpu load becomes much smaller since we have to compute only
267 // every Nth point for the output.
268 p1=(baseb_pf-fmfil70_points/2+baseband_mask)&baseband_mask;
269 p2=(baseb_pa-fmfil70_points/2+baseband_mask)&baseband_mask;
270 np=(p2-p1+baseband_size)&baseband_mask;
271 np=fm1_resample_ratio*(np/fm1_resample_ratio);
272 k=(p1+np)&baseband_mask;
273 while(p1 != k)
274 {
275 nn=p1;
276 t2=fmfil70_fir[fmfil70_points/2]*baseb_fm_demod[nn];
277 mm=nn;
278 for(i=fmfil70_points/2-1; i>=0; i--)
279 {
280 nn=(nn+baseband_mask)&baseband_mask;
281 mm=(mm+1)&baseband_mask;
282 t2+=fmfil70_fir[i]*(baseb_fm_demod[nn]+baseb_fm_demod[mm]);
283 }
284 fm1_all[fm1_pa]=t2;
285 p1=(p1+fm1_resample_ratio)&baseband_mask;
286 fm1_pa=(fm1_pa+1)&fm1_mask;
287 }
288 baseb_pf=(baseb_pf+np)&baseband_mask;
289 #if NET_BASEBRAW_MODE == WFM_FM_FIRST_LOWPASS_RESAMP || \
290 NET_BASEBRAW_MODE == WFM_AM_FIRST_LOWPASS_RESAMP
291 return;
292 #endif
293
294
295
296 fm1_p0=0;
297 p0=fm1_p0;
298 p1=(baseb_p0-fm1_resample_ratio/2+baseband_mask)&baseband_mask;
299 p2=(baseb_pa-fm1_resample_ratio/2+baseband_size)&baseband_mask;
300 np=(p2-p1+baseband_size)&baseband_mask;
301 np=fm1_resample_ratio*(np/fm1_resample_ratio);
302 k=(p1+np)&baseband_mask;
303 while(p1 != k)
304 {
305 t1=fm1_all[p0];
306 nn=p1;
307 mm=p2;
308 while(mm != nn)
309 {
310 nn=(nn+baseband_mask)&baseband_mask;
311 mm=(mm+1)&baseband_mask;
312 baseb_fm_phase[nn]=baseb_fm_demod[nn];
313 nn=(nn+1)&baseband_mask;
314 }
315 p1=(p1+fm1_resample_ratio)&baseband_mask;
316 p2=(p2+fm1_resample_ratio)&baseband_mask;
317 p0=(p0+1)&fm1_mask;
318 }
319 return;
320 */
321
322 }
323 // ********** Step 2 (for WFM stereo) *************
324 // Apply low pass filters as specified by fmfil55fir and
325 // fmfil70fir
326 // Put the lowest frequency range, 0 to 55 kHz in baseb_fm_demod_low[]
327 // Put the middle part, 55 to 70 kHz in baseb_fm_high[]
328 // ******************************************************
329
330 p1=(baseb_pa-fmfil55_points/2+baseband_mask)&baseband_mask;
331 p0=baseb_pa;
332 while(p0 != last_point)
333 {
334 nn=p1;
335 t1=fmfil55_fir[fmfil55_points/2]*baseb_fm_demod[nn];
336 mm=nn;
337 for(i=fmfil55_points/2-1; i>=0; i--)
338 {
339 nn=(nn+baseband_mask)&baseband_mask;
340 mm=(mm+1)&baseband_mask;
341 t1+=fmfil55_fir[i]*(baseb_fm_demod[nn]+baseb_fm_demod[mm]);
342 }
343 #if NET_BASEBRAW_MODE != WFM_STEREO_HIGH
344 baseb_fm_demod_low[p0]=t1;
345 #endif
346 #if NET_BASEBRAW_MODE != WFM_STEREO_LOW
347 nn=p1;
348 t2=fmfil70_fir[fmfil70_points/2]*baseb_fm_demod[nn];
349 mm=nn;
350 for(i=fmfil70_points/2-1; i>=0; i--)
351 {
352 nn=(nn+baseband_mask)&baseband_mask;
353 mm=(mm+1)&baseband_mask;
354 t2+=fmfil70_fir[i]*(baseb_fm_demod[nn]+baseb_fm_demod[mm]);
355 }
356 #if NET_BASEBRAW_MODE == WFM_STEREO_HIGH
357 baseb_fm_demod_low[p0]=t2-t1;
358 #else
359 baseb_fm_demod_high[p0]=t2-t1;
360 #endif
361 #endif
362 p1=(p1+1)&baseband_mask;
363 p0=(p0+1)&baseband_mask;
364 }
365 #if NET_BASEBRAW_MODE == WFM_STEREO_LOW || \
366 NET_BASEBRAW_MODE == WFM_STEREO_HIGH
367 if( (ui.network_flag & NET_RXOUT_BASEBRAW) != 0)return;
368 #endif
369 // ********** Step 3 (for wideband stereo) *************
370 // Look for the pilot tone.
371 // ********************************************************
372 // baseb_fm_demod[0]
373 // baseb_fm_demod_low[fmfil55_points/2]
374 // baseb_fm_demod_high[fmfil55_points/2]
375 p1=last_point;
376 t1=0;
377 t2=0;
378 for(i=0; i<fm_pilot_size; i++)
379 {
380 p1=(p1+baseband_mask)&baseband_mask;
381 t1+=baseb_fm_demod_low[p1]*fm_pilot_tone[2*i ];
382 t2+=baseb_fm_demod_low[p1]*fm_pilot_tone[2*i+1];
383 }
384 // ********** Step 4 (for wideband stereo) *************
385 // Detect the stereo channel and the RDS channel.
386 // for development purposes, remember the pilot tone and its overtones.
387 // ********************************************************
388 p0=(baseb_pa+baseband_mask)&baseband_mask;
389 p1=last_point;
390 t1=atan2(t1,t2);
391 t2=2*PI_L*19000./baseband_sampling_speed;
392 pil2_sin=sin(2*t1);
393 pil2_cos=cos(2*t1);
394 pil2_phstep_sin=sin(2*t2);
395 pil2_phstep_cos=cos(2*t2);
396 while(p0 != p1)
397 {
398 p1=(p1+baseband_mask)&baseband_mask;
399 baseb_fm_pil2det[p1]=pil2_sin*baseb_fm_demod_low[p1];
400 t3=pil2_cos;
401 pil2_cos= pil2_cos*pil2_phstep_cos-pil2_sin*pil2_phstep_sin;
402 pil2_sin= pil2_sin*pil2_phstep_cos+t3*pil2_phstep_sin;
403 // baseb_fm_pil3[2*p1 ]=pil3_sin;
404 // baseb_fm_pil3det[2*p1 ]=pil3_sin*baseb_fm_demod_high[p1];
405 // baseb_fm_pil3[2*p1+1]=pil3_cos;
406 // baseb_fm_pil3det[2*p1+1]=pil3_cos*baseb_fm_demod_high[p1];
407 // ************** probe the RDS signal ****************
408 // baseb_out[2*p1 ]=baseb_fm_pil3det[2*p1 ];
409 // baseb_out[2*p1+1]=baseb_fm_pil3det[2*p1+1];
410 //Ö t3=pil3_cos;
411 //Ö pil3_cos= pil3_cos*pil3_phstep_cos-pil3_sin*pil3_phstep_sin;
412 //Ö pil3_sin= pil3_sin*pil3_phstep_cos+t3*pil3_phstep_sin;
413 }
414 // ********** Step 5 (for wideband stereo) *************
415 // Apply a low pass filter with cutoff frequency as specified
416 // by fm_audiofil_fir to the sound channels.
417 // ********************************************************
418 // baseb_fm_demod[0]
419 // baseb_fm_demod_low[fmfil55_points/2]
420 // baseb_fm_demod_high[fmfil55_points/2]
421 // baseb_fm_pil2[fmfil55_points/2]
422 // baseb_fm_pil2det[fmfil55_points/2]
423 // baseb_fm_pil3[fmfil55_points/2]
424 // baseb_fm_pil3det[fmfil55_points/2]
425 p0=baseb_pa;
426 p1=(baseb_pa-fm_audiofil_points/2+baseband_mask)&baseband_mask;
427 while(p0 != last_point)
428 {
429 nn=p1;
430 t1=fm_audiofil_fir[fm_audiofil_points/2]*baseb_fm_demod_low[nn];
431 t2=fm_audiofil_fir[fm_audiofil_points/2]*baseb_fm_pil2det[nn];
432 mm=nn;
433 for(i=fm_audiofil_points/2-1; i>=0; i--)
434 {
435 nn=(nn+baseband_mask)&baseband_mask;
436 mm=(mm+1)&baseband_mask;
437 t1+=fm_audiofil_fir[i]*(baseb_fm_demod_low[nn]+baseb_fm_demod_low[mm]);
438 t2+=fm_audiofil_fir[i]*(baseb_fm_pil2det[nn]+baseb_fm_pil2det[mm]);
439 }
440 baseb_out[2*p0 ]=t1+t2;
441 baseb_out[2*p0+1]=t1-t2;
442 p1=(p1+1)&baseband_mask;
443 p0=(p0+1)&baseband_mask;
444 }
445 return;
446
447 /*
448 // ********** Step 6 (for wideband stereo) *************
449 // Low pass filter the RDS channels using fmfil_rds_fir.
450 // ********************************************************
451 // baseb_fm_demod[0]
452 // baseb_fm_demod_low[fmfil55_points/2]
453 // baseb_fm_demod_high[fmfil55_points/2]
454 // baseb_fm_pil2[fmfil55_points/2]
455 // baseb_fm_pil2det[fmfil55_points/2]
456 // baseb_fm_pil3[fmfil55_points/2]
457 // baseb_fm_pil3det[fmfil55_points/2]
458 // baseb_fm_sumchan[fmfil55_points/2+fmfil_points/2]
459 // baseb_fm_diffchan[fmfil55_points/2+fmfil_points/2]
460 lir_fillbox(0,0,101,101,color_scale[1]);
461 p2=(baseb_pa-(fmfil_rds_points+fmfil55_points)/2+baseband_mask)&baseband_mask;
462 p1=(baseb_pa-fmfil_rds_points/2+baseband_mask)&baseband_mask;
463 p0=baseb_pa;
464 while(p0 != last_point)
465 {
466 nn=p1;
467 r1=fmfil_rds_fir[fmfil_rds_points/2]*baseb_fm_pil3det[2*nn ];
468 r2=fmfil_rds_fir[fmfil_rds_points/2]*baseb_fm_pil3det[2*nn+1];
469 mm=nn;
470 for(i=fmfil_rds_points/2-1; i>=0; i--)
471 {
472 nn=(nn+baseband_mask)&baseband_mask;
473 mm=(mm+1)&baseband_mask;
474 r1+=fmfil_rds_fir[i]*(baseb_fm_pil3det[2*nn ]+baseb_fm_pil3det[2*mm ]);
475 r2+=fmfil_rds_fir[i]*(baseb_fm_pil3det[2*nn+1]+baseb_fm_pil3det[2*mm+1]);
476 }
477 // baseb_fm_rdsraw[2*p0 ]=r1;
478 // baseb_fm_rdsraw[2*p0+1]=r2;
479 // ************* probe the RDS signal after the filter ****************
480 //baseb_out[2*p0 ]=r1;
481 //baseb_out[2*p0+1]=r2;
482
483
484 t2=r1*r1+r2*r2;
485 if(t2 > 0.00000000001)
486 {
487 t1=atan2(r1,r2);
488 }
489 else
490 {
491 t1=0;
492 }
493 i=1;
494 if(t1-rds_phase<PI_L)rds_phase-=2*PI_L;
495 if(t1-rds_phase>PI_L)rds_phase+=2*PI_L;
496 if(t1-rds_phase<-0.5*PI_L)
497 {
498 t1+=PI_L;
499 i=-1;
500 }
501 if(t1-rds_phase>0.5*PI_L)
502 {
503 t1-=PI_L;
504 i=-1;
505 }
506
507
508 rds_power=rds_f1*rds_power+rds_f2*t2;
509 if(rds_power > 0.000000001)
510 {
511 rds_phase=(1-(t2/rds_power))*rds_phase +
512 (t2/rds_power)*(rds_f1*rds_phase+rds_f2*t1);
513 }
514 // Here it would be favourable to decode the rds signal, correct errors
515 // and to construct the correct rds signal with correct amplitude
516 // and rise times. For this test of principles, just use whatever
517 // amplitude that we have detected.
518 // the rds signal is i*sqrt(t2)
519 // Here we could put it into a downsampled array.
520 //
521 // Start to build a cleaned up version of the detected composite
522 // signal. First we store the rds signal.
523
524 baseb_fm_composite[p0]=(sin(rds_phase)*baseb_fm_pil3[2*p1 ]+
525 cos(rds_phase)*baseb_fm_pil3[2*p1+1])*rds_gain*i*sqrt(t2);
526 // here we should perhaps use the filtered signals and construct
527 // the ideal pilot tone. At good S/N it should not be needed.
528
529 baseb_fm_composite[p0]+=baseb_fm_demod_low[p1];
530
531
532 fm_phase+=baseb_fm_composite[p0];
533 if(fm_phase > PI_L)fm_phase-=2*PI_L;
534 if(fm_phase < -PI_L)fm_phase+=2*PI_L;
535
536 //baseb_out[2*p0]=baseb_fm_composite[p0]-baseb_fm_demod[p2];
537 //baseb_out[2*p0+1]=0;
538
539 t1=fm_phase-baseb_fm_phase[p2];
540 if(t1 > PI_L)t1-=2*PI_L;
541 if(t1 < -PI_L)t1+=2*PI_L;
542
543 fm_phasediff_avg=fm_lock_factor*fm_phasediff_avg+(1-fm_lock_factor)*t1;
544
545 if(fabs(t1) > PI_L/2)
546 {
547 fm_phase=baseb_fm_phase[p2];
548 }
549
550 fm_phase-=fm_phasediff_avg*(1-fm_lock_factor);
551
552
553 t1=baseb_totpwr[p2]+baseb_totpwr[(p2+baseband_mask)&baseband_mask]+
554 baseb_totpwr[(p2+baseband_mask-1)&baseband_mask];
555 t1/=3;
556 baseb_out[2*p0 ]=-sqrt(t1)*cos(fm_phase);
557 baseb_out[2*p0+1]=-sqrt(t1)*sin(fm_phase);
558
559 //baseb_out[2*p0]+=baseb_raw[2*p2];
560 //baseb_out[2*p0+1]+=baseb_raw[2*p2+1];
561
562
563
564
565
566 if(rx_daout_channels ==1)
567 {
568 baseb_out[2*p0 ]=r1;
569 baseb_out[2*p0+1]=0;
570 }
571 else
572 {
573 baseb_out[2*p0 ]=r1;
574 baseb_out[2*p0+1]=r2;
575 }
576
577 if(p0%32==0)
578 {
579 mailbox[0]=50+1000*r1;
580 mailbox[1]=50+1000*r2;
581 if(mailbox[0]<0)mailbox[0]=0;
582 if(mailbox[0]>100)mailbox[0]=100;
583 if(mailbox[1]<0)mailbox[1]=0;
584 if(mailbox[1]>100)mailbox[1]=100;
585 lir_setpixel(mailbox[0],mailbox[1],15);
586 mailbox[2]=50+45*sin(rds_phase);
587 mailbox[3]=50+45*cos(rds_phase);
588 lir_setpixel(mailbox[2],mailbox[3],15);
589 }
590
591 p2=(p2+1)&baseband_mask;
592 p1=(p1+1)&baseband_mask;
593 p0=(p0+1)&baseband_mask;
594 }
595
596
597
598
599 return;
600
601 */
602
603
604 // ******************** Step 2 for NBFM *************************
605 // Apply a low pass filter with cutoff frequency as specified
606 // by fm_audiofil_fir.
607 nbfm:;
608 p0=baseb_pa;
609 p1=(baseb_pa-fm_audiofil_points/2+baseband_mask)&baseband_mask;
610 if(baseb_channels == 1)
611 {
612 while(p0 != last_point)
613 {
614 nn=p1;
615 t1=fm_audiofil_fir[fm_audiofil_points/2]*baseb_fm_demod[nn];
616 mm=nn;
617 for(i=fm_audiofil_points/2-1; i>=0; i--)
618 {
619 nn=(nn+baseband_mask)&baseband_mask;
620 mm=(mm+1)&baseband_mask;
621 t1+=fm_audiofil_fir[i]*(baseb_fm_demod[nn]+baseb_fm_demod[mm]);
622 }
623 baseb_out[2*p0 ]=t1;
624 baseb_out[2*p0+1]=t1;
625 p1=(p1+1)&baseband_mask;
626 p0=(p0+1)&baseband_mask;
627 }
628 }
629 else
630 {
631 while(p0 != last_point)
632 {
633 nn=p1;
634 t1=fm_audiofil_fir[fm_audiofil_points/2]*baseb_fm_demod[2*nn ];
635 t2=fm_audiofil_fir[fm_audiofil_points/2]*baseb_fm_demod[2*nn+1];
636 mm=nn;
637 for(i=fm_audiofil_points/2-1; i>=0; i--)
638 {
639 nn=(nn+baseband_mask)&baseband_mask;
640 mm=(mm+1)&baseband_mask;
641 t1+=fm_audiofil_fir[i]*(baseb_fm_demod[2*nn ]+baseb_fm_demod[2*mm ]);
642 t2+=fm_audiofil_fir[i]*(baseb_fm_demod[2*nn+1]+baseb_fm_demod[2*mm+1]);
643 }
644 baseb_out[2*p0 ]=t1;
645 baseb_out[2*p0+1]=t2;
646 p1=(p1+1)&baseband_mask;
647 p0=(p0+1)&baseband_mask;
648 }
649 }
650
651
652 /*
653 // ********************* Step 3 ******************************
654 // These locations correspond to the same moment in time:
655 // baseb_fm_fil1[p0]
656 // baseb_fm_raw[p0-len]
657 // baseb_fm_demod1[p0-len]
658 //
659 // The filtered data is more accurate than the unfiltered
660 // data.
661 // Frequency modulate the original signal with the
662 // filtered demodulated signal. That should compress
663 // the bandwidth because we apply (nearly) the same
664 // FM modulation but in the opposite phase.
665 p0=baseb_pa;
666 p1=(baseb_pa-len+baseband_size)&baseband_mask;
667 t1=fm1_ph1;
668 if(baseb_channels == 1)
669 {
670 while(p0 != last_point)
671 {
672 t1+=baseb_fm_fil1[p0];
673 if(t1>PI_L)t1-=2*PI_L;
674 if(t1<-PI_L)t1+=2*PI_L;
675
676 baseb_compressed_fm1[2*p0 ]= baseb_raw[2*p1 ]*cos(t1)+
677 baseb_raw[2*p1+1]*sin(t1);
678 baseb_compressed_fm1[2*p0+1]= baseb_raw[2*p1+1]*cos(t1)+
679 -baseb_raw[2*p1 ]*sin(t1);
680 // baseb_compressed_fm1[2*p0 ]=baseb_raw[2*p1 ];
681 // baseb_compressed_fm1[2*p0+1]=baseb_raw[2*p1+1];
682 p0=(p0+1)&baseband_mask;
683 p1=(p1+1)&baseband_mask;
684 }
685 }
686 fm1_ph1=t1;
687 */
688
689 }
690