Add L2C tracking (WIP)

src/Makefile

@@ -98,6 +98,7 @@ CSRC := $(STARTUPSRC) \
         $(SWIFTNAV_ROOT)/src/track_internal.o \
         $(SWIFTNAV_ROOT)/src/track_api.o \
         $(SWIFTNAV_ROOT)/src/track_gps_l1ca.o \
+        $(SWIFTNAV_ROOT)/src/track_gps_l2cm.o \
         $(SWIFTNAV_ROOT)/src/manage.o \
         $(SWIFTNAV_ROOT)/src/settings.o \
         $(SWIFTNAV_ROOT)/src/timing.o \

src/board/nap/track_channel.h

@@ -1,5 +1,5 @@
 /*
- * Copyright (C) 2011-2014 Swift Navigation Inc.
+ * Copyright (C) 2011-2014,2016 Swift Navigation Inc.
  * Contact: Fergus Noble <[email protected]>
  *          Colin Beighley <[email protected]>
  *
@@ -29,7 +29,7 @@ extern u8 nap_track_n_channels;
 u32 nap_track_init(u8 channel, gnss_signal_t sid, u32 ref_timing_count,
                    float carrier_freq, float code_phase);
 
-void nap_track_update_wr_blocking(u8 channel, s32 carrier_freq,
+void nap_track_update_wr_blocking(code_t code, u8 channel, s32 carrier_freq,
                                   u32 code_phase_rate, u8 rollover_count,
                                   u8 corr_spacing);
 void nap_track_corr_rd_blocking(u8 channel,

src/board/v3/nap/nap_regs.h

@@ -99,6 +99,15 @@ typedef struct {
 #define NAP_TRK_CONTROL_PRN_Pos (3u)
 #define NAP_TRK_CONTROL_PRN_Msk (0x1FU << NAP_TRK_CONTROL_PRN_Pos)
 
+#define NAP_TRK_CONTROL_CODE_Pos (9U)
+#define NAP_TRK_CONTROL_CODE_Msk (0x3U << NAP_TRK_CONTROL_CODE_Pos)
+
+#define NAP_TRK_CONTROL_RF_FE_Pos (1U)
+#define NAP_TRK_CONTROL_RF_FE_Msk (0x3U << NAP_TRK_CONTROL_RF_FE_Pos)
+
+#define NAP_TRK_CONTROL_RF_FE_CH_Pos (0U)
+#define NAP_TRK_CONTROL_RF_FE_CH_Msk (0x1U << NAP_TRK_CONTROL_RF_FE_CH_Pos)
+
 #define NAP_TRK_STATUS_RUNNING (1 << 31)
 
 /* Instances */

src/board/v3/nap/nap_track.c

@@ -28,80 +28,187 @@ struct {
  * \param carrier_freq Current carrier frequency (i.e. Doppler) in Hz used for
  *                     carrier aiding.
  * \param n_samples    N, the number of samples to propagate for.
+ * \param code         Code identifier.
  *
  * \return The propagated code phase in chips.
  */
-static double propagate_code_phase(double code_phase, double carrier_freq, u32 n_samples)
+static double propagate_code_phase(double code_phase, double carrier_freq,
+                                   u32 n_samples, code_t code)
 {
   /* Calculate the code phase rate with carrier aiding. */
-  double code_phase_rate = (1.0 + carrier_freq/GPS_L1_HZ) * GPS_CA_CHIPPING_RATE;
+  double code_phase_rate = (1.0 + carrier_freq/code_to_carr_freq(code)) *
+                           GPS_CA_CHIPPING_RATE;
   code_phase += n_samples * code_phase_rate / SAMPLE_FREQ;
   u32 cp_int = floor(code_phase);
-  code_phase -= cp_int - (cp_int % 1023);
+  code_phase -= cp_int - (cp_int % code_to_chip_num(code));
   return code_phase;
 }
 
-static u32 calc_length_samples(u8 codes, s32 cp_start, u32 cp_rate)
+static u32 calc_length_samples(code_t code_id, u8 codes, s32 cp_start, u32 cp_rate)
 {
-  u16 chips = codes * 1023;
+  u16 chips = codes * code_to_chip_num(code_id);
   u64 cp_units = (1ULL << NAP_TRACK_CODE_PHASE_FRACTIONAL_WIDTH) * chips
                  - cp_start;
   u32 samples = cp_units / cp_rate;
   return samples;
 }
 
+u16 sid_to_init_g1(gnss_signal_t sid)
+{
+  u16 ret;
+  u32 gps_l2cm_prns_init_values[] = {
+    /* 0  */ 0742417664,
+    /* 1  */ 0756014035,
+    /* 2  */ 0002747144,
+    /* 3  */ 0066265724,
+    /* 4  */ 0601403471,
+    /* 5  */ 0703232733,
+    /* 6  */ 0124510070,
+    /* 7  */ 0617316361,
+    /* 8  */ 0047541621,
+    /* 9  */ 0733031046,
+    /* 10 */ 0713512145,
+    /* 11 */ 0024437606,
+    /* 12 */ 0021264003,
+    /* 13 */ 0230655351,
+    /* 14 */ 0001314400,
+    /* 15 */ 0222021506,
+    /* 16 */ 0540264026,
+    /* 17 */ 0205521705,
+    /* 18 */ 0064022144,
+    /* 19 */ 0120161274,
+    /* 20 */ 0044023533,
+    /* 21 */ 0724744327,
+    /* 22 */ 0045743577,
+    /* 23 */ 0741201660,
+    /* 24 */ 0700274134,
+    /* 25 */ 0010247261,
+    /* 26 */ 0713433445,
+    /* 27 */ 0737324162,
+    /* 28 */ 0311627434,
+    /* 29 */ 0710452007,
+    /* 30 */ 0722462133,
+    /* 31 */ 0050172213,
+    /* 32 */ 0500653703,
+    /* 33 */ 0755077436,
+    /* 34 */ 0136717361,
+    /* 35 */ 0756675453,
+    /* 36 */ 0435506112
+  };
+
+  switch (sid.code) {
+
+    case CODE_GPS_L2CM:
+      ret = (u16) gps_l2cm_prns_init_values[sid.sat];
+      break;
+
+    case CODE_GPS_L1CA:
+    default:
+      ret = 0x3ff;
+      break;
+  }
+
+  return ret;
+}
+
+u8 sid_to_rf_fronend_channel(gnss_signal_t sid)
+{
+  u8 ret;
+  switch (sid.code) {
+  case CODE_GPS_L1CA:
+  case CODE_SBAS_L1CA:
+    ret = 0;
+    break;
+  case CODE_GPS_L2CM:
+    ret = 3;
+    break;
+  default:
+    assert(0);
+    break;
+  }
+  return ret;
+}
+
 u32 nap_track_init(u8 channel, gnss_signal_t sid, u32 ref_timing_count,
                    float carrier_freq, float code_phase)
 {
   u32 now = NAP->TIMING_COUNT;
   u32 track_count = now + 200000;
+  /* log_warn("---- ADEL debug %s:%d chan = %d prn = %d, carr_freq = %f, code_phase = %f", */
+  /*          __FILE__, __LINE__, (int)channel, (int)(sid.sat - 1), carrier_freq, code_phase); */
   double cp = propagate_code_phase(code_phase, carrier_freq,
-                                  track_count - ref_timing_count);
+                                  track_count - ref_timing_count, sid.code);
 
   /* Contrive for the timing strobe to occur at or close to a
    * PRN edge (code phase = 0) */
-  track_count += (SAMPLE_FREQ/GPS_CA_CHIPPING_RATE) * (1023.0-cp) *
-                 (1.0 + carrier_freq / GPS_L1_HZ);
+  track_count += (SAMPLE_FREQ / GPS_CA_CHIPPING_RATE) *
+                 (code_to_chip_num(sid.code) - cp) *
+                 (1.0 + carrier_freq / code_to_carr_freq(sid.code));
+
+  /* log_warn("---- ADEL debug %s:%d chan = %d prn = %d, carr_freq = %f, code_phase = %f", */
+  /*          __FILE__, __LINE__, (int)channel, (int)(sid.sat - 1), carrier_freq, code_phase); */
 
   u8 prn = sid.sat - 1;
-  NAP->TRK_CH[channel].CONTROL = prn << NAP_TRK_CONTROL_PRN_Pos;
+  u16 control = (prn << NAP_TRK_CONTROL_PRN_Pos) & NAP_TRK_CONTROL_PRN_Msk;
+  control |= (sid_to_rf_fronend_channel(sid) << NAP_TRK_CONTROL_RF_FE_Pos) & \
+             NAP_TRK_CONTROL_RF_FE_Msk;
+  //control |= (1 << NAP_TRK_CONTROL_RF_FE_CH_Pos) & NAP_TRK_CONTROL_RF_FE_CH_Msk;
+
+  log_warn("- ADEL debug %s:%d ch=%d prn=%d, cf=%f, cp=%f, ctrl=0x%04X",
+           __FILE__, __LINE__, (int)channel, (int)(sid.sat - 1), carrier_freq, code_phase, control);
+
+  NAP->TRK_CH[channel].CONTROL = control;
+  log_warn("---- ADEL debug %s:%d", __FILE__, __LINE__);
   /* We always start at zero code phase */
   NAP->TRK_CH[channel].CODE_INIT_INT = 0;
+  log_warn("---- ADEL debug %s:%d", __FILE__, __LINE__);
   NAP->TRK_CH[channel].CODE_INIT_FRAC = 0;
-  NAP->TRK_CH[channel].CODE_INIT_G1 = 0x3ff;
+  log_warn("---- ADEL debug %s:%d", __FILE__, __LINE__);
+  NAP->TRK_CH[channel].CODE_INIT_G1 = sid_to_init_g1(sid);
+  log_warn("---- ADEL debug %s:%d", __FILE__, __LINE__);
   NAP->TRK_CH[channel].CODE_INIT_G2 = 0x3ff;
+  log_warn("---- ADEL debug %s:%d", __FILE__, __LINE__);
 
   NAP->TRK_CH[channel].SPACING = (SPACING_HALF_CHIP << 16) | SPACING_HALF_CHIP;
 
-  u32 cp_rate = (1.0 + carrier_freq/GPS_L1_HZ) * GPS_CA_CHIPPING_RATE *
-                NAP_TRACK_CODE_PHASE_RATE_UNITS_PER_HZ;
+  u32 cp_rate = (1.0 + carrier_freq / code_to_carr_freq(sid.code)) *
+                GPS_CA_CHIPPING_RATE * NAP_TRACK_CODE_PHASE_RATE_UNITS_PER_HZ;
 
   NAP->TRK_CH[channel].CARR_PINC = -carrier_freq *
                                    NAP_TRACK_CARRIER_FREQ_UNITS_PER_HZ;
   NAP->TRK_CH[channel].CODE_PINC = cp_rate;
-  pipeline[channel].len = NAP->TRK_CH[channel].LENGTH = calc_length_samples(1, 0, cp_rate)+1;
+
+  log_warn("---- ADEL debug %s:%d", __FILE__, __LINE__);
+
+  pipeline[channel].len = NAP->TRK_CH[channel].LENGTH =
+    calc_length_samples(sid.code, 1, 0, cp_rate) + 1;
   pipeline[channel].code_phase = (NAP->TRK_CH[channel].LENGTH) * cp_rate;
   pipeline[channel].carrier_phase = NAP->TRK_CH[channel].LENGTH *
                                     (s64)-NAP->TRK_CH[channel].CARR_PINC;
   NAP->TRK_CONTROL |= (1 << channel); /* Set to start on the timing strobe */
 
   NAP->TRK_TIMING_COMPARE = track_count - SAMPLE_FREQ / GPS_CA_CHIPPING_RATE;
 
+  log_warn("---- ADEL debug %s:%d", __FILE__, __LINE__);
+
   //chThdSleep(CH_CFG_ST_FREQUENCY * ceil((float)(track_count - now)/SAMPLE_FREQ));
   /* Spin until we're really running */
   while (!(NAP->TRK_CH[channel].STATUS & NAP_TRK_STATUS_RUNNING));
+
+  log_warn("---- ADEL debug %s:%d", __FILE__, __LINE__);
+
   return NAP->TRK_CH[channel].START_SNAPSHOT;
 }
 
-void nap_track_update_wr_blocking(u8 channel, s32 carrier_freq,
+void nap_track_update_wr_blocking(code_t code, u8 channel, s32 carrier_freq,
                                   u32 code_phase_rate, u8 rollover_count,
                                   u8 corr_spacing)
 {
   (void)corr_spacing; /* This is always written as 0 now... */
 
   NAP->TRK_CH[channel].CARR_PINC = -carrier_freq;
   NAP->TRK_CH[channel].CODE_PINC = code_phase_rate;
-  NAP->TRK_CH[channel].LENGTH = calc_length_samples(rollover_count + 1,
+  NAP->TRK_CH[channel].LENGTH = calc_length_samples(code, rollover_count + 1,
                                     pipeline[channel].code_phase,
                                     code_phase_rate);
 

src/decode.c

@@ -56,7 +56,7 @@
  *   are allocated independently when initializing decoding.
  */
 
-#define NUM_DECODER_CHANNELS  12
+#define NUM_DECODER_CHANNELS  32
 
 typedef enum {
   DECODER_CHANNEL_STATE_DISABLED,

src/main.c

@@ -27,6 +27,7 @@
 #include "manage.h"
 #include "track.h"
 #include "track_gps_l1ca.h"
+#include "track_gps_l2cm.h"
 #include "timing.h"
 #include "ext_events.h"
 #include "solution.h"
@@ -44,6 +45,8 @@
 
 extern void ext_setup(void);
 
+void track_gps_l2cm_register(void) TRK_WEAK;
+
 /** Compare version strings.
  * Compares a version of the form 'vX.Y-Z-'. If the first character of the
  * version is not 'v' then that string will be considered older than any
@@ -174,6 +177,7 @@ int main(void)
   position_setup();
   track_setup();
   track_gps_l1ca_register();
+  track_gps_l2cm_register();
   decode_setup();
   decode_gps_l1_register();
 

src/manage.c

@@ -193,6 +193,13 @@ void manage_acq_setup()
     track_mask[i] = false;
     almanac[i].valid = 0;
 
+    if (CODE_GPS_L2CM == acq_status[i].sid.code) {
+      /* Do not acquire GPS L2C.
+       * Do GPS L1 C/A to L2C handover at the tracking stage instead. */
+      acq_status[i].state = ACQ_PRN_SKIP;
+      acq_status[i].masked = true;
+    }
+
     if (!sbas_enabled &&
         (sid_to_constellation(acq_status[i].sid) == CONSTELLATION_SBAS)) {
       acq_status[i].masked = true;
@@ -260,8 +267,9 @@ static u16 manage_warm_start(gnss_signal_t sid, const gps_time_t* t,
       /* sat_pos now holds unit vector from us to satellite */
       vector_subtract(3, sat_vel, position_solution.vel_ecef, sat_vel);
       /* sat_vel now holds velocity of sat relative to us */
-      dopp_hint = -GPS_L1_HZ * (vector_dot(3, sat_pos, sat_vel) / GPS_C
-                                + position_solution.clock_bias);
+      dopp_hint = -code_to_carr_freq(sid.code) *
+                  (vector_dot(3, sat_pos, sat_vel) / GPS_C
+                   + position_solution.clock_bias);
       /* TODO: Check sign of receiver frequency offset correction */
       if (time_quality >= TIME_FINE)
         dopp_uncertainty = DOPP_UNCERT_EPHEM;
@@ -646,6 +654,16 @@ u8 tracking_channels_ready()
   return n_ready;
 }
 
+void tracking_lock(void)
+{
+  chMtxTryLock(&tracking_startup_mutex);
+}
+
+void tracking_unlock(void)
+{
+  chMtxUnlock(&tracking_startup_mutex);
+}
+
 /** Queue a request to start up tracking and decoding for the specified sid.
  *
  * \note This function is thread-safe and non-blocking.
@@ -696,6 +714,8 @@ static void manage_tracking_startup(void)
       continue;
     }
 
+    tracking_lock();
+
     /* Start the tracking channel */
     if(!tracker_channel_init(chan, startup_params.sid,
                              startup_params.sample_count,
@@ -705,6 +725,9 @@ static void manage_tracking_startup(void)
                              TRACKING_ELEVATION_UNKNOWN)) {
       log_error("tracker channel init failed");
     }
+
+    tracking_unlock();
+
     /* TODO: Initialize elevation from ephemeris if we know it precisely */
 
     /* Start the decoder channel */

src/manage.h

@@ -83,4 +83,7 @@ u8 tracking_channels_ready(void);
 
 bool tracking_startup_request(const tracking_startup_params_t *startup_params);
 
+void tracking_lock(void);
+void tracking_unlock(void);
+
 #endif

src/signal.c

@@ -172,12 +172,16 @@ bool sid_supported(gnss_signal_t sid)
 bool code_supported(enum code code)
 {
   /* Verify general validity */
-  if (!code_valid(code))
+  if (!code_valid(code)) {
+      log_warn("---- ADEL debug %s:%d. code: %d", __FILE__, __LINE__, (int)code);
     return false;
+  }
 
   /* Verify that the code is supported on this platform */
-  if (code_signal_counts[code] == 0)
+  if (code_signal_counts[code] == 0) {
+    log_warn("---- ADEL debug %s:%d. code: %d", __FILE__, __LINE__, (int)code);
     return false;
+  }
 
   return true;
 }

src/signal.h

@@ -19,7 +19,7 @@
 
 /* Platform-specific code support ==== */
 #define CODE_GPS_L1CA_SUPPORT     1
-#define CODE_GPS_L2CM_SUPPORT     0
+#define CODE_GPS_L2CM_SUPPORT     1
 #define CODE_SBAS_L1CA_SUPPORT    1
 #define CODE_GLO_L1CA_SUPPORT     0
 #define CODE_GLO_L2CA_SUPPORT     0

src/simulator.c

@@ -346,7 +346,8 @@ void populate_nav_meas(navigation_measurement_t *nav_meas, double dist, double e
   nav_meas->raw_pseudorange += rand_gaussian(sim_settings.pseudorange_sigma *
                                              sim_settings.pseudorange_sigma);
 
-  nav_meas->carrier_phase =    dist / (GPS_C / GPS_L1_HZ);
+  nav_meas->carrier_phase =     dist / (GPS_C /
+            code_to_carr_freq(simulation_almanacs[almanac_i].sid.code));
   nav_meas->carrier_phase +=   simulation_fake_carrier_bias[almanac_i];
   nav_meas->carrier_phase +=   rand_gaussian(sim_settings.phase_sigma *
                                              sim_settings.phase_sigma);

src/solution.c

@@ -513,7 +513,7 @@ static void solution_thread(void *arg)
         /* Propagate observation to desired time. */
         for (u8 i=0; i<n_ready_tdcp; i++) {
           nav_meas_tdcp[i].pseudorange -= t_err * nav_meas_tdcp[i].doppler *
-            (GPS_C / GPS_L1_HZ);
+            (GPS_C / code_to_carr_freq(nav_meas_tdcp[i].sid.code));
           nav_meas_tdcp[i].carrier_phase += t_err * nav_meas_tdcp[i].doppler;
         }