Add Iono and Tropo correction usage

Author: Dmitry Tatarinov

src/Makefile

@@ -123,6 +123,7 @@ 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
 

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/decode/decode_gps_l1ca.c

@@ -23,6 +23,7 @@
 #include "sbp_utils.h"
 #include "signal.h"
 #include "l2c_capb.h"
+#include "iono.h"
 
 typedef struct {
   nav_msg_t nav_msg;
@@ -115,6 +116,12 @@ static void decoder_gps_l1ca_process(const decoder_channel_info_t *channel_info,
     gps_l2cm_l2c_cap_store(dd.gps_l2c_sv_capability);
   }
 
+  if (dd.iono_corr_upd_flag) {
+    /* store new iono parameters */
+    log_debug("Iono parameters received");
+    gps_iono_params_store(&dd.iono);
+  }
+
   if(dd.ephemeris_upd_flag) {
     /* Decoded a new ephemeris. */
     ephemeris_new(&dd.ephemeris);

src/iono.c

@@ -0,0 +1,52 @@
+/*
+ * Copyright (C) 2016 Swift Navigation Inc.
+ * Contact: Dmitry Tatarinov <[email protected]>
+ *
+ * This source is subject to the license found in the file 'LICENSE' which must
+ * be be distributed together with this source. All other rights reserved.
+ *
+ * THIS CODE AND INFORMATION IS PROVIDED "AS IS" WITHOUT WARRANTY OF ANY KIND,
+ * EITHER EXPRESSED OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE IMPLIED
+ * WARRANTIES OF MERCHANTABILITY AND/OR FITNESS FOR A PARTICULAR PURPOSE.
+ */
+
+#include <assert.h>
+#include <string.h>
+#include <ch.h>
+
+#include "iono.h"
+#include "signal.h"
+
+static MUTEX_DECL(iono_mutex);
+
+static ionosphere_t iono_params;
+static bool iono_params_decoded_flag = false;
+
+/** Stores ionospheric parameters
+ * \param params pointer to ionospheric parameters to be stored
+ */
+void gps_iono_params_store(const ionosphere_t *params)
+{
+  assert(params != NULL);
+  chMtxLock(&iono_mutex);
+  memcpy(&iono_params, params, sizeof(ionosphere_t));
+  iono_params_decoded_flag = true;
+  chMtxUnlock(&iono_mutex);
+}
+
+/** Reads ionospheric parameters
+ * \param params pointer to ionospheric
+ * \return 1 if iono parameters available otherwise 0
+ */
+u8 gps_iono_params_read(ionosphere_t *params)
+{
+  u8 result = 0;
+  assert(params != NULL);
+  chMtxLock(&iono_mutex);
+  if (iono_params_decoded_flag) {
+    memcpy(params, &iono_params, sizeof(ionosphere_t));
+    result = 1;
+  }
+  chMtxUnlock(&iono_mutex);
+  return result;
+}

src/iono.h

@@ -0,0 +1,22 @@
+/*
+ * Copyright (C) 2016 Swift Navigation Inc.
+ * Contact: Dmitry Tatarinov <[email protected]>
+ *
+ * This source is subject to the license found in the file 'LICENSE' which must
+ * be be distributed together with this source. All other rights reserved.
+ *
+ * THIS CODE AND INFORMATION IS PROVIDED "AS IS" WITHOUT WARRANTY OF ANY KIND,
+ * EITHER EXPRESSED OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE IMPLIED
+ * WARRANTIES OF MERCHANTABILITY AND/OR FITNESS FOR A PARTICULAR PURPOSE.
+ */
+
+#ifndef SRC_IONO_H_
+#define SRC_IONO_H_
+
+#include <libswiftnav/common.h>
+#include <libswiftnav/ionosphere.h>
+
+void gps_iono_params_store(const ionosphere_t *params);
+u8 gps_iono_params_read(ionosphere_t *params);
+
+#endif /* SRC_IONO_H_ */

src/solution.c

@@ -9,7 +9,6 @@
  * EITHER EXPRESSED OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE IMPLIED
  * WARRANTIES OF MERCHANTABILITY AND/OR FITNESS FOR A PARTICULAR PURPOSE.
  */
-
 #include <stdio.h>
 #include <string.h>
 
@@ -23,6 +22,7 @@
 #include <libswiftnav/dgnss_management.h>
 #include <libswiftnav/baseline.h>
 #include <libswiftnav/linear_algebra.h>
+#include <libswiftnav/troposphere.h>
 
 #define memory_pool_t MemoryPool
 #include <ch.h>
@@ -43,6 +43,7 @@
 #include "signal.h"
 #include "system_monitor.h"
 #include "main.h"
+#include "iono.h"
 #include "sid_set.h"
 
 /* Maximum CPU time the solution thread is allowed to use. */
@@ -433,6 +434,9 @@ static void sol_thd_sleep(systime_t *deadline, systime_t interval)
 static THD_WORKING_AREA(wa_solution_thread, 8000);
 static void solution_thread(void *arg)
 {
+  /* The flag is true when we have a fix */
+  bool soln_flag = false;
+
   (void)arg;
   chRegSetThreadName("solution");
 
@@ -534,6 +538,20 @@ static void solution_thread(void *arg)
       continue;
     }
 
+    /* check if we have a solution, if yes calc iono and tropo correction */
+    if (soln_flag) {
+      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(n_ready_tdcp, nav_meas_tdcp,
+                      position_solution.pos_ecef,
+                      position_solution.pos_llh,
+                      p_i_params);
+    }
+
     dops_t dops;
     /* Calculate the SPP position
      * disable_raim controlled by external setting. Defaults to false. */
@@ -548,11 +566,15 @@ static void solution_thread(void *arg)
         log_warn("PVT solver: %s (code %d)", pvt_err_msg[-pvt_ret-1], pvt_ret);
       );
 
+      soln_flag = false;
+
       /* Send just the DOPs and exit the loop */
       solution_send_sbp(0, &dops, clock_jump);
       continue;
     }
 
+    soln_flag = true;
+
     if (pvt_ret == 1)
 	  log_warn("calc_PVT: RAIM repair");