WIP: Add necessary changes for L2C track loop

include/libswiftnav/constants.h

@@ -37,6 +37,9 @@
 /** The GPS L1 center frequency in Hz. */
 #define GPS_L1_HZ 1.57542e9
 
+/** The GPS L2 center frequency in Hz. */
+#define GPS_L2_HZ 1.2276e9
+
 /** Earth's rotation rate as defined in the ICD in rad / s
  * \note This is actually not identical to the usual WGS84 definition. */
 #define GPS_OMEGAE_DOT 7.2921151467e-5
@@ -56,10 +59,6 @@
  * \note This is GPS_C / mu where mu is 1.0002926 */
 #define GPS_C_NO_VAC (GPS_C / 1.0002926)
 
-/** The wavelength of L1 in a vacuum.
- * \note This is GPS_C / GPS_L1_HZ. */
-#define GPS_L1_LAMBDA (GPS_C / GPS_L1_HZ)
-
 /** The wavelength of L1 in air at standard temperature and pressure.
  * \note This is GPS_C_NO_VAC / GPS_L1_HZ. */
 #define GPS_L1_LAMBDA_NO_VAC (GPS_C_NO_VAC / GPS_L1_HZ)

include/libswiftnav/signal.h

@@ -98,5 +98,7 @@ gnss_signal_t sid_from_code_index(code_t code, u16 code_index);
 u16 sid_to_code_index(gnss_signal_t sid);
 enum constellation sid_to_constellation(gnss_signal_t sid);
 enum constellation code_to_constellation(code_t code);
+double code_to_carr_freq(code_t code);
+u16 code_to_chip_num(code_t code);
 
 #endif /* LIBSWIFTNAV_SIGNAL_H */

src/almanac.c

@@ -238,7 +238,7 @@ double calc_sat_doppler_almanac(const almanac_t* alm, double t, s16 week,
                            vector_norm(3, vec_ref_sat);
 
   /* Return the Doppler shift. */
-  return GPS_L1_HZ * radial_velocity / GPS_C;
+  return code_to_carr_freq(alm->sid.code) * radial_velocity / GPS_C;
 }
 
 /** \} */

src/bit_sync.c

@@ -28,7 +28,8 @@
 /* Table of bit lengths for each code type */
 static const u8 bit_length_table[CODE_COUNT] = {
   [CODE_GPS_L1CA] = BIT_LENGTH_GPS_L1CA,
-  [CODE_GPS_L2CM] = 0,
+  [CODE_GPS_L2CM] = BIT_LENGTH_GPS_L1CA, /*same like in L1CA, because 
+                                          we use 2 symbols per bit*/
   [CODE_SBAS_L1CA] = BIT_LENGTH_SBAS_L1CA
 };
 

src/observation.c

@@ -218,7 +218,8 @@ u8 make_propagated_sdiffs(u8 n_local, navigation_measurement_t *m_local,
                             + dist_diff);
       sds[n].carrier_phase = m_local[i].carrier_phase
                            - (m_remote[j].carrier_phase
-                              - dist_diff / GPS_L1_LAMBDA);
+                             - dist_diff /
+                                (GPS_C/code_to_carr_freq(e[i]->sid.code)));
 
       /* Doppler is not propagated.
        * sds[n].doppler = m_local[i].raw_doppler - m_remote[j].raw_doppler; */

src/pvt.c

@@ -49,7 +49,8 @@ static double vel_solve(double rx_vel[],
     pdot_pred = -vector_dot(3, G[j], nav_meas[j]->sat_vel);
 
     /* The residual is due to the user's motion. */
-    tempvX[j] = -nav_meas[j]->doppler * GPS_C / GPS_L1_HZ - pdot_pred;
+    tempvX[j] = -nav_meas[j]->doppler * GPS_C /
+                  code_to_carr_freq(nav_meas[j]->sid.code)- pdot_pred;
   }
 
   /* Use X to map our pseudorange rate residuals onto the Jacobian update.

src/signal.c

@@ -15,6 +15,7 @@
 #include <assert.h>
 
 #include <libswiftnav/signal.h>
+#include <libswiftnav/constants.h>
 
 /** \defgroup signal GNSS signal identifiers (SID)
  * \{ */
@@ -164,4 +165,52 @@ constellation_t code_to_constellation(code_t code)
   return code_table[code].constellation;
 }
 
+/** Return the center carrier frequency for a code_t.
+ *
+ * \param code  code_t to use.
+ *
+ * \return center carrier frequency
+ */
+double code_to_carr_freq(code_t code)
+{
+  double f;
+  assert(code_valid(code));
+  switch (code) {
+    case CODE_GPS_L1CA:
+    case CODE_SBAS_L1CA:
+      f = GPS_L1_HZ;
+      break;
+    case CODE_GPS_L2CM:
+      f = GPS_L2_HZ;
+      break;
+    default:
+      f = 0.0;
+  }
+  return f;
+}
+
+/** Return the chips number for a code_t.
+ *
+ * \param code  code_t to use.
+ *
+ * \return chips number
+ */
+u16 code_to_chip_num(code_t code)
+{
+  u16 cn;
+  assert(code_valid(code));
+  switch (code) {
+    case CODE_GPS_L1CA:
+    case CODE_SBAS_L1CA:
+      cn = 1023;
+      break;
+    case CODE_GPS_L2CM:
+      cn = 10230;
+      break;
+    default:
+      cn = 0;
+  }
+  return cn;
+}
+
 /* \} */

src/track.c

@@ -802,7 +802,8 @@ void calc_navigation_measurement(u8 n_channels, const channel_measurement_t *mea
 
     nav_meas[i]->pseudorange = nav_meas[i]->raw_pseudorange \
                                + clock_err[i]*GPS_C;
-    nav_meas[i]->doppler = nav_meas[i]->raw_doppler + clock_rate_err[i]*GPS_L1_HZ;
+    nav_meas[i]->doppler = nav_meas[i]->raw_doppler +
+                clock_rate_err[i]*code_to_carr_freq(nav_meas[i]->sid.code);
 
     nav_meas[i]->tot.tow -= clock_err[i];
     normalize_gps_time(&nav_meas[i]->tot);