WIP: Add Iono correction

Makefile

@@ -26,10 +26,12 @@ MAKEFLAGS += PIKSI_HW=$(PIKSI_HW)
 
 ifeq ($(PIKSI_HW),v2)
 	CMAKEFLAGS += -DCMAKE_SYSTEM_PROCESSOR=cortex-m4
+	CMAKEFLAGS += -DMAX_CHANNELS=11
 endif
 
 ifeq ($(PIKSI_HW),v3)
 	CMAKEFLAGS += -DCMAKE_SYSTEM_PROCESSOR=cortex-a9
+	CMAKEFLAGS += -DMAX_CHANNELS=31
 endif
 
 .PHONY: all tests firmware docs hitl_setup hitl hitlv3 .FORCE

src/Makefile

@@ -123,6 +123,8 @@ CSRC := $(STARTUPSRC) \
         $(SWIFTNAV_ROOT)/src/decode.o \
         $(SWIFTNAV_ROOT)/src/signal.o \
         $(SWIFTNAV_ROOT)/src/l2c_capb.o \
+        $(SWIFTNAV_ROOT)/src/iono.o \
+        $(SWIFTNAV_ROOT)/src/sid_set.o \
         main.c
 
 # C++ sources that can be compiled in ARM or THUMB mode depending on the global
@@ -159,6 +161,7 @@ INCDIR = $(STARTUPINC) $(PORTINC) $(KERNINC) \
          $(SWIFTNAV_ROOT)/libswiftnav/include \
          $(SWIFTNAV_ROOT)/src \
          $(SWIFTNAV_ROOT)/src/board \
+         $(SWIFTNAV_ROOT)/libswiftnav/libfec/include \
 
 #
 # Project, sources and paths
@@ -216,12 +219,13 @@ DLIBDIR = $(SWIFTNAV_ROOT)/libsbp/c/build/src \
           $(SWIFTNAV_ROOT)/libswiftnav/build/clapack-3.2.1-CMAKE/BLAS/SRC \
           $(SWIFTNAV_ROOT)/libswiftnav/build/clapack-3.2.1-CMAKE/SRC \
           $(SWIFTNAV_ROOT)/libswiftnav/build/clapack-3.2.1-CMAKE/F2CLIBS/libf2c \
+          $(SWIFTNAV_ROOT)/libswiftnav/build/libfec/src \
           $(SWIFTNAV_ROOT)/libswiftnav/build/src
 
 # List all default libraries here
 DLIBS = -lsbp-static -lswiftnav-static \
         -llapack -lcblas -lblas \
-        -lf2c -lm -lc -lnosys
+        -lf2c -lm -lc -lnosys -lfec
 
 #
 # End of default section

src/base_obs.c

@@ -35,6 +35,7 @@
 #include "base_obs.h"
 #include "ephemeris.h"
 #include "signal.h"
+#include "iono.h"
 
 extern bool disable_raim;
 
@@ -146,6 +147,21 @@ static void update_obss(obss_t *new_obss)
     gnss_solution soln;
     dops_t dops;
 
+    /* check if we have fix, if yes, calculate iono and tropo correction */
+    if(base_obss.has_pos) {
+      double llh[3];
+      wgsecef2llh(base_obss.pos_ecef, llh);
+      log_debug("Base: IONO/TROPO correction");
+      ionosphere_t i_params;
+      ionosphere_t *p_i_params = &i_params;
+      /* get iono parameters if available */
+      if(!gps_iono_params_read(p_i_params)) {
+        p_i_params = NULL;
+      }
+      calc_iono_tropo(base_obss.n, base_obss.nm, base_obss.pos_ecef, llh,
+                      p_i_params);
+    }
+
     /* Calculate a position solution. */
     /* disable_raim controlled by external setting (see solution.c). */
     s32 ret = calc_PVT(base_obss.n, base_obss.nm, disable_raim, &soln, &dops);

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]>
  *
@@ -27,10 +27,10 @@ extern u8 nap_track_n_channels;
 /** \} */
 
 void nap_track_init(u8 channel, gnss_signal_t sid, u32 ref_timing_count,
-                    float carrier_freq, float code_phase);
+                    float carrier_freq, float code_phase, u32 chips_to_correlate);
 
 void nap_track_update(u8 channel, double carrier_freq,
-                      double code_phase_rate, u8 rollover_count,
+                      double code_phase_rate, u32 chips_to_correlate,
                       u8 corr_spacing);
 void nap_track_read_results(u8 channel,
                             u32* count_snapshot, corr_t corrs[],

src/board/v2/nap/nap_hw.h

@@ -14,6 +14,7 @@
 #ifndef SWIFTNAV_NAP_HW_V2_H
 #define SWIFTNAV_NAP_HW_V2_H
 
+#include <libswiftnav/config.h>
 #include <libswiftnav/signal.h>
 
 /** \addtogroup nap
@@ -39,7 +40,7 @@
 #define NAP_HASH_NOTREADY           2
 
 /** Max number of tracking channels NAP configuration will be built with. */
-#define NAP_MAX_N_TRACK_CHANNELS     12
+#define NAP_MAX_N_TRACK_CHANNELS    (MAX_CHANNELS + 1)
 
 u8 nap_conf_done(void);
 u8 nap_hash_rd_done(void);

src/board/v2/nap/track_channel.c

@@ -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]>
  *
@@ -81,16 +81,19 @@ static struct nap_ch_state {
  * \param prn           CA code PRN (0-31) to track. (deprecated)
  * \param carrier_phase Initial code phase.
  * \param code_phase    Initial carrier phase.
+ * \param chips_to_correlate How many chips to correlate (unused)
  */
 void nap_track_init(u8 channel, gnss_signal_t sid, u32 ref_timing_count,
-                    float carrier_freq, float code_phase)
+                    float carrier_freq, float code_phase, u32 chips_to_correlate)
 {
   struct nap_ch_state *s = &nap_ch_state[channel];
   memset(s, 0, sizeof(*s));
 
+  (void) chips_to_correlate;
+
   u32 track_count = nap_timing_count() + 20000;
   float 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) */
@@ -133,17 +136,24 @@ void nap_track_init(u8 channel, gnss_signal_t sid, u32 ref_timing_count,
  * \param channel         NAP track channel whose UPDATE register to write.
  * \param carrier_freq    Next correlation period's carrier frequency.
  * \param code_phase_rate Next correlation period's code phase rate.
+ * \param chips_to_correlate How many chips to correlate over.
+ * \param corr_spacing    Spacing between correlator's EPL replicas.
  */
 void nap_track_update(u8 channel, double carrier_freq,
-                      double code_phase_rate, u8 rollover_count,
+                      double code_phase_rate, u32 chips_to_correlate,
                       u8 corr_spacing)
 {
+  u8 rollover_count;
   struct nap_ch_state *s = &nap_ch_state[channel];
   s->carr_pinc_prev = s->carr_pinc;
   s->carr_pinc = carrier_freq * NAP_TRACK_CARRIER_FREQ_UNITS_PER_HZ;
   s->code_pinc_prev = s->code_pinc;
   s->code_pinc = code_phase_rate * NAP_TRACK_CODE_PHASE_RATE_UNITS_PER_HZ;
 
+  rollover_count = (u8) (chips_to_correlate / GPS_L1CA_CHIPS_NUM);
+  if (rollover_count) {
+    rollover_count -= 1;
+  }
   nap_track_update_wr_blocking(channel, s->carr_pinc, s->code_pinc,
                                rollover_count, corr_spacing);
 }

src/board/v3/Makefile.inc

@@ -77,7 +77,9 @@ BOARDSRC := \
         $(BOARDDIR)/nap/track_channel.o \
         $(BOARDDIR)/platform_signal.o \
         $(SWIFTNAV_ROOT)/src/track/track_gps_l1ca.o \
+        $(SWIFTNAV_ROOT)/src/track/track_gps_l2cm.o \
         $(SWIFTNAV_ROOT)/src/decode/decode_gps_l1ca.o \
+        $(SWIFTNAV_ROOT)/src/decode/decode_gps_l2c.o \
 
 BOARDASM := \
         $(BOARDDIR)/cpu_init.s \

src/board/v3/nap/nap_common.c

@@ -47,7 +47,11 @@ void nap_setup(void)
   axi_dma_init();
   axi_dma_start(&AXIDMADriver1);
 
-  NAP->FE_PINC[0] = NAP_FE_BASEBAND_MIXER_PINC;
+  /* NAP_FE10_PINC initialization for GPS L1C/A processing */
+  NAP->FE_PINC[0] = NAP_FE_L1CA_BASEBAND_MIXER_PINC;
+
+  /* NAP_FE40_PINC initialization for GPS L2C processing */
+  NAP->FE_PINC[6] = NAP_FE_L2C_BASEBAND_MIXER_PINC;
 
   /* Enable NAP interrupt */
   chThdCreateStatic(wa_nap_exti, sizeof(wa_nap_exti), HIGHPRIO-1, nap_exti_thread, NULL);

src/board/v3/nap/nap_constants.h

@@ -44,7 +44,14 @@
  * @brief   The phase increment used to mix the frontend samples to baseband
  * @note    4294967296 is 2^32 and the .5 is for rounding
  */
-#define NAP_FE_BASEBAND_MIXER_PINC     (u32)(14.58e6 * 4294967296.0 \
+#define NAP_FE_L1CA_BASEBAND_MIXER_PINC   (u32)(14.58e6 * 4294967296.0 \
+                                          / NAP_FRONTEND_SAMPLE_RATE_Hz + 0.5)
+
+/**
+ * @brief   The phase increment used to mix the frontend samples to baseband
+ * @note    4294967296 is 2^32 and the .5 is for rounding
+ */
+#define NAP_FE_L2C_BASEBAND_MIXER_PINC    (u32)(7.4e6 * 4294967296.0 \
                                           / NAP_FRONTEND_SAMPLE_RATE_Hz + 0.5)
 
 #define NAP_KEY_LENGTH                                                   (16)

src/board/v3/nap/nap_hw.h

@@ -13,10 +13,11 @@
 #ifndef SWIFTNAV_NAP_REGS_H
 #define SWIFTNAV_NAP_REGS_H
 
+#include <libswiftnav/config.h>
 #include <stdint.h>
 
 /** Max number of tracking channels NAP configuration will be built with. */
-#define NAP_MAX_N_TRACK_CHANNELS     32
+#define NAP_MAX_N_TRACK_CHANNELS     (MAX_CHANNELS + 1)
 
 typedef struct {
   volatile uint32_t STATUS;
@@ -116,6 +117,31 @@ typedef struct {
 #define NAP_TRK_SPACING_INNER_Pos (16U)
 #define NAP_TRK_SPACING_INNER_Msk (0xFFFFU << NAP_TRK_SPACING_INNER_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)
+
+/* NAP RF fronend channel ID */
+enum {
+  NAP_RF_FRONTEND_CHANNEL_1 = 0, /* GPS L1 */
+  NAP_RF_FRONTEND_CHANNEL_2,     /* Glonass L1 */
+  NAP_RF_FRONTEND_CHANNEL_3,     /* Glonass L2 */
+  NAP_RF_FRONTEND_CHANNEL_4      /* GPS L2 */
+};
+
+/* The values of NAP_TRKx_CONTROL::CODE field */
+enum {
+  NAP_CODE_GPS_L1CA_SBAS_L1CA = 0, /* GPS L1C/A, SBAS L1CA */
+  NAP_CODE_GPS_L2CM                /* GPS L2CM */
+};
+
 /* Instances */
 #define NAP ((nap_t *)0x43C00000)