PageRenderTime 65ms CodeModel.GetById 27ms RepoModel.GetById 0ms app.codeStats 0ms

/fldigi-3.21.41/src/mt63/mt63base.cxx

#
C++ | 1478 lines | 1019 code | 209 blank | 250 comment | 169 complexity | a27256703fb7e66fac3dd1513956d444 MD5 | raw file
Possible License(s): GPL-3.0
  1. /*
  2. * mt63base.cxx -- MT63 transmitter and receiver in C++ for LINUX
  3. *
  4. * Copyright (C) 1999-2004 Pawel Jalocha, SP9VRC
  5. * Copyright (c) 2007-2011 Dave Freese, W1HKJ
  6. *
  7. * base class for use by fldigi
  8. * modified from original
  9. * excluded CW_ID which is a part of the base modem class for fldigi
  10. * changed all floats to double and removed all float functions/methods
  11. * changed from int* to double* for all sound card buffer transfers
  12. *
  13. * Modified base class for rx and tx to allow variable center frequency
  14. * for baseband signal
  15. *
  16. * based on mt63 code by Pawel Jalocha
  17. *
  18. * This file is part of fldigi.
  19. *
  20. * Fldigi is free software: you can redistribute it and/or modify
  21. * it under the terms of the GNU General Public License as published by
  22. * the Free Software Foundation, either version 3 of the License, or
  23. * (at your option) any later version.
  24. *
  25. * Fldigi is distributed in the hope that it will be useful,
  26. * but WITHOUT ANY WARRANTY; without even the implied warranty of
  27. * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
  28. * GNU General Public License for more details.
  29. *
  30. * You should have received a copy of the GNU General Public License
  31. * along with fldigi. If not, see <http://www.gnu.org/licenses/>.
  32. *
  33. */
  34. #include <config.h>
  35. #include <stdio.h> // only for control printf's
  36. // #include <alloc.h>
  37. #include <iostream>
  38. #include "dsp.h"
  39. #include "mt63base.h"
  40. #include "symbol.dat" // symbol shape
  41. #include "mt63intl.dat" // interleave patterns
  42. // W1HKJ
  43. // fixed filter shapes replaced by maximally flat blackman3 filters
  44. // that are generated as required as signal center frequency is changed
  45. //#include "alias_k5.dat" // anti-alias filter shapes
  46. //#include "alias_1k.dat" // for 500, 1000 and 2000 Hz modes
  47. //#include "alias_2k.dat"
  48. // ==========================================================================
  49. // MT63 transmitter code
  50. MT63tx::MT63tx()
  51. {
  52. TxVect = NULL;
  53. dspPhaseCorr = NULL;
  54. }
  55. MT63tx::~MT63tx()
  56. {
  57. free(TxVect);
  58. free(dspPhaseCorr);
  59. }
  60. void MT63tx::Free(void)
  61. {
  62. free(TxVect);
  63. TxVect = NULL;
  64. free(dspPhaseCorr);
  65. dspPhaseCorr = NULL;
  66. Encoder.Free();
  67. FFT.Free();
  68. Window.Free();
  69. Comb.Free();
  70. WindowBuff.Free();
  71. }
  72. // W1HKJ
  73. // added freq paramter to Preset
  74. int MT63tx::Preset(double freq, int BandWidth, int LongInterleave)
  75. {
  76. int i, p, step, incr, mask;
  77. // W1HKJ
  78. // values used to computer the blackman3 passband filter shape
  79. double hbw = 1.5*BandWidth / 2;
  80. double omega_low = (freq - hbw);
  81. double omega_high = (freq + hbw);
  82. if (omega_low < 100) omega_low = 100;
  83. if (omega_high > 4000) omega_high = 4000;
  84. omega_low *= (M_PI / 4000);
  85. omega_high *= (M_PI / 4000);
  86. mask = FFT.Size - 1;
  87. DataCarriers = 64;
  88. switch(BandWidth) {
  89. case 500:
  90. FirstDataCarr = (int)floor((freq - BandWidth / 2.0) * 256 / 500 + .5);
  91. AliasFilterLen = 128;
  92. DecimateRatio = 8;
  93. break;
  94. case 1000:
  95. FirstDataCarr = (int)floor((freq - BandWidth / 2.0) * 128 / 500 + 0.5);
  96. AliasFilterLen = 64;
  97. DecimateRatio = 4;
  98. break;
  99. case 2000:
  100. FirstDataCarr = (int)floor((freq - BandWidth / 2.0) * 64 / 500 + 0.5);
  101. AliasFilterLen = 64;
  102. DecimateRatio = 2;
  103. break;
  104. default:
  105. return -1;
  106. }
  107. WindowLen = SymbolLen;
  108. TxWindow = SymbolShape;
  109. TxAmpl = 4.0 / DataCarriers; // for maximum output level we can set TxAmpl=4.0/DataCarriers
  110. CarrMarkCode = 0x16918BBEL;
  111. CarrMarkAmpl = 0;
  112. if (LongInterleave) {
  113. DataInterleave = 64;
  114. InterleavePattern = LongIntlvPatt;
  115. }
  116. else {
  117. DataInterleave = 32;
  118. InterleavePattern = ShortIntlvPatt;
  119. }
  120. if (dspRedspAllocArray(&TxVect, DataCarriers))
  121. goto Error;
  122. if (dspRedspAllocArray(&dspPhaseCorr, DataCarriers))
  123. goto Error;
  124. if (WindowBuff.EnsureSpace(2 * WindowLen))
  125. goto Error;
  126. WindowBuff.Len = 2 * WindowLen;
  127. if (Encoder.Preset(DataCarriers, DataInterleave, InterleavePattern, 1))
  128. goto Error;
  129. if (FFT.Preset(WindowLen))
  130. goto Error;
  131. if (Window.Preset(WindowLen, SymbolSepar / 2, TxWindow))
  132. goto Error;
  133. // W1HKJ
  134. // Preset the combining instance, NULL pointers in lieu of fixed filter shapes
  135. // blackman3 filter provides flat passband and sufficient out-of-band rejection
  136. // to insure that all unwanted FFT components (periodic signal) are suppressed
  137. // by 70 dB or more
  138. if ( Comb.Preset( AliasFilterLen, NULL, NULL, DecimateRatio ) )
  139. goto Error;
  140. // compute new combining filter shape
  141. Comb.ComputeShape(omega_low, omega_high, dspWindowBlackman3);
  142. // Preset the initial dspPhase for each data carrier.
  143. // Here we only compute indexes to the FFT twiddle factors
  144. // so the actual vector is FFT.Twiddle[TxVect[i]]
  145. for (step = 0, incr = 1, p = 0, i = 0; i < DataCarriers; i++) {
  146. TxVect[i] = p;
  147. step += incr;
  148. p = (p + step) & mask;
  149. }
  150. // compute dspPhase correction between successive FFTs separated by SymbolSepar
  151. // Like above we compute indexes to the FFT.Twiddle[]
  152. incr = (SymbolSepar * DataCarrSepar) & mask;
  153. for (p = (SymbolSepar * FirstDataCarr) & mask, i = 0; i < DataCarriers; i++) {
  154. dspPhaseCorr[i] = p;
  155. p = (p + incr) & mask;
  156. }
  157. return 0;
  158. Error:
  159. Free();
  160. return -1;
  161. }
  162. // W1HKJ
  163. // SendTune and ProcessTxVect are both modified to allow the FirstDataCarr
  164. // to be other than WindowLen / 2 as in the original design
  165. // The peridocity of the FFT is taken advantage of by computing the positions
  166. // of the bit indices modulo FFT.size, i.e. r = FFT.BitRevIdx[c & (FFT.Size - 1)]
  167. int MT63tx::SendTune(bool twotones)
  168. {
  169. int i, c, r, mask;
  170. double Ampl;
  171. mask = FFT.Size - 1;
  172. Ampl = TxAmpl * sqrt(DataCarriers / 2);
  173. for (i = 0; i < DataCarriers; i++)
  174. TxVect[i] = (TxVect[i] + dspPhaseCorr[i]) & mask;
  175. for (i = 0; i < 2 * WindowLen; i++)
  176. WindowBuff.Data[i].im = WindowBuff.Data[i].re = 0.0;
  177. // W1HKJ
  178. // first tone at the lowest most MT63 carrier
  179. i = 0;
  180. c = FirstDataCarr;
  181. r = FFT.BitRevIdx[c & mask];
  182. WindowBuff.Data[r].re = Ampl * FFT.Twiddle[TxVect[i]].re;
  183. WindowBuff.Data[r].im = (-Ampl) * FFT.Twiddle[TxVect[i]].im;
  184. // W1HKJ
  185. // 2nd tone at the highest most MT63 carrier + 1
  186. // MT63 is specified as 500, 1000 and 2000 Hz wide signal format, but in
  187. // fact are narrower by one carrier spacing, i.e. 0 to N-1 carriers where
  188. // N = 64
  189. if (twotones) {
  190. i = DataCarriers - 1;
  191. c = (FirstDataCarr + i * DataCarrSepar);
  192. r = WindowLen + FFT.BitRevIdx[c & mask];
  193. WindowBuff.Data[r].re = Ampl * FFT.Twiddle[TxVect[i]].re;
  194. WindowBuff.Data[r].im = (-Ampl) * FFT.Twiddle[TxVect[i]].im;
  195. }
  196. // inverse FFT: WindowBuff is already scrambled
  197. FFT.CoreProc(WindowBuff.Data);
  198. FFT.CoreProc(WindowBuff.Data + WindowLen);
  199. // negate the imaginary part for the IFFT
  200. for (i = 0; i < 2 * WindowLen; i++)
  201. WindowBuff.Data[i].im *= (-1.0);
  202. // process the FFT values to produce a complex time domain vector
  203. Window.Process(&WindowBuff);
  204. // W1HKJ
  205. // convert the complex time domain vector to a real time domain signal
  206. // suitably filtered by the anti-alias filter used in the combiner
  207. Comb.Process(&Window.Output);
  208. return 0;
  209. }
  210. int MT63tx::SendChar(char ch)
  211. {
  212. int i,mask,flip;
  213. Encoder.Process(ch); // encode and interleave the character
  214. // print the character and the DataBlock being sent
  215. // printf("0x%02x [%c] => ", ch, ch>=' ' ? ch : '.');
  216. // for (i=0; i<DataCarriers; i++) printf("%c",'0'+Encoder.Output[i]);
  217. // printf("\n");
  218. // here we encode the Encoder.Output into dspPhase flips
  219. mask = FFT.Size - 1;
  220. flip = FFT.Size / 2;
  221. for (i = 0; i < DataCarriers; i++) {
  222. // data bit = 1 => only dspPhase correction
  223. if (Encoder.Output[i])
  224. TxVect[i] = (TxVect[i] + dspPhaseCorr[i]) & mask;
  225. // data bit = 0 => dspPhase flip + dspPhase correction
  226. else
  227. TxVect[i] = (TxVect[i] + dspPhaseCorr[i] + flip) & mask;
  228. }
  229. ProcessTxVect();
  230. return 0;
  231. }
  232. int MT63tx::SendJam(void)
  233. {
  234. int i,mask,left,right;
  235. int j = 0;
  236. mask = FFT.Size-1;
  237. left = FFT.Size / 4;
  238. right = 3 * (FFT.Size / 4);
  239. for (i = 0; i < DataCarriers; i++) {
  240. j = i & mask;
  241. if (rand() & 0x100) // turn left 90 degrees
  242. TxVect[j] = (TxVect[j] + dspPhaseCorr[j] + left) & mask;
  243. else // turn right 90 degrees
  244. TxVect[j] = (TxVect[j] + dspPhaseCorr[j] + right) & mask;
  245. }
  246. ProcessTxVect();
  247. return 0;
  248. }
  249. // W1HKJ
  250. // principal change from original is modulo arithmetic used to creat
  251. // WindowBuff.Data vectors
  252. int MT63tx::ProcessTxVect(void)
  253. {
  254. int i, c, r, mask;
  255. mask = FFT.Size - 1;
  256. for (i = 0; i < 2 * WindowLen; i++)
  257. WindowBuff.Data[i].im = WindowBuff.Data[i].re = 0.0;
  258. for ( i = 0, c = FirstDataCarr; i < DataCarriers; i++, c += DataCarrSepar) {
  259. r = FFT.BitRevIdx[c & mask] + WindowLen * (i & 1);
  260. WindowBuff.Data[r].re = TxAmpl*FFT.Twiddle[TxVect[i]].re;
  261. WindowBuff.Data[r].im = (-TxAmpl)*FFT.Twiddle[TxVect[i]].im;
  262. }
  263. FFT.CoreProc(WindowBuff.Data);
  264. FFT.CoreProc(WindowBuff.Data + WindowLen);
  265. // negate the imaginary part for the IFFT
  266. for (i = 0; i < 2 * WindowLen; i++)
  267. WindowBuff.Data[i].im *= (-1.0);
  268. Window.Process(&WindowBuff);
  269. // W1HKJ
  270. // audio output to be sent out is in Comb.Output
  271. Comb.Process(&Window.Output);
  272. return 0;
  273. }
  274. int MT63tx::SendSilence(void)
  275. {
  276. Window.ProcessSilence(2);
  277. Comb.Process(&Window.Output);
  278. return 0;
  279. }
  280. // ==========================================================================
  281. // Character encoder and block interleave for the MT63 modem
  282. MT63encoder::MT63encoder()
  283. {
  284. IntlvPipe = NULL;
  285. WalshBuff = NULL;
  286. Output = NULL;
  287. IntlvPatt=NULL;
  288. }
  289. MT63encoder::~MT63encoder()
  290. {
  291. free(IntlvPipe);
  292. free(WalshBuff);
  293. free(Output);
  294. free(IntlvPatt);
  295. }
  296. void MT63encoder::Free()
  297. {
  298. free(IntlvPipe);
  299. free(WalshBuff);
  300. free(Output);
  301. free(IntlvPatt);
  302. IntlvPipe = NULL;
  303. WalshBuff = NULL;
  304. Output = NULL;
  305. IntlvPatt = NULL;
  306. }
  307. int MT63encoder::Preset(int Carriers, int Intlv, int *Pattern, int PreFill)
  308. {
  309. int i, p;
  310. if (!dspPowerOf2(Carriers)) goto Error;
  311. DataCarriers = Carriers;
  312. IntlvLen = Intlv;
  313. IntlvSize = IntlvLen * DataCarriers;
  314. if (IntlvLen) {
  315. if (dspRedspAllocArray(&IntlvPipe, IntlvSize)) goto Error;
  316. if (PreFill)
  317. for (i = 0; i < IntlvSize; i++)
  318. IntlvPipe[i] = rand() & 1;
  319. else
  320. dspClearArray(IntlvPipe,IntlvSize);
  321. if (dspRedspAllocArray(&IntlvPatt, DataCarriers)) goto Error;
  322. IntlvPtr = 0;
  323. }
  324. if (dspRedspAllocArray(&WalshBuff, DataCarriers)) goto Error;
  325. if (dspRedspAllocArray(&Output, DataCarriers)) goto Error;
  326. CodeMask = 2 * DataCarriers - 1;
  327. for (p = 0, i = 0; i < DataCarriers; i++) {
  328. IntlvPatt[i] = p * DataCarriers;
  329. p += Pattern[i];
  330. if (p >= IntlvLen) p -= IntlvLen;
  331. }
  332. return 0;
  333. Error:
  334. Free();
  335. return -1;
  336. }
  337. int MT63encoder::Process(char code) // encode an ASCII character "code"
  338. {
  339. int i, k;
  340. code &= CodeMask;
  341. for (i = 0; i < DataCarriers; i++)
  342. WalshBuff[i] = 0;
  343. if (code < DataCarriers)
  344. WalshBuff[(int)code] = 1.0;
  345. else WalshBuff[code-DataCarriers] = (-1.0);
  346. dspWalshInvTrans(WalshBuff, DataCarriers);
  347. if (IntlvLen) {
  348. for (i = 0; i < DataCarriers; i++)
  349. IntlvPipe[IntlvPtr + i] = (WalshBuff[i] < 0.0);
  350. for (i = 0; i < DataCarriers; i++) {
  351. k = IntlvPtr + IntlvPatt[i];
  352. if (k >= IntlvSize)
  353. k -= IntlvSize;
  354. Output[i] = IntlvPipe[k+i];
  355. }
  356. IntlvPtr += DataCarriers;
  357. if (IntlvPtr >= IntlvSize)
  358. IntlvPtr -= IntlvSize;
  359. } else
  360. for (i = 0; i < DataCarriers; i++)
  361. Output[i] = (WalshBuff[i] < 0.0);
  362. return 0;
  363. }
  364. // After encoding the "Output" array contains the bits to be transmitted
  365. // ==========================================================================
  366. // MT63 decoder and deinterleaver
  367. MT63decoder::MT63decoder()
  368. {
  369. IntlvPipe = NULL;
  370. IntlvPatt = NULL;
  371. WalshBuff = NULL;
  372. DecodeSnrMid = NULL;
  373. DecodeSnrOut = NULL;
  374. DecodePipe = NULL;
  375. }
  376. MT63decoder::~MT63decoder()
  377. {
  378. free(IntlvPipe);
  379. free(IntlvPatt);
  380. free(WalshBuff);
  381. free(DecodeSnrMid);
  382. free(DecodeSnrOut);
  383. free(DecodePipe);
  384. }
  385. void MT63decoder::Free()
  386. {
  387. free(IntlvPipe);
  388. IntlvPipe = NULL;
  389. free(IntlvPatt);
  390. IntlvPatt = NULL;
  391. free(WalshBuff);
  392. WalshBuff = NULL;
  393. free(DecodeSnrMid);
  394. free(DecodeSnrOut);
  395. DecodeSnrMid = NULL;
  396. DecodeSnrOut = NULL;
  397. free(DecodePipe);
  398. DecodePipe = NULL;
  399. }
  400. int MT63decoder::Preset(int Carriers, int Intlv, int *Pattern, int Margin, int Integ)
  401. {
  402. int i,p;
  403. if (!dspPowerOf2(Carriers)) goto Error;
  404. DataCarriers = Carriers;
  405. ScanLen = 2 * Margin + 1;
  406. ScanSize = DataCarriers + 2 * Margin;
  407. dspLowPass2Coeff(Integ,W1,W2,W5);
  408. DecodeLen = Integ / 2;
  409. DecodeSize = DecodeLen * ScanLen;
  410. if (dspRedspAllocArray(&DecodePipe, DecodeSize)) goto Error;
  411. dspClearArray(DecodePipe, DecodeSize);
  412. DecodePtr = 0;
  413. IntlvLen = Intlv; // printf("%d:",IntlvLen);
  414. if (dspRedspAllocArray(&IntlvPatt, DataCarriers)) goto Error;
  415. for (p = 0, i = 0; i < DataCarriers; i++) {
  416. IntlvPatt[i] = p * ScanSize; // printf(" %2d",p);
  417. p += Pattern[i];
  418. if (p >= IntlvLen) p -= IntlvLen;
  419. }
  420. // printf("\n");
  421. IntlvSize = (IntlvLen + 1) * ScanSize;
  422. if (dspRedspAllocArray(&IntlvPipe, IntlvSize)) goto Error;
  423. dspClearArray(IntlvPipe, IntlvSize);
  424. IntlvPtr = 0;
  425. if (dspRedspAllocArray(&WalshBuff, DataCarriers)) goto Error;
  426. if (dspRedspAllocArray(&DecodeSnrMid, ScanLen)) goto Error;
  427. if (dspRedspAllocArray(&DecodeSnrOut, ScanLen)) goto Error;
  428. dspClearArray(DecodeSnrMid, ScanLen);
  429. dspClearArray(DecodeSnrOut, ScanLen);
  430. SignalToNoise = 0.0;
  431. CarrOfs = 0;
  432. return 0;
  433. Error:
  434. Free();
  435. return -1;
  436. }
  437. int MT63decoder::Process(double *data)
  438. {
  439. int s, i, k;
  440. double Min, Max, Sig, Noise, SNR;
  441. int MinPos,MaxPos,code;
  442. dspCopyArray(IntlvPipe + IntlvPtr, data, ScanSize);
  443. // printf("Decoder [%d/%d/%d]: \n",IntlvPtr,IntlvSize,ScanSize);
  444. for (s = 0; s < ScanLen; s++) {
  445. // printf(" %2d:",s);
  446. for (i = 0; i < DataCarriers; i++) {
  447. k = IntlvPtr - ScanSize - IntlvPatt[i];
  448. if (k < 0) k += IntlvSize;
  449. if ((s & 1) && (i & 1)) {
  450. k += ScanSize;
  451. if (k >= IntlvSize) k-=IntlvSize;
  452. }
  453. WalshBuff[i] = IntlvPipe[k + s + i];
  454. // printf(" %4d",k/ScanSize);
  455. }
  456. // printf("\n");
  457. dspWalshTrans(WalshBuff, DataCarriers);
  458. Min = dspFindMin(WalshBuff, DataCarriers, MinPos);
  459. Max = dspFindMax(WalshBuff, DataCarriers, MaxPos);
  460. if (fabs(Max) > fabs(Min)) {
  461. code = MaxPos + DataCarriers;
  462. Sig = fabs(Max);
  463. WalshBuff[MaxPos] = 0.0;
  464. } else {
  465. code = MinPos;
  466. Sig = fabs(Min);
  467. WalshBuff[MinPos] = 0.0;
  468. }
  469. Noise = dspRMS(WalshBuff, DataCarriers);
  470. if (Noise > 0.0)
  471. SNR = Sig/Noise;
  472. else SNR = 0.0;
  473. dspLowPass2(SNR, DecodeSnrMid[s], DecodeSnrOut[s], W1, W2, W5);
  474. // printf("%2d: %02x => %c, %5.2f/%5.2f=>%5.2f <%5.2f>\n",
  475. // s,code,code<' ' ? '.' : (char)code,
  476. // Sig,Noise,SNR,DecodeSnrOut[s]);
  477. DecodePipe[DecodePtr+s]=code;
  478. }
  479. IntlvPtr += ScanSize;
  480. if (IntlvPtr >= IntlvSize) IntlvPtr = 0;
  481. DecodePtr += ScanLen;
  482. if (DecodePtr >= DecodeSize) DecodePtr = 0;
  483. Max = dspFindMax(DecodeSnrOut, ScanLen, MaxPos);
  484. Output = DecodePipe[DecodePtr + MaxPos];
  485. SignalToNoise = Max;
  486. CarrOfs = MaxPos - (ScanLen - 1) / 2;
  487. /*
  488. code=Output;
  489. if ((code>=' ')||(code=='\n')||(code=='\r')) printf("%c",code);
  490. else if (code!='\0') printf("<%02X>",code);
  491. */
  492. return 0;
  493. }
  494. // ==========================================================================
  495. // MT63 receiver code
  496. MT63rx::MT63rx()
  497. {
  498. int s;
  499. FFTbuff = NULL;
  500. FFTbuff2 = NULL;
  501. for (s = 0; s < 4; s++)
  502. SyncPipe[s] = NULL;
  503. SyncPhCorr = NULL;
  504. for (s = 0; s < 4; s++) {
  505. CorrelMid[s] = NULL;
  506. CorrelOut[s] = NULL;
  507. }
  508. dspPowerMid = NULL;
  509. dspPowerOut = NULL;
  510. for (s = 0; s < 4; s++)
  511. CorrelNorm[s] = NULL;
  512. for (s = 0; s < 4; s++)
  513. CorrelAver[s] = NULL;
  514. SymbFit = NULL;
  515. SymbPipe = NULL;
  516. FreqPipe = NULL;
  517. RefDataSlice = NULL;
  518. DataPipeLen = 0;
  519. DataPipe = NULL;
  520. DataPwrMid = NULL;
  521. DataPwrOut = NULL;
  522. DataSqrMid = NULL;
  523. DataSqrOut = NULL;
  524. DataVect = NULL;
  525. DatadspPhase = NULL;
  526. DatadspPhase2 = NULL;
  527. SpectradspPower = NULL;
  528. }
  529. MT63rx::~MT63rx()
  530. {
  531. int s;
  532. free(FFTbuff);
  533. free(FFTbuff2);
  534. for (s = 0; s < 4; s++)
  535. free(SyncPipe[s]);
  536. free(SyncPhCorr);
  537. for (s = 0; s < 4; s++) {
  538. free(CorrelMid[s]);
  539. free(CorrelOut[s]);
  540. }
  541. free(dspPowerMid);
  542. free(dspPowerOut);
  543. for (s = 0; s < 4; s++)
  544. free(CorrelNorm[s]);
  545. for (s = 0; s < 4; s++)
  546. free(CorrelAver[s]);
  547. free(SymbFit);
  548. free(SymbPipe);
  549. free(FreqPipe);
  550. free(RefDataSlice);
  551. dspFreeArray2D(DataPipe, DataPipeLen);
  552. // for (s=0; s<DataPipeLen; s++) free(DataPipe[s]); free(DataPipe);
  553. free(DataPwrMid);
  554. free(DataPwrOut);
  555. free(DataSqrMid);
  556. free(DataSqrOut);
  557. free(DataVect);
  558. free(DatadspPhase);
  559. free(DatadspPhase2);
  560. free(SpectradspPower);
  561. }
  562. void MT63rx::Free(void)
  563. {
  564. int s;
  565. FFT.Free();
  566. InpSplit.Free();
  567. TestOfs.Free();
  568. ProcLine.Free();
  569. free(FFTbuff);
  570. FFTbuff = NULL;
  571. free(FFTbuff2);
  572. FFTbuff2 = NULL;
  573. for (s = 0; s < 4; s++) {
  574. free(SyncPipe[s]);
  575. SyncPipe[s] = NULL;
  576. }
  577. free(SyncPhCorr);
  578. SyncPhCorr = NULL;
  579. for (s = 0; s < 4; s++) {
  580. free(CorrelMid[s]);
  581. CorrelMid[s] = NULL;
  582. free(CorrelOut[s]);
  583. CorrelOut[s] = NULL;
  584. }
  585. free(dspPowerMid);
  586. dspPowerMid = NULL;
  587. free(dspPowerOut);
  588. dspPowerOut = NULL;
  589. for (s = 0; s < 4; s++) {
  590. free(CorrelNorm[s]);
  591. CorrelNorm[s] = NULL;
  592. }
  593. for (s = 0; s < 4; s++) {
  594. free(CorrelAver[s]);
  595. CorrelAver[s] = NULL;
  596. }
  597. free(SymbFit);
  598. SymbFit = NULL;
  599. free(SymbPipe);
  600. SymbPipe = NULL;
  601. free(FreqPipe);
  602. FreqPipe = NULL;
  603. free(RefDataSlice);
  604. RefDataSlice = NULL;
  605. dspFreeArray2D(DataPipe, DataPipeLen);
  606. // for (s=0; s<DataPipeLen; s++) free(DataPipe[s]); free(DataPipe);
  607. DataPipeLen = 0;
  608. DataPipe = NULL;
  609. free(DataPwrMid);
  610. free(DataPwrOut);
  611. DataPwrMid = NULL;
  612. DataPwrOut = NULL;
  613. free(DataSqrMid);
  614. free(DataSqrOut);
  615. DataSqrMid = NULL;
  616. DataSqrOut = NULL;
  617. free(DataVect);
  618. DataVect = NULL;
  619. free(DatadspPhase);
  620. DatadspPhase = NULL;
  621. free(DatadspPhase2);
  622. DatadspPhase2 = NULL;
  623. Decoder.Free();
  624. free(SpectradspPower);
  625. SpectradspPower = NULL;
  626. }
  627. // added freq parameter to Preset
  628. int MT63rx::Preset(double freq, int BandWidth, int LongInterleave, int Integ,
  629. void (*Display)(double *Spectra, int Len))
  630. {
  631. int err,s,i,c;
  632. // W1HKJ
  633. // variables used for generating the anti-alias filter
  634. double hbw = 1.5*BandWidth / 2;
  635. double omega_low = (freq - hbw);
  636. double omega_high = (freq + hbw);
  637. if (omega_low < 100) omega_low = 100;
  638. if (omega_high > 4000) omega_high = 4000;
  639. omega_low *= (M_PI / 4000);
  640. omega_high *= (M_PI/ 4000);
  641. switch(BandWidth) {
  642. case 500:
  643. FirstDataCarr = (int)floor((freq - BandWidth / 2.0) * 256 / 500 + 0.5);
  644. AliasFilterLen = 128;
  645. DecimateRatio = 8;
  646. break;
  647. case 1000:
  648. FirstDataCarr = (int)floor((freq - BandWidth / 2.0) * 128 / 500 + 0.5);
  649. AliasFilterLen = 64;
  650. DecimateRatio = 4;
  651. break;
  652. case 2000:
  653. FirstDataCarr = (int)floor((freq - BandWidth / 2.0) * 64 / 500 + 0.5);
  654. AliasFilterLen = 64;
  655. DecimateRatio = 2;
  656. break;
  657. default:
  658. return -1;
  659. }
  660. DataCarriers = 64; // 64 carriers
  661. WindowLen = SymbolLen; // the symbol length
  662. RxWindow = SymbolShape; // the symbol shape
  663. // RxWindow, WindowLen, SymbolSepar, DataCarrSepar are tuned one for another
  664. // to minimize inter-symbol interference (ISI) and one should not change
  665. // them independently or ISI will increase.
  666. CarrMarkCode = 0x16918BBEL;
  667. IntegLen = Integ; // sync. integration period
  668. SymbolDiv = 4; // don't change this
  669. ScanMargin = 8; // we look 8 data carriers up and down
  670. SyncStep = SymbolSepar/SymbolDiv;
  671. ProcdspDelay = IntegLen * SymbolSepar;
  672. TrackPipeLen = IntegLen;
  673. if (LongInterleave) {
  674. DataInterleave = 64;
  675. InterleavePattern = LongIntlvPatt;
  676. } else {
  677. DataInterleave = 32;
  678. InterleavePattern = ShortIntlvPatt;
  679. }
  680. DataScanMargin = 8;
  681. err = FFT.Preset(WindowLen);
  682. if (err) goto Error;
  683. if (dspRedspAllocArray(&FFTbuff, WindowLen)) goto Error;
  684. if (dspRedspAllocArray(&FFTbuff2, WindowLen)) goto Error;
  685. WindowLenMask = WindowLen - 1;
  686. // W1HKJ
  687. // InpSplit is the anti-aliasing filter that converts a real time domain
  688. // signal into a complex time domain signal with pre-filtering.
  689. // the black3man3 filter provides very sharp skirts with a flat
  690. // passband.
  691. err = InpSplit.Preset(AliasFilterLen, NULL, NULL, DecimateRatio);
  692. if (err) goto Error;
  693. err = InpSplit.ComputeShape(omega_low, omega_high, dspWindowBlackman3);
  694. if (err) goto Error;
  695. err = TestOfs.Preset(-0.25 * (2.0 * M_PI / WindowLen)); // for decoder tests only
  696. if (err) goto Error;
  697. err = ProcLine.Preset(ProcdspDelay + WindowLen + SymbolSepar);
  698. if (err) goto Error;
  699. SyncProcPtr = 0;
  700. ScanFirst = FirstDataCarr - ScanMargin * DataCarrSepar; // first FFT bin to scan
  701. if (ScanFirst < 0) ScanFirst += WindowLen;
  702. ScanLen = (DataCarriers + 2 * ScanMargin) * DataCarrSepar; // number of FFT bins to scan
  703. for (s = 0; s < SymbolDiv; s++) {
  704. if (dspRedspAllocArray(&SyncPipe[s], ScanLen)) goto Error;
  705. dspClearArray(SyncPipe[s], ScanLen);
  706. }
  707. SyncPtr = 0;
  708. if (dspRedspAllocArray(&SyncPhCorr, ScanLen)) goto Error;
  709. for (c = (ScanFirst * SymbolSepar) & WindowLenMask, i = 0; i < ScanLen; i++) {
  710. SyncPhCorr[i].re = FFT.Twiddle[c].re * FFT.Twiddle[c].re -
  711. FFT.Twiddle[c].im * FFT.Twiddle[c].im;
  712. SyncPhCorr[i].im = 2 * FFT.Twiddle[c].re * FFT.Twiddle[c].im;
  713. c = (c + SymbolSepar) & WindowLenMask;
  714. }
  715. for (s = 0; s < SymbolDiv; s++) {
  716. if (dspRedspAllocArray(&CorrelMid[s], ScanLen)) goto Error;
  717. dspClearArray(CorrelMid[s], ScanLen);
  718. if (dspRedspAllocArray(&CorrelOut[s], ScanLen)) goto Error;
  719. dspClearArray(CorrelOut[s], ScanLen);
  720. }
  721. dspLowPass2Coeff(IntegLen, W1, W2, W5);
  722. if (dspRedspAllocArray(&dspPowerMid, ScanLen)) goto Error;
  723. dspClearArray(dspPowerMid, ScanLen);
  724. if (dspRedspAllocArray(&dspPowerOut, ScanLen)) goto Error;
  725. dspClearArray(dspPowerOut, ScanLen);
  726. dspLowPass2Coeff(IntegLen * SymbolDiv, W1p, W2p, W5p);
  727. for (s = 0; s < SymbolDiv; s++) {
  728. if (dspRedspAllocArray(&CorrelNorm[s], ScanLen)) goto Error;
  729. }
  730. FitLen = 2 * ScanMargin * DataCarrSepar;
  731. for (s = 0; s < SymbolDiv; s++) {
  732. if (dspRedspAllocArray(&CorrelAver[s], FitLen)) goto Error;
  733. }
  734. if (dspRedspAllocArray(&SymbFit, FitLen)) goto Error;
  735. if (dspRedspAllocArray(&SymbPipe, TrackPipeLen)) goto Error;
  736. dspClearArray(SymbPipe, TrackPipeLen);
  737. if (dspRedspAllocArray(&FreqPipe, TrackPipeLen)) goto Error;
  738. dspClearArray(FreqPipe, TrackPipeLen);
  739. TrackPipePtr = 0;
  740. SymbFitPos = ScanMargin * DataCarrSepar;
  741. SyncLocked = 0;
  742. SyncSymbConf = 0.0;
  743. SyncFreqOfs = 0.0;
  744. SyncFreqDev = 0.0;
  745. SymbPtr = 0;
  746. SyncSymbShift = 0.0;
  747. SyncHoldThres = 1.5 * sqrt(1.0 / (IntegLen * DataCarriers));
  748. SyncLockThres = 1.5 * SyncHoldThres;
  749. DataProcPtr = (-ProcdspDelay);
  750. DataScanLen = DataCarriers + 2 * DataScanMargin;
  751. DataScanFirst = FirstDataCarr - DataScanMargin * DataCarrSepar;
  752. if (dspRedspAllocArray(&RefDataSlice, DataScanLen)) goto Error;
  753. dspClearArray(RefDataSlice, DataScanLen);
  754. dspFreeArray2D(DataPipe, DataPipeLen);
  755. DataPipeLen = IntegLen / 2;
  756. dspLowPass2Coeff(IntegLen, dW1, dW2, dW5);
  757. if (dspAllocArray2D(&DataPipe, DataPipeLen, DataScanLen)) {
  758. DataPipeLen = 0;
  759. goto Error;
  760. }
  761. dspClearArray2D(DataPipe, DataPipeLen, DataScanLen);
  762. DataPipePtr = 0;
  763. if (dspRedspAllocArray(&DataPwrMid, DataScanLen)) goto Error;
  764. dspClearArray(DataPwrMid, DataScanLen);
  765. if (dspRedspAllocArray(&DataPwrOut, DataScanLen)) goto Error;
  766. dspClearArray(DataPwrOut, DataScanLen);
  767. if (dspRedspAllocArray(&DataSqrMid, DataScanLen)) goto Error;
  768. dspClearArray(DataSqrMid, DataScanLen);
  769. if (dspRedspAllocArray(&DataSqrOut, DataScanLen)) goto Error;
  770. dspClearArray(DataSqrOut, DataScanLen);
  771. if (dspRedspAllocArray(&DataVect, DataScanLen)) goto Error;
  772. if (dspRedspAllocArray(&DatadspPhase, DataScanLen)) goto Error;
  773. if (dspRedspAllocArray(&DatadspPhase2, DataScanLen)) goto Error;
  774. err = Decoder.Preset(DataCarriers, DataInterleave,
  775. InterleavePattern, DataScanMargin, IntegLen);
  776. if (err) goto Error;
  777. SpectraDisplay = Display;
  778. if (SpectraDisplay) {
  779. if (dspRedspAllocArray(&SpectradspPower, WindowLen))
  780. goto Error;
  781. }
  782. return 0;
  783. Error:
  784. Free();
  785. return -1;
  786. }
  787. int MT63rx::Process(double_buff *Input)
  788. {
  789. int s1,s2;
  790. // TestOfs.Omega+=(-0.005*(2.0*M_PI/512)); // simulate frequency drift
  791. Output.Len = 0;
  792. // W1HKJ
  793. // convert the real data input into a complex time domain signal,
  794. // anti-aliased using the blackman3 filter
  795. // subsequent rx signal processing takes advantage of the periodic nature
  796. // of the resultant FFT of the anti-aliased input signal. Actual decoding
  797. // is at baseband.
  798. InpSplit.Process(Input);
  799. ProcLine.Process(&InpSplit.Output);
  800. // TestOfs.Process(&InpSplit.Output);
  801. // ProcLine.Process(&TestOfs.Output);
  802. // printf("New input, Len=%d/%d\n",Input->Len,ProcLine.InpLen);
  803. while((SyncProcPtr+WindowLen) < ProcLine.InpLen) {
  804. SyncProcess(ProcLine.InpPtr + SyncProcPtr);
  805. // printf("SyncSymbConf=%5.2f, SyncLock=%d, SyncProcPtr=%d, SyncPtr=%d, SymbPtr=%d, SyncSymbShift=%5.1f, SyncFreqOfs=%5.2f =>",
  806. // SyncSymbConf,SyncLocked,SyncProcPtr,SyncPtr,SymbPtr,SyncSymbShift,SyncFreqOfs);
  807. if (SyncPtr == SymbPtr) {
  808. s1 = SyncProcPtr - ProcdspDelay +
  809. ((int)SyncSymbShift - SymbPtr * SyncStep);
  810. s2 = s1 + SymbolSepar / 2;
  811. // printf(" Sample at %d,%d (SyncProcPtr-%d), time diff.=%d\n",s1,s2,SyncProcPtr-s1,s1-DataProcPtr);
  812. DataProcess(ProcLine.InpPtr + s1, ProcLine.InpPtr + s2,
  813. SyncFreqOfs, s1 - DataProcPtr);
  814. DataProcPtr = s1;
  815. }
  816. // printf("\n");
  817. SyncProcPtr += SyncStep;
  818. }
  819. SyncProcPtr -= ProcLine.InpLen;
  820. DataProcPtr -= ProcLine.InpLen;
  821. return 0;
  822. }
  823. void MT63rx::DoCorrelSum(dspCmpx *Correl1, dspCmpx *Correl2, dspCmpx *Aver)
  824. {
  825. dspCmpx sx;
  826. int i, s, d;
  827. s = 2 * DataCarrSepar;
  828. d = DataCarriers * DataCarrSepar;
  829. sx.re = sx.im = 0.0;
  830. for (i = 0; i < d; i+=s) {
  831. sx.re += Correl1[i].re;
  832. sx.im += Correl1[i].im;
  833. sx.re += Correl2[i].re;
  834. sx.im += Correl2[i].im;
  835. }
  836. Aver[0].re = sx.re / DataCarriers;
  837. Aver[0].im = sx.im / DataCarriers;
  838. for (i = 0; i < (FitLen-s); ) {
  839. sx.re -= Correl1[i].re;
  840. sx.im -= Correl1[i].im;
  841. sx.re -= Correl2[i].re;
  842. sx.im -= Correl2[i].im;
  843. sx.re += Correl1[i+d].re;
  844. sx.im -= Correl1[i+d].im;
  845. sx.re += Correl2[i+d].re;
  846. sx.im -= Correl2[i+d].im;
  847. i += s;
  848. Aver[i].re = sx.re / DataCarriers;
  849. Aver[i].im = sx.im / DataCarriers; }
  850. }
  851. void MT63rx::SyncProcess(dspCmpx *Slice)
  852. {
  853. int i, j, k, r, s, s2;
  854. double pI, pQ;
  855. dspCmpx Correl;
  856. dspCmpx *PrevSlice;
  857. double I, Q;
  858. double dI, dQ;
  859. double P,A;
  860. double w0, w1;
  861. double Fl, F0, Fu;
  862. dspCmpx SymbTime;
  863. double SymbConf, SymbShift, FreqOfs;
  864. double dspRMS;
  865. int Loops, Incl;
  866. SyncPtr = (SyncPtr + 1) & (SymbolDiv - 1); // increment the correlators pointer
  867. for (i = 0; i < WindowLen; i++) {
  868. r = FFT.BitRevIdx[i];
  869. FFTbuff[r].re = Slice[i].re * RxWindow[i];
  870. FFTbuff[r].im = Slice[i].im * RxWindow[i];
  871. }
  872. FFT.CoreProc(FFTbuff);
  873. if (SpectraDisplay) {
  874. for ( i = 0,
  875. j = FirstDataCarr + (DataCarriers / 2) * DataCarrSepar -
  876. WindowLen / 2;
  877. (i < WindowLen) && ( j <WindowLen); i++,j++)
  878. SpectradspPower[i] = dspPower(FFTbuff[j]);
  879. for (j = 0; (i < WindowLen) && (j < WindowLen); i++,j++)
  880. SpectradspPower[i] = dspPower(FFTbuff[j]);
  881. (*SpectraDisplay)(SpectradspPower, WindowLen);
  882. }
  883. // EnvSync.Process(FFTbuff); // experimental synchronizer
  884. PrevSlice = SyncPipe[SyncPtr];
  885. for (i = 0; i < ScanLen; i++) {
  886. k = (ScanFirst+i) & WindowLenMask;
  887. I = FFTbuff[k].re;
  888. Q = FFTbuff[k].im;
  889. P = I * I + Q * Q;
  890. A = sqrt(P);
  891. if (P > 0.0) {
  892. dI = (I * I - Q * Q) / A;
  893. dQ = (2 * I * Q) / A;
  894. } else {
  895. dI = dQ = 0.0;
  896. }
  897. dspLowPass2(P, dspPowerMid[i], dspPowerOut[i], W1p, W2p, W5p);
  898. pI = PrevSlice[i].re * SyncPhCorr[i].re -
  899. PrevSlice[i].im * SyncPhCorr[i].im;
  900. pQ = PrevSlice[i].re * SyncPhCorr[i].im +
  901. PrevSlice[i].im * SyncPhCorr[i].re;
  902. Correl.re = dQ * pQ + dI * pI;
  903. Correl.im = dQ * pI - dI * pQ;
  904. dspLowPass2(&Correl, CorrelMid[SyncPtr] + i,
  905. CorrelOut[SyncPtr] + i, W1, W2, W5);
  906. PrevSlice[i].re = dI;
  907. PrevSlice[i].im = dQ;
  908. }
  909. if (SyncPtr == (SymbPtr^2)) {
  910. for (s = 0; s < SymbolDiv; s++) { // normalize the correlations
  911. for (i = 0; i < ScanLen; i++) {
  912. if (dspPowerOut[i] > 0.0) {
  913. CorrelNorm[s][i].re = CorrelOut[s][i].re / dspPowerOut[i];
  914. CorrelNorm[s][i].im = CorrelOut[s][i].im / dspPowerOut[i];
  915. } else
  916. CorrelNorm[s][i].im = CorrelNorm[s][i].re = 0.0;
  917. }
  918. }
  919. /*
  920. // another way to normalize - a better one ?
  921. for (i=0; i<ScanLen; i++)
  922. { for (P=0.0,s=0; s<SymbolDiv; s++)
  923. P+=dspPower(CorrelOut[s][i]);
  924. if (P>0.0)
  925. { for (s=0; s<SymbolDiv; s++)
  926. { CorrelNorm[s][i].re=CorrelOut[s][i].re/P;
  927. CorrelNorm[s][i].im=CorrelOut[s][i].im/P; }
  928. } else
  929. { for (s=0; s<SymbolDiv; s++)
  930. CorrelNorm[s][i].re=CorrelNorm[s][i].im=0.0; }
  931. }
  932. */
  933. // make a sum for each possible carrier positions
  934. for (s = 0; s < SymbolDiv; s++) {
  935. s2 = (s + SymbolDiv / 2) & (SymbolDiv - 1);
  936. for (k = 0; k < 2 * DataCarrSepar; k++)
  937. DoCorrelSum( CorrelNorm[s] + k,
  938. CorrelNorm[s2] + k + DataCarrSepar,
  939. CorrelAver[s] + k);
  940. }
  941. // symbol-shift dspPhase fitting
  942. for (i = 0; i < FitLen; i++) {
  943. SymbFit[i].re = dspAmpl(CorrelAver[0][i]) -
  944. dspAmpl(CorrelAver[2][i]);
  945. SymbFit[i].im = dspAmpl(CorrelAver[1][i]) -
  946. dspAmpl(CorrelAver[3][i]);
  947. }
  948. // P=dspFindMaxdspPower(SymbFit+30,4,j); j+=30;
  949. P = dspFindMaxdspPower(SymbFit + 2, FitLen- 4 , j);
  950. j += 2;
  951. // printf("[%2d,%2d]",j,SymbFitPos);
  952. k = (j - SymbFitPos) / DataCarrSepar;
  953. if (k > 1)
  954. j -= (k - 1) * DataCarrSepar;
  955. else if (k < (-1))
  956. j -= (k + 1) * DataCarrSepar;
  957. SymbFitPos = j;
  958. // printf(" => %2d",j);
  959. if (P > 0.0) {
  960. SymbConf = dspAmpl(SymbFit[j]) +
  961. 0.5 * (dspAmpl(SymbFit[j + 1]) + dspAmpl(SymbFit[j - 1]));
  962. SymbConf *= 0.5;
  963. I = SymbFit[j].re + 0.5 * (SymbFit[j - 1].re + SymbFit[j + 1].re);
  964. Q = SymbFit[j].im + 0.5 * (SymbFit[j - 1].im + SymbFit[j + 1].im);
  965. SymbTime.re = I;
  966. SymbTime.im = Q;
  967. SymbShift = (dspPhase(SymbTime) / (2 * M_PI)) * SymbolDiv;
  968. if (SymbShift < 0)
  969. SymbShift += SymbolDiv;
  970. // for (i=j-1; i<=j+1; i++) printf(" [%+5.2f,%+5.2f]",SymbFit[i].re,SymbFit[i].im);
  971. // make first estimation of FreqOfs
  972. // printf(" -> [%+5.2f,%+5.2f] =>",I,Q);
  973. // for (i=j-2; i<=j+2; i++) printf(" %+6.3f",I*SymbFit[i].re+Q*SymbFit[i].im);
  974. pI = dspScalProd(I, Q, SymbFit[j])
  975. + 0.7 * dspScalProd(I, Q, SymbFit[j - 1])
  976. + 0.7 * dspScalProd(I, Q, SymbFit[j + 1]);
  977. pQ = 0.7 * dspScalProd(I, Q, SymbFit[j + 1])
  978. - 0.7 * dspScalProd(I, Q, SymbFit[j - 1])
  979. + 0.5 * dspScalProd(I, Q, SymbFit[j + 2])
  980. - 0.5 * dspScalProd(I, Q, SymbFit[j - 2]);
  981. FreqOfs = j + dspPhase(pI, pQ) / (2.0 * M_PI / 8);
  982. /* SYNC TEST */
  983. // refine the FreqOfs
  984. i = (int)floor(FreqOfs + 0.5);
  985. s = (int)floor(SymbShift);
  986. s2 = (s + 1) & (SymbolDiv - 1);
  987. // printf(" [%5.2f,%2d,%d,%d] ",FreqOfs,i,s,s2);
  988. w0 = (s + 1 - SymbShift);
  989. w1 = (SymbShift - s);
  990. // printf(" [%4.2f,%4.2f] ",w0,w1);
  991. A = (0.5 * WindowLen) / SymbolSepar;
  992. I = w0 * CorrelAver[s][i].re + w1 * CorrelAver[s2][i].re;
  993. Q = w0 * CorrelAver[s][i].im + w1 * CorrelAver[s2][i].im;
  994. // printf(" [%5.2f,%2d] -> [%+5.2f,%+5.2f]",FreqOfs,i,I,Q);
  995. // FreqOfs=i+dspPhase(I,Q)/(2.0*M_PI)*0.5*A;
  996. // printf(" => %5.2f",FreqOfs);
  997. F0 = i + dspPhase(I, Q) / (2.0 * M_PI) * A - FreqOfs;
  998. Fl = F0 - A;
  999. Fu = F0 + A;
  1000. if (fabs(Fl) < fabs(F0))
  1001. FreqOfs += (fabs(Fu) < fabs(Fl)) ? Fu : Fl;
  1002. else
  1003. FreqOfs += (fabs(Fu) < fabs(F0)) ? Fu : F0;
  1004. // printf(" => (%5.2f,%5.2f,%5.2f) => %5.2f",Fl,F0,Fu,FreqOfs);
  1005. } else {
  1006. SymbTime.re = SymbTime.im = 0.0;
  1007. SymbConf = 0.0;
  1008. SymbShift = 0.0;
  1009. FreqOfs = 0.0;
  1010. }
  1011. // here we have FreqOfs and SymbTime.re/im
  1012. // printf("FreqOfs=%5.2f",FreqOfs);
  1013. if (SyncLocked) { // flip the SymbTime if it doesn't agree with the dspAverage
  1014. if (dspScalProd(SymbTime, AverSymb) < 0.0) {
  1015. SymbTime.re = (-SymbTime.re);
  1016. SymbTime.im = (-SymbTime.im);
  1017. FreqOfs -= DataCarrSepar;
  1018. }
  1019. // reduce the freq. offset towards the dspAverage offset
  1020. A = 2 * DataCarrSepar;
  1021. k = (int)floor((FreqOfs - AverFreq) / A + 0.5);
  1022. FreqOfs -= k * A;
  1023. /* SYNC TEST */
  1024. A = (0.5 * WindowLen) / SymbolSepar;
  1025. F0 = FreqOfs - AverFreq; // correct freq. auto-correlator wrap
  1026. Fl = F0 - A;
  1027. Fu = F0 + A;
  1028. if (fabs(Fl) < fabs(F0))
  1029. FreqOfs += (fabs(Fu) < fabs(Fl)) ? A : -A;
  1030. else
  1031. FreqOfs += (fabs(Fu) < fabs(F0)) ? A : 0.0;
  1032. // printf(" => (%5.2f,%5.2f,%5.2f) => %5.2f",Fl,F0,Fu,FreqOfs);
  1033. } else { // of if (SyncLocked)
  1034. // flip SymbTime if it doesn't agree with the previous
  1035. if (dspScalProd(SymbTime, SymbPipe[TrackPipePtr]) < 0.0) {
  1036. SymbTime.re = (-SymbTime.re);
  1037. SymbTime.im = (-SymbTime.im);
  1038. FreqOfs -= DataCarrSepar;
  1039. }
  1040. // reduce the FreqOfs towards zero
  1041. A = 2 * DataCarrSepar;
  1042. k = (int)floor(FreqOfs / A + 0.5);
  1043. FreqOfs -= k * A;
  1044. /* SYNC TEST */
  1045. F0 = FreqOfs - FreqPipe[TrackPipePtr];
  1046. Fl = F0 - A;
  1047. Fu = F0 + A;
  1048. if (fabs(Fl) < fabs(F0))
  1049. FreqOfs += (fabs(Fu) < fabs(Fl)) ? A : -A;
  1050. else
  1051. FreqOfs += (fabs(Fu) < fabs(F0)) ? A : 0.0;
  1052. }
  1053. // printf(" => [%+5.2f,%+5.2f], %5.2f",SymbTime.re,SymbTime.im,FreqOfs);
  1054. TrackPipePtr += 1;
  1055. if (TrackPipePtr >= TrackPipeLen)
  1056. TrackPipePtr -= TrackPipeLen;
  1057. SymbPipe[TrackPipePtr] = SymbTime; // put SymbTime and FreqOfs into pipes
  1058. FreqPipe[TrackPipePtr] = FreqOfs; // for averaging
  1059. // find dspAverage symbol time
  1060. Loops = dspSelFitAver( SymbPipe,
  1061. TrackPipeLen,
  1062. (double)3.0,
  1063. 4,
  1064. AverSymb,
  1065. dspRMS,
  1066. Incl);
  1067. // printf(" AverSymb=[%+5.2f,%+5.2f], dspRMS=%5.3f/%2d",
  1068. // AverSymb.re,AverSymb.im,dspRMS,Incl);
  1069. // find dspAverage freq. offset
  1070. Loops = dspSelFitAver( FreqPipe,
  1071. TrackPipeLen,
  1072. (double)2.5,
  1073. 4,
  1074. AverFreq,
  1075. dspRMS,
  1076. Incl);
  1077. SyncFreqDev = dspRMS;
  1078. // printf(" AverFreq=%+5.2f, dspRMS=%5.3f/%2d",AverFreq,dspRMS,Incl);
  1079. SymbConf = dspAmpl(AverSymb);
  1080. SyncSymbConf = SymbConf;
  1081. SyncFreqOfs = AverFreq;
  1082. if (SymbConf > 0.0) {
  1083. SymbShift = dspPhase(AverSymb) / (2 * M_PI) * SymbolSepar;
  1084. if (SymbShift < 0.0)
  1085. SymbShift += SymbolSepar;
  1086. SymbPtr = (int)floor((dspPhase(AverSymb) / (2 * M_PI)) * SymbolDiv);
  1087. if (SymbPtr < 0)
  1088. SymbPtr += SymbolDiv;
  1089. SyncSymbShift = SymbShift;
  1090. }
  1091. if (SyncLocked) {
  1092. if ((SyncSymbConf < SyncHoldThres) || (SyncFreqDev > 0.250))
  1093. SyncLocked = 0;
  1094. } else {
  1095. if ((SyncSymbConf > SyncLockThres) && (SyncFreqDev < 0.125))
  1096. SyncLocked = 1;
  1097. }
  1098. SyncSymbConf *= 0.5;
  1099. // printf(" => SyncLocked=%d, SyncSymbShift=%5.1f, SymbPtr=%d",
  1100. // SyncLocked,SyncSymbShift,SymbPtr);
  1101. // printf("\n");
  1102. } // enf of if (SyncPtr==(SymbPtr^2))
  1103. }
  1104. void MT63rx::DataProcess(dspCmpx *EvenSlice, dspCmpx *OddSlice, double FreqOfs, int TimeDist)
  1105. {
  1106. int i, c, r;
  1107. dspCmpx Freq, Phas;
  1108. int incr, p;
  1109. double I, Q, P;
  1110. dspCmpx Dtmp;
  1111. dspCmpx Ftmp;
  1112. // double Aver,dspRMS; int Loops,Incl;
  1113. // Here we pickup a symbol in the data history. The time/freq. synchronizer
  1114. // told us where it is in time and at which frequency offset (FreqOfs)
  1115. // TimeDist is the distance in samples from the symbol we analyzed
  1116. // in the previous call to this routine
  1117. // FreqOfs=0.0; // for DEBUG only !
  1118. // printf("DataProcess: FreqOfs=%5.3f, TimeDist=%d, Locked=%d\n",
  1119. // FreqOfs,TimeDist,SyncLocked);
  1120. P = (-2 * M_PI * FreqOfs) / WindowLen; // make ready for frequency correction
  1121. Freq.re = cos(P);
  1122. Freq.im = sin(P);
  1123. Phas.re = 1.0;
  1124. Phas.im = 0.0;
  1125. for (i = 0; i < WindowLen; i++) { // prepare slices for the FFT
  1126. r = FFT.BitRevIdx[i]; // multiply by window and pre-scramble
  1127. // if (i==2*ScanMargin)
  1128. // printf("%3d: [%5.2f,%5.2f] [%5.2f,%5.2f]\n",
  1129. // i, dspPhase.re,dspPhase.im, EvenSlice[i].re,EvenSlice[i].im);
  1130. CdspcmpxMultAxB(I, Q, EvenSlice[i], Phas);
  1131. FFTbuff[r].re = I * RxWindow[i];
  1132. FFTbuff[r].im = Q * RxWindow[i];
  1133. CdspcmpxMultAxB(I, Q, OddSlice[i], Phas);
  1134. FFTbuff2[r].re = I * RxWindow[i];
  1135. FFTbuff2[r].im = Q * RxWindow[i];
  1136. CdspcmpxMultAxB(Dtmp, Phas, Freq);
  1137. Phas = Dtmp;
  1138. }
  1139. FFT.CoreProc(FFTbuff);
  1140. FFT.CoreProc(FFTbuff2);
  1141. /*
  1142. printf("FFTbuff [%3d...]:",FirstDataCarr-16);
  1143. for (i=FirstDataCarr-16; i<=FirstDataCarr+32; i++)
  1144. printf(" %+3d/%4.2f",i-FirstDataCarr,dspAmpl(FFTbuff[i]));
  1145. printf("\n");
  1146. printf("FFTbuff2[%3d...]:",FirstDataCarr-16);
  1147. for (i=FirstDataCarr-16; i<=FirstDataCarr+32; i++)
  1148. printf(" %+3d/%4.2f",i-FirstDataCarr,dspAmpl(FFTbuff2[i]));
  1149. printf("\n");
  1150. */
  1151. // printf(" FreqOfs=%5.2f: ",FreqOfs);
  1152. // printf("Symbol vectors:\n");
  1153. incr = (TimeDist * DataCarrSepar) & WindowLenMask; // correct FFT dspPhase shift
  1154. p = (TimeDist * DataScanFirst) & WindowLenMask; // due to time shift by
  1155. for (c = DataScanFirst, i = 0; i < DataScanLen; ) { // TimeDist
  1156. // printf("%2d,%3d:",i,c);
  1157. // printf(" [%6.3f,%6.3f] [%6.3f,%6.3f]",
  1158. // FFTbuff[c].re,FFTbuff[c].im,
  1159. // FFTbuff2[c+DataCarrSepar].re,FFTbuff2[c+DataCarrSepar].im);
  1160. // printf(" [%6.3f,%6.3f]/[%6.3f,%6.3f]",
  1161. // FFTbuff2[c].re,FFTbuff2[c].im,
  1162. // FFTbuff[c+DataCarrSepar].re,FFTbuff[c+DataCarrSepar].im);
  1163. // printf(" %5.3f/%5.3f",dspAmpl(FFTbuff[c]),dspAmpl(FFTbuff[c+DataCarrSepar]));
  1164. // printf(" %5.3f/%5.3f",dspAmpl(FFTbuff2[c+DataCarrSepar]),dspAmpl(FFTbuff2[c]));
  1165. // printf("\n");
  1166. Phas = FFT.Twiddle[p];
  1167. CdspcmpxMultAxB(Dtmp, RefDataSlice[i], Phas);
  1168. CdspcmpxMultAxBs(DataVect[i], FFTbuff[c], Dtmp);
  1169. // printf("%3d,%2d: [%8.5f,%8.5f] / %8.5f\n",
  1170. // c,i,FFTbuff[c].re,FFTbuff[c].im,DataPwrOut[i]);
  1171. dspLowPass2( dspPower(FFTbuff[c]),
  1172. DataPwrMid[i],
  1173. DataPwrOut[i], dW1, dW2, dW5);
  1174. RefDataSlice[i++] = FFTbuff[c];
  1175. c = (c + DataCarrSepar) & WindowLenMask;
  1176. p = (p + incr) & WindowLenMask;
  1177. Phas = FFT.Twiddle[p];
  1178. CdspcmpxMultAxB(Dtmp, RefDataSlice[i], Phas);
  1179. CdspcmpxMultAxBs(DataVect[i], FFTbuff2[c], Dtmp);
  1180. // printf("%3d,%2d: [%8.5f,%8.5f] / %8.5f\n",
  1181. // c,i,FFTbuff2[c].re,FFTbuff2[c].im,DataPwrOut[i]);
  1182. dspLowPass2( dspPower(FFTbuff2[c]),
  1183. DataPwrMid[i],
  1184. DataPwrOut[i], dW1, dW2, dW5);
  1185. RefDataSlice[i++] = FFTbuff2[c];
  1186. c = (c + DataCarrSepar) & WindowLenMask;
  1187. p = (p + incr) & WindowLenMask;
  1188. }
  1189. P = (-TimeDist * 2 * M_PI * FreqOfs) / WindowLen;
  1190. Freq.re = cos(P);
  1191. Freq.im = sin(P);
  1192. for (i = 0; i < DataScanLen; i++) {
  1193. CdspcmpxMultAxB(Ftmp, DataVect[i], Freq);
  1194. // dspLowPass2(dspPower(Ftmp),DataPwrMid[i],DataPwrOut[i],dW1,dW2,dW5);
  1195. // CdspcmpxMultAxB(Dtmp,Ftmp,Ftmp);
  1196. // Dtmp.re=Ftmp.re*Ftmp.re-Ftmp.im*Ftmp.im; Dtmp.im=2*Ftmp.re*Ftmp.im;
  1197. // dspLowPass2(&Dtmp,DataSqrMid+i,DataSqrOut+i,dW1,dW2,dW5);
  1198. DataVect[i] = DataPipe[DataPipePtr][i];
  1199. DataPipe[DataPipePtr][i] = Ftmp;
  1200. }
  1201. DataPipePtr += 1;
  1202. if (DataPipePtr >= DataPipeLen)
  1203. DataPipePtr = 0;
  1204. for (i = 0; i < DataScanLen; i++) {
  1205. if (DataPwrOut[i] > 0.0) {
  1206. P = DataVect[i].re / DataPwrOut[i];
  1207. if (P > 1.0)
  1208. P = 1.0;
  1209. else if (P < (-1.0))
  1210. P = (-1.0);
  1211. DatadspPhase[i] = P;
  1212. } else
  1213. DatadspPhase[i] = 0.0;
  1214. }
  1215. Decoder.Process(DatadspPhase);
  1216. Output.EnsureSpace(Output.Len + 1);
  1217. Output.Data[Output.Len] = Decoder.Output;
  1218. Output.Len += 1;
  1219. /*
  1220. printf("Demodulator output vectors:\n");
  1221. for (i=0; i<DataScanLen; i++)
  1222. { printf("%2d: [%8.5f,%8.5f] / %8.5f => %8.5f\n",
  1223. i,DataVect[i].re,DataVect[i].im,DataPwrOut[i], DatadspPhase[i]);
  1224. }
  1225. */
  1226. /*
  1227. for (i=0; i<DataScanLen; i++)
  1228. { // printf("%2d: [%8.5f,%8.5f]\n",i,DataVect[i].re,DataVect[i].im);
  1229. if (dspPower(DataVect[i])>0.0) P=dspPhase(DataVect[i]); else P=0.0;
  1230. DatadspPhase[i]=P;
  1231. P*=2; if (P>M_PI) P-=2*M_PI; else if (P<(-M_PI)) P+=2*M_PI;
  1232. DatadspPhase2[i]=P;
  1233. printf("%2d: %6.3f [%6.3f,%6.3f] [%8.5f,%8.5f], %5.2f, %5.2f",
  1234. i, DataPwrOut[i], DataSqrOut[i].re,DataSqrOut[i].im,
  1235. DataVect[i].re,DataVect[i].im, DatadspPhase[i],DatadspPhase2[i]);
  1236. if (DataPwrOut[i]>0.0)
  1237. printf(" %6.3f",dspAmpl(DataSqrOut[i])/DataPwrOut[i]);
  1238. printf("\n");
  1239. }
  1240. Loops=dspSelFitAver(DatadspPhase2,DataScanLen,(double)2.5,4,Aver,dspRMS,Incl);
  1241. printf("Aver=%5.2f, dspRMS=%5.2f, Incl=%d\n",Aver,dspRMS,Incl);
  1242. */
  1243. }
  1244. int MT63rx::SYNC_LockStatus(void) {
  1245. return SyncLocked;
  1246. }
  1247. double MT63rx::SYNC_Confidence(void) {
  1248. return SyncSymbConf <= 1.0 ? SyncSymbConf : 1.0;
  1249. }
  1250. double MT63rx::SYNC_FreqOffset(void) {
  1251. return SyncFreqOfs / DataCarrSepar;
  1252. }
  1253. double MT63rx::SYNC_FreqDevdspRMS(void) {
  1254. return SyncFreqDev / DataCarrSepar;
  1255. }
  1256. double MT63rx::SYNC_TimeOffset(void) {
  1257. return SyncSymbShift / SymbolSepar;
  1258. }
  1259. double MT63rx::FEC_SNR(void) {
  1260. return Decoder.SignalToNoise;
  1261. }
  1262. int MT63rx::FEC_CarrOffset(void) {
  1263. return Decoder.CarrOfs;
  1264. }
  1265. double MT63rx::TotalFreqOffset(void) {
  1266. return ( SyncFreqOfs + DataCarrSepar * Decoder.CarrOfs) *
  1267. (8000.0 / DecimateRatio) / WindowLen;
  1268. }