CPP 9
Search.cpp Guest on 4th October 2020 06:51:01 PM
  1. //////////////////////////////////////////////////////////////////////////
  2. // Homemade GPS Receiver
  3. // Copyright (C) 2013 Andrew Holme
  4. //
  5. // This program is free software: you can redistribute it and/or modify
  6. // it under the terms of the GNU General Public License as published by
  7. // the Free Software Foundation, either version 3 of the License, or
  8. // (at your option) any later version.
  9. //
  10. // This program is distributed in the hope that it will be useful,
  11. // but WITHOUT ANY WARRANTY; without even the implied warranty of
  12. // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  13. // GNU General Public License for more details.
  14. //
  15. // You should have received a copy of the GNU General Public License
  16. // along with this program.  If not, see <http://www.gnu.org/licenses/>.
  17. //
  18. // http://www.aholme.co.uk/GPS/Main.htm
  19. //////////////////////////////////////////////////////////////////////////
  20.  
  21. #include <memory.h>
  22. #include <fftw3.h>
  23. #include <math.h>
  24.  
  25. #include "gps.h"
  26. #include "spi.h"
  27. #include "cacode.h"
  28.  
  29. ///////////////////////////////////////////////////////////////////////////////////////////////
  30.  
  31. struct SATELLITE {
  32.     int prn, navstar, T1, T2;
  33. };
  34.  
  35. static const SATELLITE Sats[NUM_SATS] = {
  36.     { 1,  63,  2,  6},
  37.     { 2,  56,  3,  7},
  38.     { 3,  37,  4,  8},
  39.     { 4,  35,  5,  9},
  40.     { 5,  64,  1,  9},
  41.     { 6,  36,  2, 10},
  42.     { 7,  62,  1,  8},
  43.     { 8,  44,  2,  9},
  44.     { 9,  33,  3, 10},
  45.     {10,  38,  2,  3},
  46.     {11,  46,  3,  4},
  47.     {12,  59,  5,  6},
  48.     {13,  43,  6,  7},
  49.     {14,  49,  7,  8},
  50.     {15,  60,  8,  9},
  51.     {16,  51,  9, 10},
  52.     {17,  57,  1,  4},
  53.     {18,  50,  2,  5},
  54.     {19,  54,  3,  6},
  55.     {20,  47,  4,  7},
  56.     {21,  52,  5,  8},
  57.     {22,  53,  6,  9},
  58.     {23,  55,  1,  3},
  59.     {24,  23,  4,  6},
  60.     {25,  24,  5,  7},
  61.     {26,  26,  6,  8},
  62.     {27,  27,  7,  9},
  63.     {28,  48,  8, 10},
  64.     {29,  61,  1,  6},
  65.     {30,  39,  2,  7},
  66.     {31,  58,  3,  8},
  67.     {32,  22,  4,  9},
  68. };
  69.  
  70. static bool Busy[NUM_SATS];
  71.  
  72. ///////////////////////////////////////////////////////////////////////////////////////////////
  73.  
  74. static fftwf_complex code[NUM_SATS][FFT_LEN];
  75.  
  76. static fftwf_complex fwd_buf[FFT_LEN],
  77.                      rev_buf[FFT_LEN];
  78.  
  79. static fftwf_plan fwd_plan, rev_plan;
  80.  
  81. ///////////////////////////////////////////////////////////////////////////////////////////////
  82.  
  83. float inline Bipolar(int bit) {
  84.     return bit? -1.0 : +1.0;
  85. }
  86.  
  87. ///////////////////////////////////////////////////////////////////////////////////////////////
  88.  
  89. int SearchInit() {
  90.  
  91.     const float ca_rate = CPS/FS;
  92.  
  93.     fwd_plan = fftwf_plan_dft_1d(FFT_LEN, fwd_buf, fwd_buf, FFTW_FORWARD,  FFTW_ESTIMATE);
  94.     rev_plan = fftwf_plan_dft_1d(FFT_LEN, rev_buf, rev_buf, FFTW_BACKWARD, FFTW_ESTIMATE);
  95.  
  96.     for (int sv=0; sv<NUM_SATS; sv++) {
  97.  
  98.         CACODE ca(Sats[sv].T1, Sats[sv].T2);
  99.         float ca_phase=0;
  100.  
  101.         for (int i=0; i<FFT_LEN; i++) {
  102.  
  103.             float chip = Bipolar(ca.Chip()); // chip at start of sample period
  104.  
  105.             ca_phase += ca_rate; // NCO phase at end of period
  106.  
  107.             if (ca_phase >= 1.0) { // reached or crossed chip boundary?
  108.                 ca_phase -= 1.0;
  109.                 ca.Clock();
  110.  
  111.                 // These two lines do not make much difference
  112.                 chip *= 1.0 - ca_phase;                 // prev chip
  113.                 chip += ca_phase * Bipolar(ca.Chip());  // next chip
  114.             }
  115.  
  116.             fwd_buf[i][0] = chip;
  117.             fwd_buf[i][1] = 0;
  118.         }
  119.  
  120.         fftwf_execute(fwd_plan);
  121.         memcpy(code[sv], fwd_buf, sizeof fwd_buf);
  122.     }
  123.  
  124.     return 0;
  125. }
  126.  
  127. ///////////////////////////////////////////////////////////////////////////////////////////////
  128.  
  129. void SearchFree() {
  130.     fftwf_destroy_plan(fwd_plan);
  131.     fftwf_destroy_plan(rev_plan);
  132. }
  133.  
  134. ///////////////////////////////////////////////////////////////////////////////////////////////
  135.  
  136. static void Sample() {
  137.     const int lo_sin[] = {1,1,0,0}; // Quadrature local oscillators
  138.     const int lo_cos[] = {1,0,0,1};
  139.  
  140.     const float lo_rate = 4*FC/FS; // NCO rate
  141.  
  142.     const int MS = 1000*FFT_LEN/FS; // Sample length
  143.     const int PACKET = 512;
  144.  
  145.     float lo_phase=0; // NCO phase accumulator
  146.     int i=0, j=PACKET, k;
  147.     SPI_MISO rx;
  148.  
  149.     spi_set(CmdSample); // Trigger sampler and reset code generator in FPGA
  150.     TimerWait(MS);
  151.  
  152.     while (i<FFT_LEN) {
  153.         if (j==PACKET) {
  154.             spi_get(CmdGetSamples, &rx, PACKET);
  155.             j=0;
  156.         }
  157.         int byte = rx.byte[j++];
  158.         for (k=i+8; i<k; i++) {
  159.             int bit = byte&1;
  160.             byte>>=1;
  161.  
  162.             // Down convert to complex (IQ) baseband by mixing (XORing)
  163.             // samples with quadrature local oscillators
  164.             fwd_buf[i][0] = Bipolar(bit ^ lo_sin[int(lo_phase)]);
  165.             fwd_buf[i][1] = Bipolar(bit ^ lo_cos[int(lo_phase)]);
  166.  
  167.             lo_phase += lo_rate;
  168.             if (lo_phase>=4) lo_phase-=4;
  169.         }
  170.     }
  171.  
  172.     fftwf_execute(fwd_plan); // Transform to frequency domain
  173.     NextTask();
  174. }
  175.  
  176. ///////////////////////////////////////////////////////////////////////////////////////////////
  177.  
  178. static float Correlate(int sv, int *max_snr_dop, int *max_snr_i) {
  179.  
  180.     fftwf_complex *data = fwd_buf;
  181.     fftwf_complex *prod = rev_buf;
  182.     float max_snr=0;
  183.     int i;
  184.  
  185.     for (int dop=-5000*FFT_LEN/FS; dop<=5000*FFT_LEN/FS; dop++) {
  186.         float max_pwr=0, tot_pwr=0;
  187.         int max_pwr_i=0;
  188.  
  189.         // (a-ib)(x+iy) = (ax+by) + i(ay-bx)
  190.         for (i=0; i<FFT_LEN; i++) {
  191.             int j=(i-dop+FFT_LEN)%FFT_LEN;
  192.             prod[i][0] = data[i][0]*code[sv][j][0] + data[i][1]*code[sv][j][1];
  193.             prod[i][1] = data[i][0]*code[sv][j][1] - data[i][1]*code[sv][j][0];
  194.         }
  195.  
  196.         fftwf_execute(rev_plan);
  197.         NextTask();
  198.  
  199.         for (i=0; i<FS/1000; i++) {
  200.             float pwr = prod[i][0]*prod[i][0] + prod[i][1]*prod[i][1];
  201.             if (pwr>max_pwr) max_pwr=pwr, max_pwr_i=i;
  202.             tot_pwr += pwr;
  203.         }
  204.  
  205.         float ave_pwr = tot_pwr/i;
  206.         float snr = max_pwr/ave_pwr;
  207.         if (snr>max_snr) max_snr=snr, *max_snr_dop=dop, *max_snr_i=max_pwr_i;
  208.     }
  209.     return max_snr;
  210. }
  211.  
  212. ///////////////////////////////////////////////////////////////////////////////////////////////
  213.  
  214. int SearchCode(int sv, unsigned int g1) { // Could do this with look-up tables
  215.     int chips=0;
  216.     for (CACODE ca(Sats[sv].T1, Sats[sv].T2); ca.GetG1()!=g1; ca.Clock()) chips++;
  217.     return chips;
  218. }
  219.  
  220. ///////////////////////////////////////////////////////////////////////////////////////////////
  221.  
  222. void SearchEnable(int sv) {
  223.     Busy[sv] = false;
  224. }
  225.  
  226. ///////////////////////////////////////////////////////////////////////////////////////////////
  227.  
  228. void SearchTask() {
  229.     int ch, sv, t_sample, lo_shift=0, ca_shift=0;
  230.     float snr;
  231.  
  232.     for(;;)
  233.         for (sv=0; sv<NUM_SATS; sv++) {
  234.             if (Busy[sv]) // SV already acquired?
  235.                 continue;
  236.  
  237.             while((ch=ChanReset())<0) // all channels busy?
  238.                 NextTask();
  239.  
  240.             t_sample = Microseconds(); // sample time
  241.             Sample();
  242.             snr = Correlate(sv, &lo_shift, &ca_shift);
  243.  
  244.             UserStat(STAT_PRN, snr, Sats[sv].prn);
  245.  
  246.             if (snr<25)
  247.                 continue;
  248.  
  249.             Busy[sv] = true;
  250.             ChanStart(ch, sv, t_sample, (Sats[sv].T1<<4) +
  251.                                          Sats[sv].T2, lo_shift, ca_shift);
  252.         }
  253. }

Paste is for source code and general debugging text.

Login or Register to edit, delete and keep track of your pastes and more.

Raw Paste

Login or Register to edit or fork this paste. It's free.