diff --git a/src/Makefile b/src/Makefile index 1a7128d655..c6818a9ac8 100644 --- a/src/Makefile +++ b/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 diff --git a/src/base_obs.c b/src/base_obs.c index 729bb995dd..de3e407fdc 100644 --- a/src/base_obs.c +++ b/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); diff --git a/src/decode/decode_gps_l1ca.c b/src/decode/decode_gps_l1ca.c index 72e26acea2..bfd3dd059c 100644 --- a/src/decode/decode_gps_l1ca.c +++ b/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); diff --git a/src/iono.c b/src/iono.c new file mode 100644 index 0000000000..0e7969901d --- /dev/null +++ b/src/iono.c @@ -0,0 +1,52 @@ +/* + * Copyright (C) 2016 Swift Navigation Inc. + * Contact: Dmitry Tatarinov + * + * 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 +#include +#include + +#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; +} diff --git a/src/iono.h b/src/iono.h new file mode 100644 index 0000000000..46172feaf1 --- /dev/null +++ b/src/iono.h @@ -0,0 +1,22 @@ +/* + * Copyright (C) 2016 Swift Navigation Inc. + * Contact: Dmitry Tatarinov + * + * 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 +#include + +void gps_iono_params_store(const ionosphere_t *params); +u8 gps_iono_params_read(ionosphere_t *params); + +#endif /* SRC_IONO_H_ */ diff --git a/src/solution.c b/src/solution.c index ed467f4705..42b3b12df8 100644 --- a/src/solution.c +++ b/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 #include @@ -23,6 +22,7 @@ #include #include #include +#include #define memory_pool_t MemoryPool #include @@ -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"); diff --git a/src/track/track_gps_l1ca.c b/src/track/track_gps_l1ca.c index f6382c2565..10d2cc238c 100644 --- a/src/track/track_gps_l1ca.c +++ b/src/track/track_gps_l1ca.c @@ -49,7 +49,7 @@ #define LD_PARAMS_EXTRAOPT "0.02, 0.8, 150, 50" #define LD_PARAMS_DISABLE "0.02, 1e-6, 1, 1" -#define CN0_EST_LPF_CUTOFF 5 +#define CN0_EST_LPF_CUTOFF 0.1 #define INTEG_PERIOD_1_MS 1 #define INTEG_PERIOD_2_MS 2 @@ -86,7 +86,7 @@ static struct lock_detect_params { u16 lp, lo; } lock_detect_params; -static float track_cn0_use_thres = 31.0; /* dBHz */ +static float track_cn0_use_thres = 37.0; /* dBHz */ static float track_cn0_drop_thres = 31.0; static char loop_params_string[120] = LOOP_PARAMS_MED; diff --git a/src/track/track_gps_l2cm.c b/src/track/track_gps_l2cm.c index 2dcbfad673..1b92ccec1e 100644 --- a/src/track/track_gps_l2cm.c +++ b/src/track/track_gps_l2cm.c @@ -62,7 +62,7 @@ #define LD_PARAMS "0.0247, 1.5, 50, 240" #define LD_PARAMS_DISABLE "0.02, 1e-6, 1, 1" -#define CN0_EST_LPF_CUTOFF 5 +#define CN0_EST_LPF_CUTOFF 0.1f #define INTEG_PERIOD_20_MS 20 @@ -86,7 +86,7 @@ static struct lock_detect_params { u16 lp, lo; } lock_detect_params; -static float track_cn0_use_thres = 31.0; /* dBHz */ +static float track_cn0_use_thres = 37.0; /* dBHz */ static float track_cn0_drop_thres = 31.0; /* dBHz */ static char loop_params_string[120] = LOOP_PARAMS_MED;