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 "fft2def.h"
28 #include "screendef.h"
29 #include "seldef.h"
30 #include "sigdef.h"
31 #include "rusage.h"
32 #include "thrdef.h"
33 int update_spur_pol(void);
34
35
eliminate_spurs(void)36 void eliminate_spurs(void)
37 {
38 int ni, nx, iter;
39 int ind, knd, diffind;
40 int i, j, k;
41 float *z;
42 int *uind;
43 float *zsig;
44 float *spt;
45 float t1, t2, t3, t4, r1, r2, r3, r4;
46 float ampl, phase, phase_slope, phase_curv;
47 float freq, rot;
48 short int *sptmmx, *zmmx;
49 // Give the pointers a value.
50 // Otherwise the compiler will complain.
51 spt=spur_table;
52 sptmmx=spur_table_mmx;
53 nx=(ffts_na+1)&fftxn_mask;
54 for(spurno=0; spurno<no_of_spurs; spurno++)
55 {
56 if( (spurno & 11111)==0)lir_sched_yield();
57 spurp0=spurno*spur_block;
58 i=3*spurno;
59 sp_c1=spur_pol[i ];
60 sp_c2=spur_pol[i+1];
61 sp_c3=spur_pol[i+2];
62 uind=&spur_ind[spurno*max_fftxn];
63 zsig=&spur_signal[twice_rxchan*max_fftxn*spurno];
64 if(swmmx_fft2)
65 {
66 sptmmx=&spur_table_mmx[spurp0+ffts_na*SPUR_WIDTH*twice_rxchan];
67 }
68 else
69 {
70 spt=&spur_table[spurp0+ffts_na*SPUR_WIDTH*twice_rxchan];
71 }
72 if(spur_flag[spurno] == 1)
73 {
74 j=(int)(spur_freq[spurno])+2-spur_location[spurno]-SPUR_SIZE/2;
75 if(j < 0 || j > 1)
76 {
77 if(j < -1 )j=-1;
78 if(j > 2)j=2;
79 shift_spur_table(j);
80 }
81 }
82 if(spur_flag[spurno] != 0)
83 {
84 if(sw_onechan)
85 {
86 if(swmmx_fft2)
87 {
88 zmmx=&fft2_short_int[2*(ffts_na*fftx_size+spur_location[spurno])];
89 for(i=0; i<SPUR_WIDTH; i++)
90 {
91 sptmmx[2*i ]=zmmx[2*i ];
92 sptmmx[2*i+1]=zmmx[2*i+1];
93 }
94 }
95 else
96 {
97 z=&fftx[2*(ffts_na*fftx_size+spur_location[spurno])];
98 for(i=0; i<SPUR_WIDTH; i++)
99 {
100 spt[2*i ]=z[2*i ];
101 spt[2*i+1]=z[2*i+1];
102 }
103 }
104 }
105 else
106 {
107 if(swmmx_fft2)
108 {
109 zmmx=&fft2_short_int[4*(ffts_na*fftx_size+spur_location[spurno])];
110 for(i=0; i<SPUR_WIDTH; i++)
111 {
112 sptmmx[4*i ]=zmmx[4*i ];
113 sptmmx[4*i+1]=zmmx[4*i+1];
114 sptmmx[4*i+2]=zmmx[4*i+2];
115 sptmmx[4*i+3]=zmmx[4*i+3];
116 }
117 }
118 else
119 {
120 z=&fftx[4*(ffts_na*fftx_size+spur_location[spurno])];
121 for(i=0; i<SPUR_WIDTH; i++)
122 {
123 spt[4*i ]=z[4*i ];
124 spt[4*i+1]=z[4*i+1];
125 spt[4*i+2]=z[4*i+2];
126 spt[4*i+3]=z[4*i+3];
127 }
128 }
129 }
130 k=spur_flag[spurno];
131 if(k > 5)k*=10;
132 k*=2;
133 spur_flag[spurno]++;
134 if(spur_flag[spurno]>1000000)
135 {
136 spur_flag[spurno]-=2*3*5*7*spur_speknum;
137 }
138 if(k%spur_speknum ==0)
139 {
140 // If we just lost locking or
141 // if we have been unlocked for a while, try to lock again.
142 i=spur_relock();
143 if(genparm[AFC_ENABLE] == 2 && i == FALSE)
144 {
145 no_of_spurs--;
146 if(no_of_spurs==0)return;
147 if(spurno != no_of_spurs)
148 {
149 remove_spur(spurno);
150 }
151 spurno--;
152 }
153 }
154 goto lock_fail;
155 }
156 // Use old PLL data to get the expected phase slope for the new transform
157 // Calculate spur_signal and store at zsig.
158 phase_slope=spur_d1pha[spurno]+spur_d2pha[spurno];
159 rot=-0.5*phase_slope/PI_L;
160 i=spur_freq[spurno]*spur_freq_factor-rot+0.5;
161 rot+=i;
162 freq=rot/spur_freq_factor;
163 spur_freq[spurno]=freq;
164 gtpos1:;
165 j=(int)(freq)+2-spur_location[spurno]-SPUR_SIZE/2;
166 if(j < 0 || j > 1)
167 {
168 if(j < -1 || j > 2)
169 {
170 spur_flag[spurno]=1;
171 goto lock_fail;
172 }
173 shift_spur_table(j);
174 goto gtpos1;
175 }
176 j=1-j;
177 ind=NO_OF_SPUR_SPECTRA*(freq-(int)(freq));
178 if(ind==NO_OF_SPUR_SPECTRA)ind=NO_OF_SPUR_SPECTRA-1;
179 ind=ind*SPUR_SIZE+j;
180 uind[ffts_na]=ind;
181 if(sw_onechan)
182 {
183 r1=0;
184 r2=0;
185 if(swmmx_fft2)
186 {
187 zmmx=&fft2_short_int[2*(ffts_na*fftx_size+spur_location[spurno])];
188 for(i=0; i<SPUR_WIDTH; i++)
189 {
190 sptmmx[2*i ]=zmmx[2*i ];
191 r1+=zmmx[2*i ]*spur_spectra[ind+i];
192 sptmmx[2*i+1]=zmmx[2*i+1];
193 r2+=zmmx[2*i+1]*spur_spectra[ind+i];
194 }
195 }
196 else
197 {
198 z=&fftx[2*(ffts_na*fftx_size+spur_location[spurno])];
199 for(i=0; i<SPUR_WIDTH; i++)
200 {
201 spt[2*i ]=z[2*i ];
202 r1+=z[2*i ]*spur_spectra[ind+i];
203 spt[2*i+1]=z[2*i+1];
204 r2+=z[2*i+1]*spur_spectra[ind+i];
205 }
206 }
207 }
208 else
209 {
210 t1=0;
211 t2=0;
212 t3=0;
213 t4=0;
214 if(swmmx_fft2)
215 {
216 zmmx=&fft2_short_int[4*(ffts_na*fftx_size+spur_location[spurno])];
217 for(i=0; i<SPUR_WIDTH; i++)
218 {
219 sptmmx[4*i ]=zmmx[4*i ];
220 t1+=zmmx[4*i ]*spur_spectra[ind+i];
221 sptmmx[4*i+1]=zmmx[4*i+1];
222 t2+=zmmx[4*i+1]*spur_spectra[ind+i];
223 sptmmx[4*i+2]=zmmx[4*i+2];
224 t3+=zmmx[4*i+2]*spur_spectra[ind+i];
225 sptmmx[4*i+3]=zmmx[4*i+3];
226 t4+=zmmx[4*i+3]*spur_spectra[ind+i];
227 }
228 }
229 else
230 {
231 z=&fftx[4*(ffts_na*fftx_size+spur_location[spurno])];
232 for(i=0; i<SPUR_WIDTH; i++)
233 {
234 spt[4*i ]=z[4*i ];
235 t1+=z[4*i ]*spur_spectra[ind+i];
236 spt[4*i+1]=z[4*i+1];
237 t2+=z[4*i+1]*spur_spectra[ind+i];
238 spt[4*i+2]=z[4*i+2];
239 t3+=z[4*i+2]*spur_spectra[ind+i];
240 spt[4*i+3]=z[4*i+3];
241 t4+=z[4*i+3]*spur_spectra[ind+i];
242 }
243 }
244 r1=sp_c1*t1+sp_c2*t3+sp_c3*t4;
245 r2=sp_c1*t2+sp_c2*t4-sp_c3*t3;
246 r3=sp_c1*t3-sp_c2*t1+sp_c3*t2;
247 r4=sp_c1*t4-sp_c2*t2-sp_c3*t1;
248 if((j^(spur_location[spurno]&1))==1)
249 {
250 r3=-r3;
251 r4=-r4;
252 }
253 zsig[4*ffts_na+2]=r3;
254 zsig[4*ffts_na+3]=r4;
255 }
256 if((j^(spur_location[spurno]&1))==1)
257 {
258 r1=-r1;
259 r2=-r2;
260 }
261 zsig[twice_rxchan*ffts_na ]=r1;
262 zsig[twice_rxchan*ffts_na+1]=r2;
263 iter=0;
264 freq=spur_freq[spurno];
265 refine:;
266 iter++;
267 refine_pll_parameters();
268 // **************************************************************
269 // Now, that the LO is changed, check if we have to recalculate
270 // spur_signal. In the spur_signal evaluation we need to know
271 // the LO frequency (phase_slope) in order to weigh the
272 // individual spectral lines properly.
273 phase_slope=spur_d1pha[spurno];
274 phase_curv=spur_d2pha[spurno];
275 phase_slope+=phase_curv;
276 nx=(ffts_na-spur_speknum+fftxn_mask)&fftxn_mask;
277 diffind=0;
278 ni=ffts_na;
279 while(ni != nx)
280 {
281 rot=-0.5*phase_slope/PI_L;
282 i=freq*spur_freq_factor-rot+0.5;
283 rot+=i;
284 freq=rot/spur_freq_factor;
285 j=(int)(freq)+2-spur_location[spurno]-SPUR_SIZE/2;
286 if(j < 0 || j > 1)goto iterx;
287 j=1-j;
288 ind=NO_OF_SPUR_SPECTRA*(freq-(int)(freq));
289 if(ind==NO_OF_SPUR_SPECTRA)ind=NO_OF_SPUR_SPECTRA-1;
290 ind=ind*SPUR_SIZE+j;
291 k=(uind[ni]-ind+NO_OF_SPUR_SPECTRA*SPUR_SIZE)&
292 (NO_OF_SPUR_SPECTRA*SPUR_SIZE-1);
293 if(k > NO_OF_SPUR_SPECTRA*SPUR_SIZE/2)k=NO_OF_SPUR_SPECTRA*SPUR_SIZE-k;
294 if(k > diffind)diffind=k;
295 uind[ni]=ind;
296 if(k != 0)
297 {
298 knd=spurp0+ni*SPUR_WIDTH*twice_rxchan;
299 if(sw_onechan)
300 {
301 r1=0;
302 r2=0;
303 if(swmmx_fft2)
304 {
305 for(i=0; i<SPUR_WIDTH; i++)
306 {
307 r1+=spur_table_mmx[2*i+knd ]*spur_spectra[ind+i];
308 r2+=spur_table_mmx[2*i+knd+1]*spur_spectra[ind+i];
309 }
310 }
311 else
312 {
313 for(i=0; i<SPUR_WIDTH; i++)
314 {
315 r1+=spur_table[2*i+knd ]*spur_spectra[ind+i];
316 r2+=spur_table[2*i+knd+1]*spur_spectra[ind+i];
317 }
318 }
319 }
320 else
321 {
322 t1=0;
323 t2=0;
324 t3=0;
325 t4=0;
326 if(swmmx_fft2)
327 {
328 for(i=0; i<SPUR_WIDTH; i++)
329 {
330 t1+=spur_table_mmx[4*i+knd ]*spur_spectra[ind+i];
331 t2+=spur_table_mmx[4*i+knd+1]*spur_spectra[ind+i];
332 t3+=spur_table_mmx[4*i+knd+2]*spur_spectra[ind+i];
333 t4+=spur_table_mmx[4*i+knd+3]*spur_spectra[ind+i];
334 }
335 }
336 else
337 {
338 for(i=0; i<SPUR_WIDTH; i++)
339 {
340 t1+=spur_table[4*i+knd ]*spur_spectra[ind+i];
341 t2+=spur_table[4*i+knd+1]*spur_spectra[ind+i];
342 t3+=spur_table[4*i+knd+2]*spur_spectra[ind+i];
343 t4+=spur_table[4*i+knd+3]*spur_spectra[ind+i];
344 }
345 }
346 r1=sp_c1*t1+sp_c2*t3+sp_c3*t4;
347 r2=sp_c1*t2+sp_c2*t4-sp_c3*t3;
348 r3=sp_c1*t3-sp_c2*t1+sp_c3*t2;
349 r4=sp_c1*t4-sp_c2*t2-sp_c3*t1;
350 if((j^(spur_location[spurno]&1))==1)
351 {
352 r3=-r3;
353 r4=-r4;
354 }
355 zsig[4*ni+2]=r3;
356 zsig[4*ni+3]=r4;
357 }
358 if((j^(spur_location[spurno]&1))==1)
359 {
360 r1=-r1;
361 r2=-r2;
362 }
363 zsig[twice_rxchan*ni ]=r1;
364 zsig[twice_rxchan*ni+1]=r2;
365 }
366 phase_slope-=phase_curv;
367 ni=(ni+fftxn_mask)&fftxn_mask;
368 }
369 if(diffind > 2.5*SPUR_SIZE && iter < 5)goto refine;
370 iterx:;
371 if(diffind!=0)refine_pll_parameters();
372 if(fabs(spur_ampl[spurno])< spur_minston*spur_noise[spurno])
373 {
374 spur_flag[spurno]=1;
375 goto lock_fail;
376 }
377 // If we have reached this far, the spur phase is well established.
378 // If we have two rx channels, update the polarization.
379 // The polarization is assumed to vary slowly with time so there
380 // is no reason to remake spur_signal
381 if(ui.rx_rf_channels == 2)
382 {
383 if(update_spur_pol() != 0) refine_pll_parameters();
384 }
385 // Subtract the spur from the new transform.
386 phase=spur_d0pha[spurno];
387 phase_slope=spur_d1pha[spurno];
388 phase_curv=spur_d2pha[spurno];
389 ampl=spur_ampl[spurno];
390 phase_slope+=phase_curv;
391 phase+=phase_slope;
392 spur_d0pha[spurno]=phase;
393 spur_d1pha[spurno]=phase_slope;
394 if(spur_d0pha[spurno] > PI_L)spur_d0pha[spurno]-=2*PI_L;
395 if(spur_d0pha[spurno] <-PI_L)spur_d0pha[spurno]+=2*PI_L;
396 if(spur_d1pha[spurno] > PI_L)spur_d1pha[spurno]-=2*PI_L;
397 if(spur_d1pha[spurno] <-PI_L)spur_d1pha[spurno]+=2*PI_L;
398 if(spur_d2pha[spurno] > PI_L)spur_d2pha[spurno]-=2*PI_L;
399 if(spur_d2pha[spurno] <-PI_L)spur_d2pha[spurno]+=2*PI_L;
400 spur_avgd2[spurno]=spur_weiold*spur_avgd2[spurno]+spur_weinew*phase_curv;
401 rot=-0.5*phase_slope/PI_L;
402 i=spur_freq[spurno]*spur_freq_factor-rot+0.5;
403 rot+=i;
404 freq=rot/spur_freq_factor;
405 spur_freq[spurno]=freq;
406 gtpos3:;
407 j=(int)(freq)+2-spur_location[spurno]-SPUR_SIZE/2;
408 if(j < 0 || j > 1)
409 {
410 if(j < -1 || j > 2)
411 {
412 spur_flag[spurno]=1;
413 goto lock_fail;
414 }
415 shift_spur_table(j);
416 goto gtpos3;
417 }
418 j=1-j;
419 ind=NO_OF_SPUR_SPECTRA*(freq-(int)(freq));
420 if(ind==NO_OF_SPUR_SPECTRA)ind=NO_OF_SPUR_SPECTRA-1;
421 ind=ind*SPUR_SIZE+j;
422 if(sw_onechan)
423 {
424 if((j^(spur_location[spurno]&1))==1)
425 {
426 t1=-cos(phase)*ampl;
427 t2=-sin(phase)*ampl;
428 }
429 else
430 {
431 t1=cos(phase)*ampl;
432 t2=sin(phase)*ampl;
433 }
434 if(swmmx_fft2)
435 {
436 zmmx=&fft2_short_int[2*(ffts_na*fftx_size+spur_location[spurno])];
437 for(i=0; i<SPUR_WIDTH; i++)
438 {
439 zmmx[2*i ]-=spur_spectra[ind+i]*t1;
440 zmmx[2*i+1]-=spur_spectra[ind+i]*t2;
441 }
442 }
443 else
444 {
445 z=&fftx[2*(ffts_na*fftx_size+spur_location[spurno])];
446 for(i=0; i<SPUR_WIDTH; i++)
447 {
448 z[2*i ]-=spur_spectra[ind+i]*t1;
449 z[2*i+1]-=spur_spectra[ind+i]*t2;
450 }
451 }
452 }
453 else
454 {
455 if((j^(spur_location[spurno]&1))==1)
456 {
457 t1=-sp_c1*cos(phase)*ampl;
458 t2=-sp_c1*sin(phase)*ampl;
459 t3=-(sp_c2*cos(phase)-sp_c3*sin(phase))*ampl;
460 t4=-(sp_c2*sin(phase)+sp_c3*cos(phase))*ampl;
461 }
462 else
463 {
464 t1=sp_c1*cos(phase)*ampl;
465 t2=sp_c1*sin(phase)*ampl;
466 t3=(sp_c2*cos(phase)-sp_c3*sin(phase))*ampl;
467 t4=(sp_c2*sin(phase)+sp_c3*cos(phase))*ampl;
468 }
469 if(swmmx_fft2)
470 {
471 zmmx=&fft2_short_int[4*(ffts_na*fft2_size+spur_location[spurno])];
472 for(i=0; i<SPUR_WIDTH; i++)
473 {
474 zmmx[4*i ]-=spur_spectra[ind+i]*t1;
475 zmmx[4*i+1]-=spur_spectra[ind+i]*t2;
476 zmmx[4*i+2]-=spur_spectra[ind+i]*t3;
477 zmmx[4*i+3]-=spur_spectra[ind+i]*t4;
478 }
479 }
480 else
481 {
482 z=&fftx[4*(ffts_na*fftx_size+spur_location[spurno])];
483 for(i=0; i<SPUR_WIDTH; i++)
484 {
485 z[4*i ]-=spur_spectra[ind+i]*t1;
486 z[4*i+1]-=spur_spectra[ind+i]*t2;
487 z[4*i+2]-=spur_spectra[ind+i]*t3;
488 z[4*i+3]-=spur_spectra[ind+i]*t4;
489 }
490 }
491 }
492 lock_fail:;
493 }
494 }
495
update_spur_pol(void)496 int update_spur_pol(void)
497 {
498 int i,ni;
499 float *z;
500 float c1,c2,c3,sina;
501 float a2,b2,re,im;
502 float t1,t2,t3,t4,r1,r2,noi2,a2s,b2s;
503 // The data in spur signal is combined by use of the current
504 // polarization coefficient for the first channel to contain
505 // the spur and for the second channel to contain noise only.
506 // Get the correlation between the first and the second channel.
507 // If it is nonzero, there is some contribution from the
508 // spur in the second channel.
509 // Use the correlation to improve the polarization coefficients.
510 // See fft3_mix2() in mix2.c for explanation.
511 z=&spur_signal[4*max_fftxn*spurno];
512 a2=0;
513 b2=0;
514 re=0;
515 im=0;
516 ni=ffts_na;
517 for(i=0; i<spur_speknum; i++)
518 {
519 a2+=z[4*ni ]*z[4*ni ]+z[4*ni+1]*z[4*ni+1];
520 b2+=z[4*ni+2]*z[4*ni+2]+z[4*ni+3]*z[4*ni+3];
521 re+=z[4*ni ]*z[4*ni+2]+z[4*ni+1]*z[4*ni+3];
522 im+=z[4*ni+1]*z[4*ni+2]-z[4*ni ]*z[4*ni+3];
523 ni=(ni+fftxn_mask)&fftxn_mask;
524 }
525 t1=a2+b2;
526 a2/=t1;
527 b2/=t1;
528 re/=t1;
529 im/=t1;
530 t2=re*re+im*im;
531 noi2=a2*b2-t2;
532 a2s=a2-noi2;
533 b2s=b2-noi2;
534 if(b2s <=0.0001 || t2 == 0)return 0;
535 if(a2s > 0)
536 {
537 c1=sqrt(a2s);
538 sina=sqrt(b2s);
539 c2=sina*re/sqrt(t2);
540 c3=-sina*im/sqrt(t2);
541 t1=sqrt(c1*c1+c2*c2+c3*c3);
542 if(c2 < 0)t1=-t1;
543 c1/=t1;
544 c2/=t1;
545 c3/=t1;
546 }
547 else
548 {
549 if(a2 > b2)
550 {
551 c1=1;
552 c2=0;
553 }
554 else
555 {
556 c1=0;
557 c2=1;
558 }
559 c3=0;
560 }
561 t1=c1*sp_c1-c2*sp_c2-c3*sp_c3;
562 t2=c3*sp_c2-c2*sp_c3;
563 t3=c1*sp_c2+c2*sp_c1;
564 t4=c1*sp_c3+c3*sp_c1;
565 // We want t2 to be zero so we adjust the phase.
566 c1=sqrt(t1*t1+t2*t2);
567 r2=sqrt(t3*t3+t4*t4);
568 r1=atan2(t3,t4)+atan2(t1,t2);
569 c2=-r2*cos(r1);
570 c3=r2*sin(r1);
571 t1=sqrt(c1*c1+c2*c2+c3*c3);
572 t2=c1*sp_c1+c2*sp_c2+c3*sp_c3;
573 if(t2 < 0)t1=-t1;
574 c1/=t1;
575 c2/=t1;
576 c3/=t1;
577 t2=sqrt((c1-sp_c1)*(c1-sp_c1)+(c2-sp_c2)*(c2-sp_c2)+(c3-sp_c3)*(c3-sp_c3));
578 t2/=spur_speknum;
579 t1=spur_speknum-1;
580 sp_c1=(t1*sp_c1+c1)/spur_speknum;
581 sp_c2=(t1*sp_c2+c2)/spur_speknum;
582 sp_c3=(t1*sp_c3+c3)/spur_speknum;
583 t1=sqrt(sp_c1*sp_c1+sp_c2*sp_c2+sp_c3*sp_c3);
584 if(sp_c2 < 0)t1=-t1;
585 sp_c1/=t1;
586 sp_c2/=t1;
587 sp_c3/=t1;
588 i=3*spurno;
589 spur_pol[i ]=sp_c1;
590 spur_pol[i+1]=sp_c2;
591 spur_pol[i+2]=sp_c3;
592 if(t2 > 0.05)return 1;
593 return 0;
594 }
595
remove_spur(int ia)596 void remove_spur(int ia)
597 {
598 int i;
599 spur_flag[ia]=spur_flag[no_of_spurs];
600 spur_location[ia]=spur_location[no_of_spurs];
601 spur_d0pha[ia]=spur_d0pha[no_of_spurs];
602 spur_d1pha[ia]=spur_d1pha[no_of_spurs];
603 spur_d2pha[ia]=spur_d2pha[no_of_spurs];
604 spur_ampl[ia]=spur_ampl[no_of_spurs];
605 spur_noise[ia]=spur_noise[no_of_spurs];
606 spur_avgd2[ia]=spur_avgd2[no_of_spurs];
607 spur_pol[ia]=spur_pol[no_of_spurs];
608 spur_freq[ia]=spur_freq[no_of_spurs];
609 if(genparm[SECOND_FFT_ENABLE] !=0 && fft_cntrl[FFT2_CURMODE].mmx != 0)
610 {
611 for(i=0; i<spur_block; i++)
612 {
613 spur_table_mmx[ia*spur_block+i]=spur_table_mmx[no_of_spurs*spur_block+i];
614 }
615 }
616 else
617 {
618 for(i=0; i<spur_block; i++)
619 {
620 spur_table[ia*spur_block+i]=spur_table[no_of_spurs*spur_block+i];
621 }
622 }
623 for(i=0; i<twice_rxchan*max_fftxn; i++)
624 {
625 spur_signal[twice_rxchan*max_fftxn*ia+i]=
626 spur_signal[twice_rxchan*max_fftxn*no_of_spurs+i];
627 }
628 for(i=0; i<max_fftxn; i++)
629 {
630 spur_ind[max_fftxn*ia+i]=spur_ind[max_fftxn*no_of_spurs+i];
631 }
632 }
633
refine_pll_parameters(void)634 void refine_pll_parameters(void)
635 {
636 int i, ni;
637 float r1;
638 float a1, a2, b1, b2, d1, d2;
639 float phase, phase_slope, phase_curv;
640 float *zsig;
641 // We have at least one new data point in spur_signal.
642 // The current set of pll parameters give a good description
643 // of the spur signal for the old points.
644 // Split spur_signal in one component that is in phase with the pll LO
645 // and one component that is orthogonal.
646 zsig=&spur_signal[twice_rxchan*max_fftxn*spurno];
647 ni=ffts_na;
648 phase=spur_d0pha[spurno];
649 phase_slope=spur_d1pha[spurno];
650 phase_curv=spur_d2pha[spurno];
651 phase_slope+=phase_curv;
652 phase+=phase_slope;
653 a1=cos(phase);
654 a2=sin(phase);
655 b1=cos(phase_slope);
656 b2=sin(phase_slope);
657 d1=cos(phase_curv);
658 d2=sin(phase_curv);
659 for(i=spur_speknum-1; i>=0; i--)
660 {
661 sp_sig[2*i ]=a1*zsig[twice_rxchan*ni ]+a2*zsig[twice_rxchan*ni+1];
662 sp_sig[2*i+1]=a1*zsig[twice_rxchan*ni+1]-a2*zsig[twice_rxchan*ni ];
663 r1=a1*b1+a2*b2;
664 a2=a2*b1-a1*b2;
665 a1=r1;
666 r1=b1*d1+b2*d2;
667 b2=b2*d1-b1*d2;
668 b1=r1;
669 ni=(ni+fftxn_mask)&fftxn_mask;
670 }
671 spur_phase_parameters();
672 phase+=sp_d0;
673 phase_slope+=sp_d1;
674 phase_curv+=sp_d2;
675 phase-=phase_slope;
676 phase_slope-=phase_curv;
677 spur_d0pha[spurno]=phase;
678 spur_d1pha[spurno]=phase_slope;
679 spur_d2pha[spurno]=phase_curv;
680 }
681
spur_relock(void)682 int spur_relock(void)
683 {
684 int i, j, k, np, nx, ni, pa, izz, ind, pnt, maxrem;
685 float t1,t2,t3,t4,r1,r2,r3,r4,p1,p2;
686 float a1,a2,b1,b2,d1,d2;
687 float rot;
688 float *z, *pwr, *sumsq;
689 float freq, phase, phase_slope, phase_curv;
690 short int *zmmx;
691 TWOCHAN_POWER *pxy, *xysum;
692 int *uind;
693 // Store old transforms in spur table for the current spur and
694 // accumulate power spectrum.
695 spurp0=spurno*spur_block;
696 np=(ffts_na-spur_speknum+max_fftxn)&fftxn_mask;
697 nx=(ffts_na+1)&fftxn_mask;
698 if(swmmx_fft2)
699 {
700 if(sw_onechan)
701 {
702 // With one channel only, just use the power spectrum.
703 for(i=0; i<SPUR_WIDTH; i++)spur_power[i]=0;
704 while(np != nx)
705 {
706 k=spurp0+np*SPUR_WIDTH*twice_rxchan;
707 for(i=0; i<SPUR_WIDTH; i++)
708 {
709 t1=spur_table_mmx[2*i+k ];
710 t2=spur_table_mmx[2*i+k+1];
711 spur_power[i]+=t1*t1+t2*t2;
712 }
713 np=(np+1)&fftxn_mask;
714 }
715 }
716 else //ui.rx_rf_channels = 2
717 {
718 // We have two channels and polarization is unknown.
719 // First make an average of channel powers and correlations.
720 for(i=0; i<SPUR_WIDTH; i++)
721 {
722 spur_pxy[i].x2=0;
723 spur_pxy[i].y2=0;
724 spur_pxy[i].re_xy=0;
725 spur_pxy[i].im_xy=0;
726 }
727 while(np != nx)
728 {
729 k=spurp0+np*SPUR_WIDTH*twice_rxchan;
730 for(i=0; i<SPUR_WIDTH; i++)
731 {
732 t1=spur_table_mmx[4*i+k ];
733 t2=spur_table_mmx[4*i+k+1];
734 t3=spur_table_mmx[4*i+k+2];
735 t4=spur_table_mmx[4*i+k+3];
736 spur_pxy[i].x2+=t1*t1+t2*t2;
737 spur_pxy[i].y2+=t3*t3+t4*t4;
738 spur_pxy[i].re_xy+=t1*t3+t2*t4;
739 spur_pxy[i].im_xy+=t2*t3-t1*t4;
740 }
741 np=(np+1)&fftxn_mask;
742 }
743 }
744 }
745 else
746 {
747 if(sw_onechan)
748 {
749 // With one channel only, just use the power spectrum.
750 for(i=0; i<SPUR_WIDTH; i++)spur_power[i]=0;
751 while(np != nx)
752 {
753 k=spurp0+np*SPUR_WIDTH*twice_rxchan;
754 for(i=0; i<SPUR_WIDTH; i++)
755 {
756 t1=spur_table[2*i+k ];
757 t2=spur_table[2*i+k+1];
758 spur_power[i]+=t1*t1+t2*t2;
759 }
760 np=(np+1)&fftxn_mask;
761 }
762 }
763 else //ui.rx_rf_channels = 2
764 {
765 // We have two channels and polarization is unknown.
766 // First make an average of channel powers and correlations.
767 for(i=0; i<SPUR_WIDTH; i++)
768 {
769 spur_pxy[i].x2=0;
770 spur_pxy[i].y2=0;
771 spur_pxy[i].re_xy=0;
772 spur_pxy[i].im_xy=0;
773 }
774 while(np != nx)
775 {
776 k=spurp0+np*SPUR_WIDTH*twice_rxchan;
777 for(i=0; i<SPUR_WIDTH; i++)
778 {
779 t1=spur_table[4*i+k ];
780 t2=spur_table[4*i+k+1];
781 t3=spur_table[4*i+k+2];
782 t4=spur_table[4*i+k+3];
783 spur_pxy[i].x2+=t1*t1+t2*t2;
784 spur_pxy[i].y2+=t3*t3+t4*t4;
785 spur_pxy[i].re_xy+=t1*t3+t2*t4;
786 spur_pxy[i].im_xy+=t2*t3-t1*t4;
787 }
788 np=(np+1)&fftxn_mask;
789 }
790 }
791 }
792 if(sw_onechan)
793 {
794 spurspek_norm();
795 }
796 else
797 {
798 // Two channels. Extract the power for a signal using the channel
799 // correlation (Slightly better than just adding x2 and y2)
800 for(i=0; i<SPUR_WIDTH; i++)
801 {
802 t1=spur_pxy[i].x2+spur_pxy[i].y2;
803 spur_power[i]=t1+2*(spur_pxy[i].re_xy*spur_pxy[i].re_xy+
804 spur_pxy[i].im_xy*spur_pxy[i].im_xy
805 -spur_pxy[i].x2*spur_pxy[i].y2 ) / t1;
806 }
807 spurspek_norm();
808 make_spur_pol();
809 }
810
811
812 spur_freq[spurno]=spur_location[spurno]+(float)(SPUR_SIZE)*0.5;
813 if(make_spur_freq() != 0)
814 {
815 return FALSE;
816 }
817 if(spur_freq[spurno]+2-spur_location[spurno]-SPUR_SIZE/2 >
818 SPUR_WIDTH/2-0.5)return FALSE;
819 // The power spectrum has a maximum within range.
820 // Calculate spur_signal using the power spectrum.
821 if(spur_phase_lock(nx)!=0)return FALSE;
822 // Locking was sucessful.
823 // Remove the spur backwards in time.
824 // First find out how many old transforms we still did not use completely.
825 // Make k the number of transforms we can change to correct the latest
826 // and still unwritten line of the waterfall graph.
827 // Make j the number of transforms that will affect the output.
828 j=0;
829 if(genparm[SECOND_FFT_ENABLE] == 0)
830 {
831 maxrem=1+fft1_sumsq_counter;
832 maxrem+=((fft1_sumsq_pa-fft1_sumsq_pwg+fft1_sumsq_bufsize)&
833 fft1_sumsq_mask)/fft1_size;
834 maxrem+=wg_waterf_sum_counter;
835 if(new_baseb_flag >= 0)
836 {
837 j=(fft1_nb-fft1_nx+max_fft1n)&fft1n_mask;
838 if(j<fft1n_mask)j++;
839 }
840 }
841 else
842 {
843 maxrem=wg_waterf_sum_counter;
844 if(maxrem < hg.spek_avgnum)maxrem=hg.spek_avgnum;
845 if(new_baseb_flag >= 0)
846 {
847 j=(fft2_na-fft2_nx+max_fft2n)&fft2n_mask;
848 if(j<fft2n_mask)j++;
849 }
850 }
851 if(j>maxrem)maxrem=j;
852 if(maxrem > fftxn_mask)maxrem=fftxn_mask;
853 if(maxrem >= spur_flag[spurno])
854 {
855 maxrem=spur_flag[spurno];
856 }
857 spur_flag[spurno]=0;
858 if(maxrem > spur_speknum)
859 {
860 maxrem-=spur_speknum;
861 nx=(ffts_na-spur_speknum+max_fftxn)&fftxn_mask;
862 }
863 else
864 {
865 nx=(ffts_na-maxrem+max_fftxn)&fftxn_mask;
866 maxrem=0;
867 }
868 // First remove the spur from the transforms we have used to get the
869 // parameters from.
870 phase=spur_d0pha[spurno];
871 phase_slope=spur_d1pha[spurno];
872 phase_curv=spur_d2pha[spurno];
873 phase_slope+=phase_curv;
874 phase+=phase_slope;
875 freq=spur_freq[spurno];
876 a1=spur_ampl[spurno]*cos(phase);
877 a2=spur_ampl[spurno]*sin(phase);
878 b1=cos(phase_slope);
879 b2=sin(phase_slope);
880 d1=cos(spur_d2pha[spurno]);
881 d2=sin(spur_d2pha[spurno]);
882 pnt=spur_location[spurno];
883 izz=0;
884 ni=ffts_na;
885 uind=&spur_ind[spurno*max_fftxn];
886 while(ni != nx)
887 {
888 rot=-0.5*phase_slope/PI_L;
889 i=freq*spur_freq_factor-rot+0.5;
890 rot+=i;
891 freq=rot/spur_freq_factor;
892 j=(int)(freq)+2-spur_location[spurno]-SPUR_SIZE/2;
893 if(j < -1 || j > 2)return FALSE;
894 if(j<0)j=0;
895 j=1-j;
896 if(j<0)j=0;
897 ind=NO_OF_SPUR_SPECTRA*(freq-(int)(freq));
898 if(ind==NO_OF_SPUR_SPECTRA)ind=NO_OF_SPUR_SPECTRA-1;
899 ind=ind*SPUR_SIZE+j;
900 uind[ni]=ind;
901 if(sw_onechan)
902 {
903 if((j^(spur_location[spurno]&1))==1)
904 {
905 t1=-a1;
906 t2=-a2;
907 }
908 else
909 {
910 t1=a1;
911 t2=a2;
912 }
913 pwr=&fftx_pwr[ni*fftx_size+pnt];
914 if(swmmx_fft2)
915 {
916 zmmx=&fft2_short_int[2*(ni*fftx_size+pnt)];
917 for(i=0; i<SPUR_WIDTH; i++)
918 {
919 r1=zmmx[2*i ]-spur_spectra[ind+i]*t1;
920 r2=zmmx[2*i+1]-spur_spectra[ind+i]*t2;
921 zmmx[2*i ]=r1;
922 zmmx[2*i+1]=r2;
923 pwr[i]=r1*r1+r2*r2;
924 }
925 }
926 else
927 {
928 z=&fftx[2*(ni*fftx_size+pnt)];
929 for(i=0; i<SPUR_WIDTH; i++)
930 {
931 z[2*i ]-=spur_spectra[ind+i]*t1;
932 z[2*i+1]-=spur_spectra[ind+i]*t2;
933 pwr[i]=z[2*i]*z[2*i]+z[2*i+1]*z[2*i+1];
934 }
935 }
936 }
937 else
938 {
939 if((j^(spur_location[spurno]&1))==1)
940 {
941 t1=-sp_c1*a1;
942 t2=-sp_c1*a2;
943 t3=-sp_c2*a1+sp_c3*a2;
944 t4=-sp_c2*a2-sp_c3*a1;
945 }
946 else
947 {
948 t1=sp_c1*a1;
949 t2=sp_c1*a2;
950 t3=sp_c2*a1-sp_c3*a2;
951 t4=sp_c2*a2+sp_c3*a1;
952 }
953 pxy=(TWOCHAN_POWER*)(&fftx_xypower[np*fftx_size+pnt].x2);
954 if(swmmx_fft2)
955 {
956 zmmx=&fft2_short_int[4*(ni*fftx_size+pnt)];
957 for(i=0; i<SPUR_WIDTH; i++)
958 {
959 r1=zmmx[4*i ]-spur_spectra[ind+i]*t1;
960 r2=zmmx[4*i+1]-spur_spectra[ind+i]*t2;
961 r3=zmmx[4*i+2]-spur_spectra[ind+i]*t3;
962 r4=zmmx[4*i+3]-spur_spectra[ind+i]*t4;
963 zmmx[4*i ]=r1;
964 zmmx[4*i+1]=r2;
965 zmmx[4*i+2]=r3;
966 zmmx[4*i+3]=r4;
967 pxy[i].x2=r1*r1+r2*r2;
968 pxy[i].y2=r3*r3+r4*r4;
969 pxy[i].re_xy=r1*r3+r2*r4;
970 pxy[i].im_xy=r2*r3-r1*r4;
971 }
972 }
973 else
974 {
975 z=&fftx[4*(ni*fftx_size+pnt)];
976 for(i=0; i<SPUR_WIDTH; i++)
977 {
978 z[4*i ]-=spur_spectra[ind+i]*t1;
979 z[4*i+1]-=spur_spectra[ind+i]*t2;
980 z[4*i+2]-=spur_spectra[ind+i]*t3;
981 z[4*i+3]-=spur_spectra[ind+i]*t4;
982 pxy[i].x2=z[2*i ]*z[2*i ]+z[2*i+1]*z[2*i+1];
983 pxy[i].y2=z[2*i+2]*z[2*i+2]+z[2*i+3]*z[2*i+3];
984 pxy[i].re_xy=z[2*i ]*z[2*i+2]+z[2*i+1]*z[2*i+3];
985 pxy[i].im_xy=z[2*i+1]*z[2*i+2]-z[2*i ]*z[2*i+3];
986 }
987 }
988 }
989 r1=a1*b1+a2*b2;
990 a2=a2*b1-a1*b2;
991 a1=r1;
992 r2=b1*d1+b2*d2;
993 b2=b2*d1-b1*d2;
994 b1=r2;
995 phase_slope-=phase_curv;
996 ni=(ni+fftxn_mask)&fftxn_mask;
997 izz++;
998 }
999 // Step further backwards while checking that the total power in the
1000 // transform is actually reduced.
1001 while(maxrem >0)
1002 {
1003 maxrem--;
1004 rot=-0.5*phase_slope/PI_L;
1005 i=freq*spur_freq_factor-rot+0.5;
1006 rot+=i;
1007 freq=rot/spur_freq_factor;
1008 j=(int)(freq)+2-spur_location[spurno]-SPUR_SIZE/2;
1009 if(j < -1 || j > 2)return FALSE;
1010 if(j<0)j=0;
1011 j=1-j;
1012 if(j<0)j=0;
1013 ind=NO_OF_SPUR_SPECTRA*(freq-(int)(freq));
1014 if(ind==NO_OF_SPUR_SPECTRA)ind=NO_OF_SPUR_SPECTRA-1;
1015 ind=ind*SPUR_SIZE+j;
1016 p1=0;
1017 p2=0;
1018 if(sw_onechan)
1019 {
1020 if((j^(spur_location[spurno]&1))==1)
1021 {
1022 t1=-a1;
1023 t2=-a2;
1024 }
1025 else
1026 {
1027 t1=a1;
1028 t2=a2;
1029 }
1030 pwr=&fftx_pwr[ni*fftx_size+pnt];
1031 if(swmmx_fft2)
1032 {
1033 zmmx=&fft2_short_int[2*(ni*fftx_size+pnt)];
1034 for(i=0; i<SPUR_WIDTH; i++)
1035 {
1036 p1+=pwr[i];
1037 r1=zmmx[2*i ]-spur_spectra[ind+i]*t1;
1038 r2=zmmx[2*i+1]-spur_spectra[ind+i]*t2;
1039 zmmx[2*i ]=t1;
1040 zmmx[2*i+1]=t2;
1041 pwr[i]=r1*r1+r2*r2;
1042 p2+=pwr[i];
1043 }
1044 if(p2 > p1)
1045 {
1046 for(i=0; i<SPUR_WIDTH; i++)
1047 {
1048 r1=zmmx[2*i ]-spur_spectra[ind+i]*t1;
1049 r2=zmmx[2*i+1]-spur_spectra[ind+i]*t2;
1050 zmmx[2*i ]=r1;
1051 zmmx[2*i+1]=r2;
1052 pwr[i]=r1*r1+r2*r2;
1053 }
1054 }
1055 }
1056 else
1057 {
1058 z=&fftx[2*(ni*fftx_size+pnt)];
1059 for(i=0; i<SPUR_WIDTH; i++)
1060 {
1061 p1+=pwr[i];
1062 z[2*i ]-=spur_spectra[ind+i]*t1;
1063 z[2*i+1]-=spur_spectra[ind+i]*t2;
1064 pwr[i]=z[2*i]*z[2*i]+z[2*i+1]*z[2*i+1];
1065 p2+=pwr[i];
1066 }
1067 if(p2 > p1)
1068 {
1069 for(i=0; i<SPUR_WIDTH; i++)
1070 {
1071 z[2*i ]+=spur_spectra[ind+i]*t1;
1072 z[2*i+1]+=spur_spectra[ind+i]*t2;
1073 pwr[i]=z[2*i]*z[2*i]+z[2*i+1]*z[2*i+1];
1074 }
1075 }
1076 }
1077 }
1078 else
1079 {
1080 if((j^(spur_location[spurno]&1))==1)
1081 {
1082 t1=-sp_c1*a1;
1083 t2=-sp_c1*a2;
1084 t3=-sp_c2*a1+sp_c3*a2;
1085 t4=-sp_c2*a2-sp_c3*a1;
1086 }
1087 else
1088 {
1089 t1=sp_c1*a1;
1090 t2=sp_c1*a2;
1091 t3=sp_c2*a1-sp_c3*a2;
1092 t4=sp_c2*a2+sp_c3*a1;
1093 }
1094 pxy=(TWOCHAN_POWER*)(&fftx_xypower[np*fftx_size+pnt].x2);
1095 if(swmmx_fft2)
1096 {
1097 zmmx=&fft2_short_int[4*(ni*fftx_size+pnt)];
1098 for(i=0; i<SPUR_WIDTH; i++)
1099 {
1100 r1=zmmx[4*i ]-spur_spectra[ind+i]*t1;
1101 r2=zmmx[4*i+1]-spur_spectra[ind+i]*t2;
1102 r3=zmmx[4*i+2]-spur_spectra[ind+i]*t3;
1103 r4=zmmx[4*i+3]-spur_spectra[ind+i]*t4;
1104 zmmx[4*i ]=r1;
1105 zmmx[4*i+1]=r2;
1106 zmmx[4*i+2]=r3;
1107 zmmx[4*i+3]=r4;
1108 p1+=pxy[i].x2+pxy[i].y2;
1109 pxy[i].x2=r1*r1+r2*r2;
1110 pxy[i].y2=r3*r3+r4*r4;
1111 p2+=pxy[i].x2+pxy[i].y2;
1112 pxy[i].re_xy=r1*r3+r2*r4;
1113 pxy[i].im_xy=r2*r3-r1*r4;
1114 }
1115 if(p2 > p1)
1116 {
1117 for(i=0; i<SPUR_WIDTH; i++)
1118 {
1119 r1=zmmx[4*i ]+spur_spectra[ind+i]*t1;
1120 r2=zmmx[4*i+1]+spur_spectra[ind+i]*t2;
1121 r3=zmmx[4*i+2]+spur_spectra[ind+i]*t3;
1122 r4=zmmx[4*i+3]+spur_spectra[ind+i]*t4;
1123 zmmx[4*i ]=r1;
1124 zmmx[4*i+1]=r2;
1125 zmmx[4*i+2]=r3;
1126 zmmx[4*i+3]=r4;
1127 pxy[i].x2=r1*r1+r2*r2;
1128 pxy[i].y2=r3*r3+r4*r4;
1129 pxy[i].re_xy=r1*r3+r2*r4;
1130 pxy[i].im_xy=r2*r3-r1*r4;
1131 }
1132 }
1133 }
1134 else
1135 {
1136 z=&fftx[4*(ni*fftx_size+pnt)];
1137 for(i=0; i<SPUR_WIDTH; i++)
1138 {
1139 z[4*i ]-=spur_spectra[ind+i]*t1;
1140 z[4*i+1]-=spur_spectra[ind+i]*t2;
1141 z[4*i+2]-=spur_spectra[ind+i]*t3;
1142 z[4*i+3]-=spur_spectra[ind+i]*t4;
1143 p1+=pxy[i].x2+pxy[i].y2;
1144 pxy[i].x2=z[2*i ]*z[2*i ]+z[2*i+1]*z[2*i+1];
1145 pxy[i].y2=z[2*i+2]*z[2*i+2]+z[2*i+3]*z[2*i+3];
1146 p2+=pxy[i].x2+pxy[i].y2;
1147 pxy[i].re_xy=z[2*i ]*z[2*i+2]+z[2*i+1]*z[2*i+3];
1148 pxy[i].im_xy=z[2*i+1]*z[2*i+2]-z[2*i ]*z[2*i+3];
1149 }
1150 if(p2 > p1)
1151 {
1152 for(i=0; i<SPUR_WIDTH; i++)
1153 {
1154 z[4*i ]+=spur_spectra[ind+i]*t1;
1155 z[4*i+1]+=spur_spectra[ind+i]*t2;
1156 z[4*i+2]+=spur_spectra[ind+i]*t3;
1157 z[4*i+3]+=spur_spectra[ind+i]*t4;
1158 pxy[i].x2=z[2*i ]*z[2*i ]+z[2*i+1]*z[2*i+1];
1159 pxy[i].y2=z[2*i+2]*z[2*i+2]+z[2*i+3]*z[2*i+3];
1160 pxy[i].re_xy=z[2*i ]*z[2*i+2]+z[2*i+1]*z[2*i+3];
1161 pxy[i].im_xy=z[2*i+1]*z[2*i+2]-z[2*i ]*z[2*i+3];
1162 }
1163 }
1164 }
1165 }
1166 r1=a1*b1+a2*b2;
1167 a2=a2*b1-a1*b2;
1168 a1=r1;
1169 r2=b1*d1+b2*d2;
1170 b2=b2*d1-b1*d2;
1171 b1=r2;
1172 phase_slope-=phase_curv;
1173 ni=(ni+fftxn_mask)&fftxn_mask;
1174 izz++;
1175 }
1176 if(izz == 0)return FALSE;
1177 if(genparm[SECOND_FFT_ENABLE] == 0)
1178 {
1179 if(sw_onechan)
1180 {
1181 // Update fft1_sumsq
1182 if(fft1_sumsq_counter != 0)
1183 {
1184 izz-=fft1_sumsq_counter;
1185 sumsq=&fft1_sumsq[fft1_sumsq_pa+fft1_first_point+pnt];
1186 for(i=0; i<SPUR_WIDTH; i++)sumsq[i]=0;
1187 ni=fft1_nb;
1188 for(k=0; k<fft1_sumsq_counter; k++)
1189 {
1190 ni=(ni+fft1n_mask)&fft1n_mask;
1191 pwr=&fft1_power[ni*fft1_size+pnt];
1192 for(i=0; i<SPUR_WIDTH; i++)
1193 {
1194 fft1_sumsq[i]+=pwr[i];
1195 }
1196 }
1197 }
1198 pa=fft1_sumsq_pa;
1199 while(izz > 0)
1200 {
1201 izz-=wg.fft_avg1num;
1202 pa=(pa-fft1_size+fft1_sumsq_bufsize)&fft1_sumsq_mask;
1203 sumsq=&fft1_sumsq[pa+fft1_first_point+pnt];
1204 for(i=0; i<SPUR_WIDTH; i++)sumsq[i]=0;
1205 for(k=0; k<wg.fft_avg1num; k++)
1206 {
1207 ni=(ni+fft1n_mask)&fft1n_mask;
1208 pwr=&fft1_power[ni*fft1_size+pnt];
1209 for(i=0; i<SPUR_WIDTH; i++)
1210 {
1211 sumsq[i]+=pwr[i];
1212 }
1213 }
1214 }
1215 }
1216 else
1217 {
1218 // Update fft1_sumsq
1219 if(fft1_sumsq_counter != 0)
1220 {
1221 izz-=fft1_sumsq_counter;
1222 sumsq=&fft1_sumsq[fft1_sumsq_pa+fft1_first_point+pnt];
1223 for(i=0; i<SPUR_WIDTH; i++)sumsq[i]=0;
1224 ni=fft1_nb;
1225 for(k=0; k<fft1_sumsq_counter; k++)
1226 {
1227 ni=(ni+fft1n_mask)&fft1n_mask;
1228 pxy=&fftx_xypower[ni*fftx_size+pnt];
1229 for(i=0; i<SPUR_WIDTH; i++)
1230 {
1231 fft1_sumsq[i]+=pxy[i].x2+pxy[i].y2;
1232 }
1233 }
1234 }
1235 pa=fft1_sumsq_pa;
1236 while(izz > 0)
1237 {
1238 izz-=wg.fft_avg1num;
1239 pa=(pa-fft1_size+fft1_sumsq_bufsize)&fft1_sumsq_mask;
1240 sumsq=&fft1_sumsq[pa+fft1_first_point+pnt];
1241 for(i=0; i<SPUR_WIDTH; i++)sumsq[i]=0;
1242 for(k=0; k<wg.fft_avg1num; k++)
1243 {
1244 ni=(ni+fft1n_mask)&fft1n_mask;
1245 pxy=&fftx_xypower[ni*fftx_size+pnt];
1246 for(i=0; i<SPUR_WIDTH; i++)
1247 {
1248 sumsq[i]+=pxy[i].x2+pxy[i].y2;
1249 }
1250 }
1251 }
1252 i=(fft1_sumsq_pa-fft1_size+fft1_sumsq_bufsize)&fft1_sumsq_mask;
1253 new_fft1_averages(i, pnt+1,pnt+SPUR_WIDTH-1);
1254 pa=fft1_sumsq_pa;
1255 if(wg_waterf_sum_counter!=0)
1256 {
1257 k=wg_waterf_sum_counter;
1258 for(i=0; i<SPUR_WIDTH; i++)wg_waterf_sum[pnt+i]=0;
1259 while(k>0)
1260 {
1261 pa=(pa-fft1_size+fft1_sumsq_bufsize)&fft1_sumsq_mask;
1262 sumsq=&fft1_sumsq[pa+fft1_first_point+pnt];
1263 for(i=0; i<SPUR_WIDTH; i++)
1264 {
1265 wg_waterf_sum[pnt+i]+=sumsq[i];
1266 }
1267 k-=wg.fft_avg1num;
1268 }
1269 }
1270 }
1271 i=(fft1_sumsq_pa-fft1_size+fft1_sumsq_bufsize)&fft1_sumsq_mask;
1272 new_fft1_averages(i,pnt+1,pnt+SPUR_WIDTH-1);
1273 pa=fft1_sumsq_pa;
1274 if(wg_waterf_sum_counter!=0)
1275 {
1276 k=wg_waterf_sum_counter;
1277 for(i=0; i<SPUR_WIDTH; i++)wg_waterf_sum[pnt+i]=0;
1278 while(k>0)
1279 {
1280 pa=(pa-fft1_size+fft1_sumsq_bufsize)&fft1_sumsq_mask;
1281 sumsq=&fft1_sumsq[pa+fft1_first_point+pnt];
1282 for(i=0; i<SPUR_WIDTH; i++)
1283 {
1284 wg_waterf_sum[pnt+i]+=sumsq[i];
1285 }
1286 k-=wg.fft_avg1num;
1287 }
1288 }
1289 }
1290 else
1291 {
1292 if(sw_onechan)
1293 {
1294 // Recalculate fft2_powersum_float which is used for the waterfall display.
1295 // do not worry to correct what is already on screen.
1296 // Could be done if anyone cares to.
1297 if(wg_waterf_sum_counter != 0 && wg.waterfall_avgnum !=1)
1298 {
1299 ni=fft2_na;
1300 sumsq=&fft2_powersum_float[pnt];
1301 for(i=0; i<SPUR_WIDTH; i++)sumsq[i]=0;
1302 for(j=0; j<wg_waterf_sum_counter; j++)
1303 {
1304 ni=(ni+fft2n_mask)&fft2n_mask;
1305 pwr=&fftx_pwr[ni*fftx_size+pnt];
1306 for(i=0; i<SPUR_WIDTH; i++)
1307 {
1308 sumsq[i]+=pwr[i];
1309 }
1310 }
1311 }
1312 // Recalculate the high resolution graph in case a frequency is selected.
1313 if(mix1_selfreq[0]>0)
1314 {
1315 if(hg.spek_avgnum > 2)
1316 {
1317 if(pnt >= hg_first_point && pnt+SPUR_WIDTH < hg_last_point)
1318 {
1319 ni=fft2_nb;
1320 sumsq=&hg_fft2_pwrsum[pnt-hg_first_point];
1321 for(i=0; i<SPUR_WIDTH; i++)sumsq[i]=0;
1322 while(ni != fft2_na)
1323 {
1324 pwr=&fftx_pwr[ni*fftx_size+pnt];
1325 for(i=0; i<SPUR_WIDTH; i++)sumsq[i]+=pwr[i];
1326 ni=(ni+1)&fft2n_mask;
1327 }
1328 }
1329 }
1330 }
1331 }
1332 else
1333 {
1334 // Recalculate fft2_xysum_float which is used for the waterfall display.
1335 // do not worry to correct what is already on screen.
1336 // Could be done if anyone cares to.
1337 if(wg_waterf_sum_counter != 0 && wg.waterfall_avgnum !=1)
1338 {
1339 ni=fft2_na;
1340 xysum=&fft2_xysum[pnt];
1341 for(i=0; i<SPUR_WIDTH; i++)
1342 {
1343 xysum[i].x2=0;
1344 xysum[i].y2=0;
1345 xysum[i].re_xy=0;
1346 xysum[i].im_xy=0;
1347 }
1348 for(j=0; j<wg_waterf_sum_counter; j++)
1349 {
1350 ni=(ni+fft2n_mask)&fft2n_mask;
1351 pxy=&fftx_xypower[ni*fftx_size+pnt];
1352 for(i=0; i<SPUR_WIDTH; i++)
1353 {
1354 xysum[i].x2+=pxy[i].x2;
1355 xysum[i].y2+=pxy[i].y2;
1356 xysum[i].re_xy+=pxy[i].re_xy;
1357 xysum[i].im_xy+=pxy[i].im_xy;
1358 }
1359 }
1360 }
1361 // Recalculate the high resolution graph in case a frequency is selected.
1362 if(mix1_selfreq[0]>0)
1363 {
1364 if(hg.spek_avgnum > 2)
1365 {
1366 if(pnt >= hg_first_point && pnt+SPUR_WIDTH < hg_last_point)
1367 {
1368 ni=fft2_nb;
1369 sumsq=&hg_fft2_pwrsum[2*(pnt-hg_first_point)];
1370 for(i=0; i<2*SPUR_WIDTH; i++)sumsq[i]=0;
1371 if(swmmx_fft2)
1372 {
1373 while(ni != fft2_na)
1374 {
1375 pwr=&hg_fft2_pwr[2*(ni*hg_size+pnt-hg_first_point)];
1376 zmmx=&fft2_short_int[4*(ni*fftx_size+pnt)];
1377 for(i=0; i<SPUR_WIDTH; i++)
1378 {
1379 t1=zmmx[4*i ];
1380 t2=zmmx[4*i+1];
1381 t3=zmmx[4*i+2];
1382 t4=zmmx[4*i+3];
1383 r1=pg.c1*t1+pg.c2*t3+pg.c3*t4;
1384 r2=pg.c1*t2+pg.c2*t4-pg.c3*t3;
1385 pwr[2*i ]=r1*r1+r2*r2;
1386 sumsq[2*i ]+=pwr[2*i ];
1387 r1=pg.c1*t3-pg.c2*t1+pg.c3*t2;
1388 r2=pg.c1*t4-pg.c2*t2-pg.c3*t1;
1389 pwr[2*i+1]=r1*r1+r2*r2;
1390 sumsq[2*i+1]+=pwr[2*i+1];
1391 }
1392 ni=(ni+1)&fft2n_mask;
1393 }
1394 }
1395 else
1396 {
1397 while(ni != fft2_na)
1398 {
1399 pwr=&hg_fft2_pwr[2*(ni*hg_size+pnt-hg_first_point)];
1400 z=&fftx[4*(ni*fftx_size+pnt)];
1401 for(i=0; i<SPUR_WIDTH; i++)
1402 {
1403 t1=z[4*i ];
1404 t2=z[4*i+1];
1405 t3=z[4*i+2];
1406 t4=z[4*i+3];
1407 r1=pg.c1*t1+pg.c2*t3+pg.c3*t4;
1408 r2=pg.c1*t2+pg.c2*t4-pg.c3*t3;
1409 pwr[2*i ]=r1*r1+r2*r2;
1410 sumsq[2*i ]+=pwr[2*i ];
1411 r1=pg.c1*t3-pg.c2*t1+pg.c3*t2;
1412 r2=pg.c1*t4-pg.c2*t2-pg.c3*t1;
1413 pwr[2*i+1]=r1*r1+r2*r2;
1414 sumsq[2*i+1]+=pwr[2*i+1];
1415 }
1416 ni=(ni+1)&fft2n_mask;
1417 }
1418 }
1419 }
1420 }
1421 }
1422 }
1423 }
1424 return TRUE;
1425 }
1426
spur_phase_parameters(void)1427 void spur_phase_parameters(void)
1428 {
1429 int i;
1430 int ia,na;
1431 float t1,t2,t3,r1,r2;
1432 float a1, a2, b1, b2, d1, d2;
1433 // We want to extract phase and it's derivatives from the
1434 // complex amplitude.
1435 // We can improve S/N by averaging!
1436 // The phase may rotate a few turns over the time interval
1437 // so averaging is somewhat complicated.
1438 // First get the derivative as the cross product.
1439 // Assuming the spur is present all the time we divide out
1440 // the amplitude to have the phase only as complex numbers.
1441 for(i=1; i<spur_speknum; i++)
1442 {
1443 t1=sp_sig[2*i ]*sp_sig[2*i-2]+sp_sig[2*i+1]*sp_sig[2*i-1];
1444 t2=sp_sig[2*i+1]*sp_sig[2*i-2]-sp_sig[2*i ]*sp_sig[2*i-1];
1445 r1=sqrt(t1*t1+t2*t2);
1446 if(r1>0.000000001)
1447 {
1448 sp_der[2*i-2]=t1/r1;
1449 sp_der[2*i-1]=t2/r1;
1450 }
1451 else
1452 {
1453 sp_der[2*i-2]=0;
1454 sp_der[2*i-1]=0;
1455 }
1456 }
1457 // The derivative is amplitude*frequency offset plus random noise.
1458 // The frequency offset will not vary fast with time so we can average
1459 // out the noise.
1460 complex_lowpass(sp_der, sp_tmp, sp_avgnum, sp_numsub);
1461 if(kill_all_flag)return;
1462 // Get the average phase of the second derivative,
1463 // also from the cross product.
1464 r1=0;
1465 r2=0;
1466 for(i=1+sp_avgnum/2; i<sp_numsub-sp_avgnum/2; i++)
1467 {
1468 t1=sp_tmp[2*i ]*sp_tmp[2*i-2]+sp_tmp[2*i+1]*sp_tmp[2*i-1];
1469 t2=sp_tmp[2*i+1]*sp_tmp[2*i-2]-sp_tmp[2*i ]*sp_tmp[2*i-1];
1470 t3=sqrt(t1*t1+t2*t2);
1471 if(t3 > 0.00001)
1472 {
1473 r1+=t1/t3;
1474 r2+=t2/t3;
1475 }
1476 }
1477 sp_d2=atan2(r2,r1);
1478 // If the second derivative is too big, we have an error of some kind.
1479 // Interference or a frequency/phase jump
1480 t1=spur_d2pha[spurno]+sp_d2;
1481 if( fabs(t1) > spur_max_d2 && fabs(sp_d2) > spur_max_d2)
1482 {
1483 // If the curvature we would get is too large, skip the new value
1484 // and allow the curvature to drift towards zero
1485 sp_d2=-spur_d2pha[spurno]/spur_speknum;
1486 }
1487 else
1488 {
1489 // If S/N for the spur is poor, do not allow second derivative
1490 // to change rapidly, for a weak signal we would introduce noise.
1491 t1=spur_weiold*spur_avgd2[spurno]+spur_weinew*t1;
1492 if(spur_noise[spurno]>0.000001 && fabs(spur_ampl[spurno])>0.000001)
1493 {
1494 t2=0.1*fabs(spur_ampl[spurno])/spur_noise[spurno];
1495 t2=1/(1+t2);
1496 }
1497 else
1498 {
1499 t2=1;
1500 }
1501 sp_d2=t2*(t1-spur_d2pha[spurno])+(1-t2)*sp_d2;
1502 }
1503 // Extract the phase part and skip the amplitude part of the derivative.
1504 for(i=0; i<sp_numsub; i++)
1505 {
1506 sp_pha[i]=atan2(sp_tmp[2*i+1],sp_tmp[2*i]);
1507 }
1508 remove_phasejumps(sp_pha, sp_numsub);
1509 // Now we integrate the phase derivative to get the S/N improved phase of
1510 // the spur.
1511 sp_pha[sp_numsub]=0;
1512 for(i=sp_numsub; i>0; i--)
1513 {
1514 sp_pha[i-1]=sp_pha[i]-sp_pha[i-1];
1515 }
1516 // The phase function has an unknown integration constant but
1517 // we can use it to get derivatives.
1518 // sp_avgnum/2 points at each end come from averages, avoid them, they degrade
1519 // the statistics in the derivative calculations.
1520 // We want the derivatives to be valid for the latest
1521 // data point, spur_speknum-1
1522 // It is safe to assume that d2pha is a constant, but
1523 // the fact that it is non-zero means that d1pha varies
1524 // with the transform number.
1525 // phase(n) = phase(0) - n*(d1pha+d2pha/2) + n*n*d2pha/2
1526 // where n is transform number counted backwards from ffts_na-1
1527 // The phase shift caused by the second derivative is:
1528 // dph = - n*d2err/2 + n*n*d2err/2 = 0.5*n*(n-1)*d2err
1529 // Adjust sp_pha by adding dph so the phase function will
1530 // become a straight line from which we can get the first derivative.
1531 t1=sp_d2*0.5;
1532 for(i=2; i<spur_speknum; i++)
1533 {
1534 sp_pha[sp_numsub-i]-=i*(i-1)*t1;
1535 }
1536 na=spur_speknum-sp_avgnum;
1537 if(na < 10)na=spur_speknum-sp_avgnum/2;
1538 if(na < 3) na=spur_speknum;
1539 ia=spur_speknum-na;
1540 t1=average_slope(&sp_pha[ia], na);
1541 sp_d1=t1;
1542 //if(mailbox[1]==1)fprintf( stderr,"\nAA %f",sp_d1);
1543 // Now that we have first and second derivatives we can construct
1544 // a signal that accurately follows the frequency of the spur.
1545 // Do not touch the first point. It is the integration
1546 // constant which is still unknown.
1547 // Set up the signal with amplitude 1 as a complex amplitude.
1548 // This is a local oscillator for PLL!
1549 // Twist the phase of the original complex amplitude sp_sig in
1550 // accordance with the phase derivatives we now have.
1551 // Accumulate the average phase angle.
1552 b1=cos(sp_d1);
1553 b2=sin(sp_d1);
1554 a1=b1;
1555 a2=b2;
1556 d1=cos(sp_d2);
1557 d2=sin(sp_d2);
1558 t1=0;
1559 t2=0;
1560 for(i=sp_numsub; i>=0; i--)
1561 {
1562 r1=a1*sp_sig[2*i ]+a2*sp_sig[2*i+1];
1563 r2=a1*sp_sig[2*i+1]-a2*sp_sig[2*i ];
1564 sp_tmp[2*i ]=r1;
1565 sp_tmp[2*i+1]=r2;
1566 //if(mailbox[1]==1)fprintf( dmp,"\ntmp[%d] %f %f sig %f %f",i,
1567 // sp_tmp[2*i],sp_tmp[2*i+1],sp_sig[2*i],sp_sig[2*i+1 ]);
1568
1569 t3=sqrt(r1*r1+r2*r2);
1570 if(t3>0)
1571 {
1572 t1+=r1/t3;
1573 t2+=r2/t3;
1574 r2+=t3*t3;
1575 }
1576 r1=a1*b1+a2*b2;
1577 a2=a2*b1-a1*b2;
1578 a1=r1;
1579 r1=b1*d1+b2*d2;
1580 b2=b2*d1-b1*d2;
1581 b1=r1;
1582 }
1583 sp_d0=atan2(t2,t1);
1584 // Now twist the phase for the average phase to become zero.
1585 // Fit a straight line to phase to get a correction to sp_d1
1586 // which was not calculated with optimum noise suppression before.
1587 // Under the assumption that the phase angle is small atan(x) = x
1588 // Under the same assumption the real part is the amplitude so
1589 // we can extract the average amplitude now.
1590 //if(mailbox[1]==1)fprintf( dmp,"\navg phase %f %f ",t2,t1);
1591
1592 t3=sqrt(t1*t1+t2*t2);
1593 t1/=t3;
1594 t2/=t3;
1595 a1=0;
1596 a2=0;
1597 d1=-0.5*sp_numsub;
1598 for(i=0; i<spur_speknum; i++)
1599 {
1600 r1=t1*sp_tmp[2*i ]+t2*sp_tmp[2*i+1];
1601 r2=t1*sp_tmp[2*i+1]-t2*sp_tmp[2*i ];
1602 sp_tmp[2*i ]=r1;
1603 sp_tmp[2*i+1]=r2;
1604 a1+=r1;
1605 if(r1 >0 && fabs(r2) < fabs(r1))
1606 {
1607 a2+=d1*r2/fabs(r1);
1608 }
1609 else
1610 {
1611 a2+=d1*atan2(r2,r1);
1612 }
1613 d1+=1;
1614 }
1615 a1/=spur_speknum;
1616 spur_ampl[spurno]=a1;
1617 a2/=spur_linefit;
1618 sp_d1+=a2;
1619 //if(mailbox[1]==1)fprintf( stderr,"\nBB %f %f",sp_d1,a2);
1620 // Correct the phase in accordance with the improved first
1621 // derivative (signal frequency)
1622 // The signal should have it's real part equal to the
1623 // average amplitude and it's imaginary part equal to zero.
1624 // Any deviation is noise (or a modulation on the spur)!
1625 d2=-0.5*sp_numsub*a2;
1626 b1=cos(a2);
1627 b2=-sin(a2);
1628 a1=cos(d2);
1629 a2=sin(d2);
1630 t1=0;
1631 for(i=0; i<spur_speknum; i++)
1632 {
1633 r1=a1*sp_tmp[2*i ]-a2*sp_tmp[2*i+1];
1634 r2=a1*sp_tmp[2*i+1]+a2*sp_tmp[2*i ];
1635 sp_tmp[2*i ]=r1;
1636 sp_tmp[2*i+1]=r2;
1637 t1+=r1;
1638 r1=a1*b1+a2*b2;
1639 a2=a2*b1-a1*b2;
1640 a1=r1;
1641 }
1642 t1/=spur_speknum;
1643 spur_ampl[spurno]=t1;
1644 t2=0;
1645 for(i=0; i<spur_speknum; i++)
1646 {
1647 t2+=(sp_tmp[2*i ]-t1)*(sp_tmp[2*i ]-t1)+sp_tmp[2*i+1]*sp_tmp[2*i+1];
1648 }
1649 spur_noise[spurno]=sqrt(t2/spur_speknum);
1650 }
1651
1652
1653