Merge pull request #15 from swift-nav/pr-721-copy

Fix CN0 filter for L2CM. Verified on VV-1606301020.

Author: valeri-atamaniouk

libswiftnav

@@ -290,7 +290,8 @@ static void tracker_gps_l1ca_update(const tracker_channel_info_t *channel_info,
 
     common_data->cn0 = cn0_est(&data->cn0_est,
                                pparams,
-                               cs[1].I/data->int_ms, cs[1].Q/data->int_ms);
+                               (float) cs[1].I/data->int_ms,
+                               (float) cs[1].Q/data->int_ms);
   }
 
   if (common_data->cn0 > track_cn0_drop_thres)

src/track/track_gps_l1ca.c

@@ -64,6 +64,17 @@
 
 #define CN0_EST_LPF_CUTOFF 5
 
+#define INTEG_PERIOD_20_MS 20
+
+static const u8 integration_periods[] = {
+  INTEG_PERIOD_20_MS
+};
+
+#define INTEG_PERIODS_NUM (sizeof(integration_periods) / \
+                           sizeof(integration_periods[0]))
+
+static cn0_est_params_t cn0_est_pre_computed[INTEG_PERIODS_NUM];
+
 static struct loop_params {
   float code_bw, code_zeta, code_k, carr_to_code;
   float carr_bw, carr_zeta, carr_k, carr_fll_aid_gain;
@@ -112,6 +123,7 @@ static void tracker_gps_l2cm_update(const tracker_channel_info_t *channel_info,
 
 static bool parse_loop_params(struct setting *s, const char *val);
 static bool parse_lock_detect_params(struct setting *s, const char *val);
+static void precompute_cn0_est_params(void);
 
 static const tracker_interface_t tracker_interface_gps_l2cm = {
   .code =         CODE_GPS_L2CM,
@@ -155,6 +167,8 @@ void track_gps_l2cm_register(void)
     gps_l2cm_trackers[i].data = &gps_l2cm_tracker_data[i];
   }
 
+  precompute_cn0_est_params();
+
   tracker_interface_register(&tracker_interface_list_element_gps_l2cm);
 }
 
@@ -425,9 +439,33 @@ static void tracker_gps_l2cm_update(const tracker_channel_info_t *channel_info,
   corr_t* cs = data->cs;
 
   /* Update C/N0 estimate */
-  common_data->cn0 = cn0_est(&data->cn0_est,
-                            cs[1].I / data->int_ms,
-                            cs[1].Q / data->int_ms);
+  {
+    cn0_est_params_t params;
+    const cn0_est_params_t *pparams = NULL;
+
+    /* TODO
+     * Store a pointer to the cn0_est_params_t in the gps_l1ca_tracker_data_t
+     * structure so we don't have to scan through the whole array each time
+     */
+    for(u32 i = 0; i < INTEG_PERIODS_NUM; i++) {
+      if(data->int_ms == integration_periods[i]) {
+        pparams = &cn0_est_pre_computed[i];
+        break;
+      }
+    }
+
+    if(NULL == pparams) {
+      cn0_est_compute_params(&params, 1e3f / data->int_ms, CN0_EST_LPF_CUTOFF,
+                             1e3f / data->int_ms);
+      pparams = &params;
+    }
+
+    common_data->cn0 = cn0_est(&data->cn0_est,
+                               pparams,
+                               (float) cs[1].I/data->int_ms,
+                               (float) cs[1].Q/data->int_ms);
+  }
+
   if (common_data->cn0 > track_cn0_drop_thres) {
     common_data->cn0_above_drop_thres_count = common_data->update_count;
   }
@@ -570,3 +608,17 @@ static bool parse_lock_detect_params(struct setting *s, const char *val)
 
   return true;
 }
+
+/* Pre-compute C/N0 estimator and filter parameters. The parameters are
+ * computed using equivalent of cn0_est_compute_params() function for
+ * integration periods and cut-off frequency defined in this file.
+ */
+static void precompute_cn0_est_params(void)
+{
+  for(u32 i = 0; i < INTEG_PERIODS_NUM; i++) {
+    cn0_est_compute_params(&cn0_est_pre_computed[i],
+                           1e3f / integration_periods[i],
+                           CN0_EST_LPF_CUTOFF,
+                           1e3f / integration_periods[i]);
+  }
+}