WIP: Port tracking loops for L2C

src/Makefile

@@ -117,6 +117,7 @@ CSRC := $(PORTSRC) \
         $(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/acq.o \
         $(SWIFTNAV_ROOT)/src/manage.o \
         $(SWIFTNAV_ROOT)/src/settings.o \

src/main.c

@@ -29,6 +29,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"
@@ -46,6 +47,8 @@
 
 extern void ext_setup(void);
 
+void track_gps_l2cm_register(void) TRK_WEAK;
+
 #if !defined(SYSTEM_CLOCK)
 #define SYSTEM_CLOCK 130944000
 #endif
@@ -186,6 +189,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

@@ -263,8 +263,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;
@@ -691,12 +692,14 @@ static void manage_tracking_startup(void)
     float cp = propagate_code_phase(startup_params.code_phase,
                                     startup_params.carrier_freq,
                                     track_count -
-                                        startup_params.sample_count);
+                                        startup_params.sample_count,
+                                    startup_params.sid.code);
 
     /* Contrive for the timing strobe to occur at or close to a
      * PRN edge (code phase = 0) */
     track_count += 16 * (1023.0-cp) *
-                      (1.0 + startup_params.carrier_freq / GPS_L1_HZ);
+                      (1.0 + startup_params.carrier_freq /
+                       code_to_carr_freq(startup_params.sid.code));
 
     /* Start the tracking channel */
     if(!tracker_channel_init(chan, startup_params.sid,

src/simulator.c

@@ -344,7 +344,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

@@ -524,7 +524,7 @@ static msg_t 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;
         }
 

src/track.c

@@ -120,7 +120,8 @@ static void interface_function(tracker_channel_t *tracker_channel,
                                tracker_interface_function_t func);
 static void event(tracker_channel_t *d, event_t event);
 static void common_data_init(tracker_common_data_t *common_data,
-                             u32 sample_count, float carrier_freq, float cn0);
+                             u32 sample_count, float carrier_freq,
+                             float cn0, code_t code);
 static void tracker_channel_lock(tracker_channel_t *tracker_channel);
 static void tracker_channel_unlock(tracker_channel_t *tracker_channel);
 
@@ -209,10 +210,11 @@ void tracking_send_state()
  *
  * \return The propagated code phase in chips.
  */
-float propagate_code_phase(float code_phase, float carrier_freq, u32 n_samples)
+float propagate_code_phase(float code_phase, float carrier_freq,
+                           u32 n_samples, code_t code)
 {
   /* Calculate the code phase rate with carrier aiding. */
-  u32 code_phase_rate = (1.0 + carrier_freq/GPS_L1_HZ) *
+  u32 code_phase_rate = (1.0 + carrier_freq/code_to_carr_freq(code)) *
                             NAP_TRACK_NOMINAL_CODE_PHASE_RATE;
 
   /* Internal Swift NAP code phase is in chips*2^32:
@@ -232,7 +234,8 @@ float propagate_code_phase(float code_phase, float carrier_freq, u32 n_samples)
    * NOTE: the modulo is required to fix the fact rollover should
    * occur at 1023 not 1024.
    */
-  return (float)((u32)(propagated_code_phase >> 28) % (1023*16)) / 16.0;
+  return (float)((u32)(propagated_code_phase >> 28) %
+                      (code_to_chip_num(code)*16)) / 16.0;
 }
 
 /** Handles pending IRQs for the specified tracking channels.
@@ -313,7 +316,7 @@ bool tracker_channel_init(tracker_channel_id_t id, gnss_signal_t sid,
     start_sample_count -= 0.5*16;
 
     common_data_init(&tracker_channel->common_data, start_sample_count,
-                     carrier_freq, cn0_init);
+                     carrier_freq, cn0_init, sid.code);
     internal_data_init(&tracker_channel->internal_data, sid);
     interface_function(tracker_channel, tracker_interface->init);
 
@@ -968,15 +971,16 @@ static void event(tracker_channel_t *tracker_channel, event_t event)
  * \param cn0               C/N0 estimate.
  */
 static void common_data_init(tracker_common_data_t *common_data,
-                             u32 sample_count, float carrier_freq, float cn0)
+                             u32 sample_count, float carrier_freq,
+                             float cn0, code_t code)
 {
   /* Initialize all fields to 0 */
   memset(common_data, 0, sizeof(tracker_common_data_t));
 
   common_data->TOW_ms = TOW_INVALID;
 
   /* Calculate code phase rate with carrier aiding. */
-  common_data->code_phase_rate = (1 + carrier_freq/GPS_L1_HZ) *
+  common_data->code_phase_rate = (1 + carrier_freq/code_to_carr_freq(code)) *
                                     GPS_CA_CHIPPING_RATE;
   common_data->carrier_freq = carrier_freq;
 

src/track.h

@@ -33,11 +33,19 @@ typedef u8 tracker_channel_id_t;
 
 /** \} */
 
+#define TRK_WEAK __attribute__ ((weak, alias ("trk_not_implemented")))
+void trk_not_implemented() __attribute__ ((weak));
+inline void trk_not_implemented()
+{
+  return;
+}
+
 void track_setup(void);
 
 void tracking_send_state(void);
 
-float propagate_code_phase(float code_phase, float carrier_freq, u32 n_samples);
+float propagate_code_phase(float code_phase, float carrier_freq,
+                           u32 n_samples, code_t code);
 
 /* Update interface */
 void tracking_channels_update(u32 channels_mask);

src/track_gps_l1ca.c

@@ -9,9 +9,12 @@
  * EITHER EXPRESSED OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE IMPLIED
  * WARRANTIES OF MERCHANTABILITY AND/OR FITNESS FOR A PARTICULAR PURPOSE.
  */
+ #define DEBUG 1
 
 #include "track_gps_l1ca.h"
 #include "track_api.h"
+#include "track.h"
+#include "decode.h"
 
 #include <libswiftnav/constants.h>
 #include <libswiftnav/logging.h>
@@ -368,6 +371,83 @@ static void tracker_gps_l1ca_update(const tracker_channel_info_t *channel_info,
 
     /* Indicate that a mode change has occurred. */
     common_data->mode_change_count = common_data->update_count;
+
+    /* Transition to L2C tracking. */
+    /* Since we have bitsync do handover to L2C if availble for the SV */
+    /* First, get L2C capability for the SV from NDB */
+     u32 l2c_cpbl;
+     ndb_gps_l2cm_l2c_cap_read(&l2c_cpbl);
+     if (l2c_cpbl & (1 << channel_info->sid.sat)) {
+       /*find available tracking channel first*/
+       s16 tk_ch = -1;
+
+       /* compose SID: same SV, but code is L2CA */
+       gnss_signal_t sid = {.sat = channel_info->sid.sat,
+                            .code = CODE_GPS_L2CM};
+       for (u8 i=0; i<nap_track_n_channels; i++)
+         if (tracker_channel_available(i, sid) &&
+             decoder_channel_available(i, sid)) {
+           tk_ch = i;
+           break;
+         }
+
+       if (tk_ch > -1) {
+         /* free tracking channel found */
+         u32 track_count = nap_timing_count() + 20000;
+
+         /* recalculate doppler freq for L2 from L1*/
+         double carr_freq = tracking_channel_carrier_freq_get(
+                                            channel_info->nap_channel) *
+                            GPS_L2_HZ / GPS_L1_HZ;
+
+         log_debug("L2C Dopp %f, parent Dopp %f",
+                  carr_freq, common_data->carrier_freq);
+
+         /* recalculate code phase */
+         float cp = propagate_code_phase(common_data->code_phase_early,
+                                         carr_freq,
+                                         track_count -
+                                             common_data->sample_count,
+                                         CODE_GPS_L2CM);
+
+         log_debug("L2C CP %f, parent CP %f",
+                  cp, common_data->code_phase_early);
+
+         /* Contrive for the timing strobe to occur at or close to a
+          * PRN edge (code phase = 0) */
+         track_count += 16 * (10230.0-cp) *
+                           (1.0 + carr_freq /
+                               code_to_carr_freq(CODE_GPS_L2CM));
+
+         /* get initial cn0 from parent L1 channel*/
+         float cn0 = tracking_channel_cn0_get(channel_info->nap_channel);
+
+         /* Start the tracking channel */
+         if (!tracker_channel_init((tracker_channel_id_t)tk_ch,
+                                  sid,
+                                  carr_freq,
+                                  track_count,
+                                  cn0,
+                                  tracking_channel_evelation_degrees_get(
+                                          channel_info->nap_channel))) {
+           log_error("tracker channel init for L2C failed");
+         } else
+           log_info("L2C handover done. Tracking channel %u, parent chnnel %u",
+             (u8)tk_ch,
+             channel_info->nap_channel);
+
+
+         /* TODO: Initialize elevation from ephemeris if we know it precisely */
+         /* Start the decoder channel */
+         if (!decoder_channel_init((u8)tk_ch, sid)) {
+           log_error("decoder channel init for L2C failed");
+         }
+       }
+       else
+         log_warn("No free channel for L2C tracking");
+       /* init tracking channel for L2C */
+     } else
+       log_info("SV %u does not support L2C stream", channel_info->sid.sat);
   }
 
   tracker_retune(channel_info->context, data->carrier_freq_fp,