From fe5fa67885f006c4bbb1fe57206f25a040fd971a Mon Sep 17 00:00:00 2001 From: Scott Kovach Date: Tue, 10 Nov 2015 12:28:43 -0800 Subject: [PATCH] update for libswiftnav dgnss_state refactor --- libswiftnav | 2 +- src/STM32F405xG.ld | 1 - src/solution.c | 25 ++++++++++++++----------- src/solution.h | 3 +++ src/system_monitor.c | 3 ++- 5 files changed, 20 insertions(+), 14 deletions(-) diff --git a/libswiftnav b/libswiftnav index f8f5c85b..7f2f7ce9 160000 --- a/libswiftnav +++ b/libswiftnav @@ -1 +1 @@ -Subproject commit f8f5c85ba3d7a4201d7690116e30ef30829d61d1 +Subproject commit 7f2f7ce9d9b5d22b4d89794380ef264a91ec9ae1 diff --git a/src/STM32F405xG.ld b/src/STM32F405xG.ld index d6c6cb24..377084d5 100644 --- a/src/STM32F405xG.ld +++ b/src/STM32F405xG.ld @@ -122,7 +122,6 @@ SECTIONS .ccmram (NOLOAD) : { *(.ccmram*) - *(.bss.nkf) } > ccmram .data : diff --git a/src/solution.c b/src/solution.c index 9bc1494e..2f779feb 100644 --- a/src/solution.c +++ b/src/solution.c @@ -56,6 +56,8 @@ systime_t last_dgnss; double soln_freq = 10.0; u32 obs_output_divisor = 2; +dgnss_state_t dgnss_state _CCM; + double known_baseline[3] = {0, 0, 0}; u16 msg_obs_max_size = 104; @@ -593,7 +595,8 @@ void process_matched_obs(u8 n_sds, gps_time_t *t, sdiff_t *sds) double known_baseline_ecef[3]; wgsned2ecef(known_baseline, position_solution.pos_ecef, known_baseline_ecef); - dgnss_init_known_baseline(n_sds, sds, position_solution.pos_ecef, + dgnss_init_known_baseline(&dgnss_state, n_sds, sds, + position_solution.pos_ecef, known_baseline_ecef); init_known_base = false; } else { @@ -604,7 +607,7 @@ void process_matched_obs(u8 n_sds, gps_time_t *t, sdiff_t *sds) if (n_sds > 4) { /* Initialize filters. */ log_info("Initializing DGNSS filters"); - dgnss_init(n_sds, sds, position_solution.pos_ecef); + dgnss_init(&dgnss_state, n_sds, sds, position_solution.pos_ecef); /* Initialize ambiguity states. */ ambiguities_init(&amb_state.fixed_ambs); ambiguities_init(&amb_state.float_ambs); @@ -612,15 +615,15 @@ void process_matched_obs(u8 n_sds, gps_time_t *t, sdiff_t *sds) } } else { if (reset_iar) { - dgnss_reset_iar(); + dgnss_reset_iar(&dgnss_state); reset_iar = false; } /* Update filters. */ - dgnss_update(n_sds, sds, position_solution.pos_ecef, + dgnss_update(&dgnss_state, n_sds, sds, position_solution.pos_ecef, disable_raim, DEFAULT_RAIM_THRESHOLD); /* Update ambiguity states. */ chMtxLock(&amb_state_lock); - dgnss_update_ambiguity_state(&amb_state); + dgnss_update_ambiguity_state(&dgnss_state, &amb_state); chMtxUnlock(); /* If we are in time matched mode then calculate and output the baseline * for this observation. */ @@ -773,13 +776,13 @@ void solution_setup() SETTING("solution", "known_baseline_e", known_baseline[1], TYPE_FLOAT); SETTING("solution", "known_baseline_d", known_baseline[2], TYPE_FLOAT); - SETTING("iar", "phase_var", dgnss_settings.phase_var_test, TYPE_FLOAT); - SETTING("iar", "code_var", dgnss_settings.code_var_test, TYPE_FLOAT); + SETTING("iar", "phase_var", dgnss_state.settings.phase_var_test, TYPE_FLOAT); + SETTING("iar", "code_var", dgnss_state.settings.code_var_test, TYPE_FLOAT); - SETTING("float_kf", "phase_var", dgnss_settings.phase_var_kf, TYPE_FLOAT); - SETTING("float_kf", "code_var", dgnss_settings.code_var_kf, TYPE_FLOAT); - SETTING("float_kf", "amb_init_var", dgnss_settings.amb_init_var, TYPE_FLOAT); - SETTING("float_kf", "new_amb_var", dgnss_settings.new_int_var, TYPE_FLOAT); + SETTING("float_kf", "phase_var", dgnss_state.settings.phase_var_kf, TYPE_FLOAT); + SETTING("float_kf", "code_var", dgnss_state.settings.code_var_kf, TYPE_FLOAT); + SETTING("float_kf", "amb_init_var", dgnss_state.settings.amb_init_var, TYPE_FLOAT); + SETTING("float_kf", "new_amb_var", dgnss_state.settings.new_int_var, TYPE_FLOAT); SETTING("sbp", "obs_msg_max_size", msg_obs_max_size, TYPE_INT); diff --git a/src/solution.h b/src/solution.h index 62120c9f..6a5dd51b 100644 --- a/src/solution.h +++ b/src/solution.h @@ -18,6 +18,7 @@ #include #include #include +#include typedef enum { SOLN_MODE_LOW_LATENCY, @@ -43,6 +44,8 @@ typedef enum { extern double soln_freq; extern u32 obs_output_divisor; +extern dgnss_state_t dgnss_state; + void solution_send_sbp(gnss_solution *soln, dops_t *dops); void solution_send_nmea(gnss_solution *soln, dops_t *dops, u8 n, navigation_measurement_t *nm, diff --git a/src/system_monitor.c b/src/system_monitor.c index cb769c33..18a479ce 100644 --- a/src/system_monitor.c +++ b/src/system_monitor.c @@ -28,6 +28,7 @@ #include "main.h" #include "sbp.h" #include "manage.h" +#include "solution.h" #include "simulator.h" #include "system_monitor.h" #include "position.h" @@ -191,7 +192,7 @@ static msg_t system_monitor_thread(void *arg) if (simulation_enabled_for(SIMULATION_MODE_RTK)) { iar_state.num_hyps = 1; } else { - iar_state.num_hyps = dgnss_iar_num_hyps(); + iar_state.num_hyps = dgnss_iar_num_hyps(&dgnss_state); } sbp_send_msg(SBP_MSG_IAR_STATE, sizeof(msg_iar_state_t), (u8 *)&iar_state);