diff --git a/include/libswiftnav/dgnss_management.h b/include/libswiftnav/dgnss_management.h index 657187f8..4744e607 100644 --- a/include/libswiftnav/dgnss_management.h +++ b/include/libswiftnav/dgnss_management.h @@ -39,6 +39,19 @@ typedef struct { ambiguities_t float_ambs; } ambiguity_state_t; +typedef struct { + /* Baseline estimate */ + double b[3]; + /* Number of sdiffs used in the soluton. */ + u8 num_used; + /* bool: fixed or float filter derived */ + bool fixed_mode; + /* bool: raim available for least squares baseline estimate */ + bool raim_available; + /* bool: raim used to repair baseline */ + bool raim_repair; +} dgnss_baseline_t; + extern dgnss_settings_t dgnss_settings; void dgnss_set_settings(double phase_var_test, double code_var_test, @@ -62,9 +75,10 @@ s8 dgnss_iar_get_single_hyp(double *hyp); void dgnss_reset_iar(void); void dgnss_init_known_baseline(u8 num_sats, sdiff_t *sdiffs, double receiver_ecef[3], double b[3]); void dgnss_update_ambiguity_state(ambiguity_state_t *s); +void fill_property_flags(s8 ret, bool fixed, dgnss_baseline_t *solution); s8 dgnss_baseline(u8 num_sdiffs, const sdiff_t *sdiffs, const double ref_ecef[3], const ambiguity_state_t *s, - u8 *num_used, double b[3], + dgnss_baseline_t *solution, bool disable_raim, double raim_threshold); void measure_amb_kf_b(u8 num_sdiffs, sdiff_t *sdiffs, const double receiver_ecef[3], double *b); diff --git a/include/libswiftnav/pvt.h b/include/libswiftnav/pvt.h index 7f4c490b..cb342ef0 100644 --- a/include/libswiftnav/pvt.h +++ b/include/libswiftnav/pvt.h @@ -77,6 +77,10 @@ typedef struct __attribute__((packed)) { u8 valid; /* Number of channels used in the soluton. */ u8 n_used; + /* bit 0: raim available? + * bit 1: raim repair used? + */ + u8 raim_flag; } gnss_solution; s8 calc_PVT(const u8 n_used, diff --git a/src/dgnss_management.c b/src/dgnss_management.c index 80625cdf..f7f94a28 100644 --- a/src/dgnss_management.c +++ b/src/dgnss_management.c @@ -395,6 +395,14 @@ void dgnss_update_ambiguity_state(ambiguity_state_t *s) } } +/* Interpret raim-related content of baseline function return code. */ +void fill_property_flags(s8 ret, bool fixed, dgnss_baseline_t *solution) +{ + solution->raim_repair = ret == 1; + solution->raim_available = ret != 2; + solution->fixed_mode = fixed; +} + /** Finds the baseline using low latency sdiffs. * The low latency sdiffs are not guaranteed to match up with either the * amb_test's or the float sdiffs, and thus care must be taken to transform them @@ -405,42 +413,51 @@ void dgnss_update_ambiguity_state(ambiguity_state_t *s) * \param ref_ecef The referece position for the baseline. * (TODO is this the local or remote receiver position?) * \param s Current ambiguity test state. - * \param num_used Output number of sdiffs actually used in the baseline - * estimate. - * \param b Output baseline. + * \param solution struct containing output baseline, num sdiffs used, + * fixed mode, raim availability, whether raim was used. * \param disable_raim Flag to turn off raim checks/repair. * \param raim_threshold raim check threshold - * \return 1 if we are using an IAR resolved baseline. - * 2 if we are using a float baseline. - * -1 if we can't give a baseline. + * \return 0: solution ok + * negative value: baseline error code */ s8 dgnss_baseline(u8 num_sdiffs, const sdiff_t *sdiffs, const double ref_ecef[3], const ambiguity_state_t *s, - u8 *num_used, double b[3], + dgnss_baseline_t *solution, bool disable_raim, double raim_threshold) { - s8 ret = baseline(num_sdiffs, sdiffs, ref_ecef, &s->fixed_ambs, num_used, b, - disable_raim, raim_threshold); + s8 ret; + + /* Try IAR resolved baseline */ + ret = baseline(num_sdiffs, sdiffs, ref_ecef, &s->fixed_ambs, + &solution->num_used, solution->b, + disable_raim, raim_threshold); if (ret >= 0) { - if (ret == 1) /* TODO: Export this rather than just printing */ + if (ret == 1) log_warn("dgnss_baseline: Fixed baseline RAIM repair"); log_debug("fixed solution"); DEBUG_EXIT(); - return 1; + fill_property_flags(ret, true, solution); + return 0; } + /* We weren't able to get an IAR resolved baseline, check if we can get a * float baseline. */ - if ((ret = baseline(num_sdiffs, sdiffs, ref_ecef, &s->float_ambs, num_used, b, - disable_raim, raim_threshold)) - >= 0) { - if (ret == 1) /* TODO: Export this rather than just printing */ + ret = baseline(num_sdiffs, sdiffs, ref_ecef, &s->float_ambs, + &solution->num_used, solution->b, + disable_raim, raim_threshold); + if (ret >= 0) { + if (ret == 1) log_warn("dgnss_baseline: Float baseline RAIM repair"); log_debug("float solution"); DEBUG_EXIT(); - return 2; + fill_property_flags(ret, false, solution); + return 0; } log_debug("no baseline solution"); DEBUG_EXIT(); + + /* Must be negative! */ + assert(ret < 0); return ret; } diff --git a/src/pvt.c b/src/pvt.c index b4629322..278f61cc 100644 --- a/src/pvt.c +++ b/src/pvt.c @@ -536,17 +536,31 @@ s8 calc_PVT(const u8 n_used, soln->n_used = n_used; // Keep track of number of working channels u8 removed_prn = -1; - s8 raim_flag = pvt_solve_raim(rx_state, n_used, nav_meas, disable_raim, - H, &removed_prn, 0); + s8 code = pvt_solve_raim(rx_state, n_used, nav_meas, disable_raim, + H, &removed_prn, 0); - if (raim_flag < 0) { + if (code < 0) { /* Didn't converge or least squares integrity check failed. */ - return raim_flag - 3; + return code - 3; } - /* Initial solution failed, but repair was successful. */ - if (raim_flag == 1) { - soln->n_used--; + soln->raim_flag = 0; + + switch (code) { + case 1: + /* Initial solution failed, but repair was successful. */ + soln->n_used--; + soln->raim_flag = (1 << 1) | 1; /* 11 */ + break; + case 2: + /* raim was unavailable */ + soln->raim_flag = 0; /* 00 */ + break; + case 0: + soln->raim_flag = 1; /* 01 */ + break; + default: + log_error("Invalid pvt_solve_raim return code: %d\n", code); } /* Compute various dilution of precision metrics. */ @@ -597,6 +611,6 @@ s8 calc_PVT(const u8 n_used, soln->valid = 1; - return raim_flag; + return code; } diff --git a/tests/check_dgnss_management.c b/tests/check_dgnss_management.c index 7cce1799..f5750881 100644 --- a/tests/check_dgnss_management.c +++ b/tests/check_dgnss_management.c @@ -5,6 +5,7 @@ #include "check_utils.h" #include "dgnss_management.h" #include "ambiguity_test.h" +#include "printing_utils.h" #include "amb_kf.h" #include "printing_utils.h" @@ -205,39 +206,38 @@ START_TEST(test_dgnss_baseline_1) } }; - double b[3]; - u8 num_used; + dgnss_baseline_t solution; /* Float only */ s.fixed_ambs.n = 0; - s8 valid = dgnss_baseline(num_sdiffs, sdiffs, ref_ecef, &s, &num_used, b, + s8 valid = dgnss_baseline(num_sdiffs, sdiffs, ref_ecef, &s, &solution, false, DEFAULT_RAIM_THRESHOLD); - fail_unless(valid == 2); - fail_unless(num_used == 5); - fail_unless(within_epsilon(b[0], -0.742242)); - fail_unless(within_epsilon(b[1], -0.492905)); - fail_unless(within_epsilon(b[2], -0.0533294)); + /* baseline_flag(initial solution ok, float mode) */ + fail_unless(valid == 0); + fail_unless(solution.fixed_mode == 0); + fail_unless(solution.num_used == 5); + double expected1[] = {-0.742242, -0.492905, -0.0533294}; + fail_unless(arr_within_epsilon(3, solution.b, expected1)); /* Fixed and float */ s.fixed_ambs.n = 4; - valid = dgnss_baseline(num_sdiffs, sdiffs, ref_ecef, &s, &num_used, b, + valid = dgnss_baseline(num_sdiffs, sdiffs, ref_ecef, &s, &solution, false, DEFAULT_RAIM_THRESHOLD); - fail_unless(valid == 1); - fail_unless(num_used == 5); - fail_unless(within_epsilon(b[0], -0.417486)); - fail_unless(within_epsilon(b[1], -0.358386)); - fail_unless(within_epsilon(b[2], 0.271427)); + /* baseline_flag(initial solution ok, fixed mode) */ + fail_unless(valid == 0); + fail_unless(solution.fixed_mode == 1); + fail_unless(solution.num_used == 5); + double expected2[] = {-0.417486, -0.358386, 0.271427}; + fail_unless(arr_within_epsilon(3, solution.b, expected2)); /* No solution possible */ s.fixed_ambs.n = 0; s.float_ambs.n = 0; - valid = dgnss_baseline(num_sdiffs, sdiffs, ref_ecef, &s, &num_used, b, + valid = dgnss_baseline(num_sdiffs, sdiffs, ref_ecef, &s, &solution, false, DEFAULT_RAIM_THRESHOLD); fail_unless(valid == -1); - fail_unless(num_used == 5); - fail_unless(within_epsilon(b[0], -0.417486)); - fail_unless(within_epsilon(b[1], -0.358386)); - fail_unless(within_epsilon(b[2], 0.271427)); + fail_unless(solution.num_used == 5); + fail_unless(arr_within_epsilon(3, solution.b, expected2)); } END_TEST diff --git a/tests/check_pvt.c b/tests/check_pvt.c index 279e1660..21d8f89e 100644 --- a/tests/check_pvt.c +++ b/tests/check_pvt.c @@ -86,6 +86,8 @@ START_TEST(test_pvt_repair) s8 code = calc_PVT(n_used, nms, false, &soln, &dops); fail_unless(code == 1, "Return code should be 1 (pvt repair). Saw: %d\n", code); + fail_unless(soln.raim_flag == 3, + "raim_flag should be 0b11, saw: %d\n", soln.raim_flag); fail_unless(soln.n_used == n_used - 1, "PVT solver failed to repair solution."); } @@ -104,6 +106,8 @@ START_TEST(test_disable_pvt_raim) s8 code = calc_PVT(n_used, nms, true, &soln, &dops); fail_unless(code == 2, "Return code should be 2 (raim not used). Saw: %d\n", code); + fail_unless(soln.raim_flag == 0, + "raim_flag should be 00, saw: %d\n", soln.raim_flag); fail_unless(soln.valid == 1, "Solution should be valid!"); }