From 76f77af4f1a3c8a6e2bfc544414b5687a41f9575 Mon Sep 17 00:00:00 2001 From: Scott Kovach Date: Fri, 31 Jul 2015 14:40:53 -0700 Subject: [PATCH] move flag manipulation to sbp_utils, update for dgnss_baseline_t --- libswiftnav | 2 +- src/sbp_utils.c | 21 +++++++++++++++++++++ src/sbp_utils.h | 5 +++++ src/solution.c | 48 ++++++++++++++++++++++-------------------------- src/solution.h | 5 +++-- 5 files changed, 52 insertions(+), 29 deletions(-) diff --git a/libswiftnav b/libswiftnav index f3cf73aa..313c7e97 160000 --- a/libswiftnav +++ b/libswiftnav @@ -1 +1 @@ -Subproject commit f3cf73aa4646e1cd8331bc99633ff2d8b4abfa3b +Subproject commit 313c7e97f75843df22ab67030a3317c0abeb5121 diff --git a/src/sbp_utils.c b/src/sbp_utils.c index 7e299058..79e9d04a 100644 --- a/src/sbp_utils.c +++ b/src/sbp_utils.c @@ -286,5 +286,26 @@ void pack_ephemeris(const ephemeris_t *e, msg_ephemeris_t *msg) msg->iode = e->iode; } +u8 sbp_format_baseline_flag(dgnss_baseline_t *solution) +{ + return solution->fixed_mode + | (solution->raim_available << 3) + | (solution->raim_repair << 4); +} + +u8 sbp_format_ecef_flag(dgnss_baseline_t *solution) +{ + return (solution->fixed_mode ? 1 : 2) + | (solution->raim_available << 3) + | (solution->raim_repair << 4); +} + +u8 sbp_format_llh_flag(dgnss_baseline_t *solution) +{ + return (solution->fixed_mode ? 1 : 2) + | (solution->raim_available << 4) + | (solution->raim_repair << 5); +} + /** \} */ /** \} */ diff --git a/src/sbp_utils.h b/src/sbp_utils.h index 4225e1ff..b308caa1 100644 --- a/src/sbp_utils.h +++ b/src/sbp_utils.h @@ -18,6 +18,7 @@ #include #include #include +#include void sbp_make_gps_time(msg_gps_time_t *t_out, const gps_time_t *t_in, u8 flags); void sbp_make_pos_llh(msg_pos_llh_t *pos_llh, const gnss_solution *soln, u8 flags); @@ -59,6 +60,10 @@ void unpack_ephemeris(const msg_ephemeris_t *msg, ephemeris_t *e); void pack_ephemeris(const ephemeris_t *e, msg_ephemeris_t *msg); +u8 sbp_format_baseline_flag(dgnss_baseline_t *solution); +u8 sbp_format_ecef_flag(dgnss_baseline_t *solution); +u8 sbp_format_llh_flag(dgnss_baseline_t *solution); + /** Value specifying the size of the SBP framing */ #define SBP_FRAMING_SIZE_BYTES 8 /** Value defining maximum SBP packet size */ diff --git a/src/solution.c b/src/solution.c index e8774bed..faf8df66 100644 --- a/src/solution.c +++ b/src/solution.c @@ -129,11 +129,15 @@ void solution_send_nmea(gnss_solution *soln, dops_t *dops, * for conversion from ECEF to local NED coordinates (meters) * \param flags u8 RTK solution flags. 1 if float, 0 if fixed */ -void solution_send_baseline(const gps_time_t *t, u8 n_sats, double b_ecef[3], - double ref_ecef[3], u8 flags) +void solution_send_baseline(const gps_time_t *t, dgnss_baseline_t *solution, + double ref_ecef[3]) { double* base_station_pos; + u8 n_sats = solution->num_used; + double *b_ecef = solution->b; + msg_baseline_ecef_t sbp_ecef; + u8 flags = sbp_format_baseline_flag(solution); sbp_make_baseline_ecef(&sbp_ecef, t, n_sats, b_ecef, flags); sbp_send_msg(SBP_MSG_BASELINE_ECEF, sizeof(sbp_ecef), (u8 *)&sbp_ecef); @@ -161,21 +165,18 @@ void solution_send_baseline(const gps_time_t *t, u8 n_sats, double b_ecef[3], vector_add(3, base_station_pos, b_ecef, pseudo_absolute_ecef); wgsecef2llh(pseudo_absolute_ecef, pseudo_absolute_llh); - u8 fix_mode = (flags & 1) ? NMEA_GGA_FIX_RTK : NMEA_GGA_FIX_FLOAT; + u8 fix_mode = (solution->fixed_mode) ? NMEA_GGA_FIX_RTK : NMEA_GGA_FIX_FLOAT; /* TODO: Don't fake DOP!! */ nmea_gpgga(pseudo_absolute_llh, t, n_sats, fix_mode, 1.5); - /* now send pseudo absolute sbp message */ - /* TODO: Derive these flag functions from the yaml and remove hardcoding */ + + /* now send pseudo absolute sbp messages */ msg_pos_llh_t pos_llh; - u8 llh_flag = (flag_fixed_mode(flags) ? 1 : 2) - | (flag_raim_available(flags) << 4) - | (flag_raim_repaired(flags) << 5); - u8 ecef_flag = (flag_fixed_mode(flags) ? 1 : 2) - | (flag_raim_available(flags) << 3) - | (flag_raim_repaired(flags) << 4); + u8 llh_flag = sbp_format_llh_flag(solution); sbp_make_pos_llh_vect(&pos_llh, pseudo_absolute_llh, t, n_sats, llh_flag); sbp_send_msg(SBP_MSG_POS_LLH, sizeof(pos_llh), (u8 *) &pos_llh); + msg_pos_ecef_t pos_ecef; + u8 ecef_flag = sbp_format_ecef_flag(solution); sbp_make_pos_ecef_vect(&pos_ecef, pseudo_absolute_ecef, t, n_sats, ecef_flag); sbp_send_msg(SBP_MSG_POS_ECEF, sizeof(pos_ecef), (u8 *) &pos_ecef); } @@ -185,8 +186,7 @@ void solution_send_baseline(const gps_time_t *t, u8 n_sats, double b_ecef[3], static void output_baseline(u8 num_sdiffs, const sdiff_t *sdiffs, const gps_time_t *t) { - double b[3]; - u8 num_used, flags; + dgnss_baseline_t solution; s8 ret; switch (dgnss_filter) { @@ -194,23 +194,19 @@ static void output_baseline(u8 num_sdiffs, const sdiff_t *sdiffs, case FILTER_FIXED: chMtxLock(&amb_state_lock); ret = dgnss_baseline(num_sdiffs, sdiffs, position_solution.pos_ecef, - &amb_state, &num_used, b, + &amb_state, &solution, disable_raim, DEFAULT_RAIM_THRESHOLD); chMtxUnlock(); if (ret < 0) { log_warn("dgnss_baseline returned error: %d\n", ret); return; } - - /* dgnss_baseline return value is sbp baseline flag format, - * if non-negative. */ - flags = ret; break; case FILTER_FLOAT: chMtxLock(&amb_state_lock); ret = baseline(num_sdiffs, sdiffs, position_solution.pos_ecef, - &amb_state.float_ambs, &num_used, b, + &amb_state.float_ambs, &solution.num_used, solution.b, disable_raim, DEFAULT_RAIM_THRESHOLD); chMtxUnlock(); if (ret == 1) @@ -221,11 +217,11 @@ static void output_baseline(u8 num_sdiffs, const sdiff_t *sdiffs, return; } /* baseline_flag(baseline return code, float mode); */ - flags = baseline_flag(ret, false); + fill_property_flags(ret, false, &solution); break; } - solution_send_baseline(t, num_used, b, position_solution.pos_ecef, flags); + solution_send_baseline(t, &solution, position_solution.pos_ecef); } void send_observations(u8 n, gps_time_t *t, navigation_measurement_t *m) @@ -334,12 +330,12 @@ static void solution_simulation() simulation_enabled_for(SIMULATION_MODE_RTK)) { /* raim is "unavailable" */ - u8 flags = simulation_enabled_for(SIMULATION_MODE_RTK) ? 1 : 0; + dgnss_baseline_t solution; + solution.fixed_mode = simulation_enabled_for(SIMULATION_MODE_RTK); + solution.raim_available = 0; + solution.raim_repair = 0; - solution_send_baseline(&(soln->time), - simulation_current_num_sats(), - simulation_current_baseline_ecef(), - simulation_ref_ecef(), flags); + solution_send_baseline(&(soln->time), &solution, simulation_ref_ecef()); double t_check = expected_tow * (soln_freq / obs_output_divisor); if (fabs(t_check - (u32)t_check) < TIME_MATCH_THRESHOLD) { diff --git a/src/solution.h b/src/solution.h index c9a7380a..2a6aa954 100644 --- a/src/solution.h +++ b/src/solution.h @@ -18,6 +18,7 @@ #include #include #include +#include typedef enum { SOLN_MODE_LOW_LATENCY, @@ -47,8 +48,8 @@ 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, u8 fix_type); -void solution_send_baseline(const gps_time_t *t, u8 n_sats, double b_ecef[3], - double ref_ecef[3], u8 flags); +void solution_send_baseline(const gps_time_t *t, dgnss_baseline_t *solution, + double ref_ecef[3]); void solution_setup(void); #endif