From c52a1fd9aee58727d2b3542bd6c182523b881ff9 Mon Sep 17 00:00:00 2001 From: Scott Kovach Date: Tue, 21 Jul 2015 12:26:45 -0700 Subject: [PATCH 1/3] update flag output for raim changes to four sbp messages --- libswiftnav | 2 +- src/solution.c | 33 ++++++++++++++++++++------------- 2 files changed, 21 insertions(+), 14 deletions(-) diff --git a/libswiftnav b/libswiftnav index 0a1ac496..6800cad6 160000 --- a/libswiftnav +++ b/libswiftnav @@ -1 +1 @@ -Subproject commit 0a1ac49650bb0149a8a17f322e6ebac3e1695a24 +Subproject commit 6800cad662abc894bab33b1200caa8c7c5d10484 diff --git a/src/solution.c b/src/solution.c index 9bc1494e..e1096502 100644 --- a/src/solution.c +++ b/src/solution.c @@ -73,12 +73,12 @@ void solution_send_sbp(gnss_solution *soln, dops_t *dops) /* Position in LLH. */ msg_pos_llh_t pos_llh; - sbp_make_pos_llh(&pos_llh, soln, 0); + sbp_make_pos_llh(&pos_llh, soln, soln->raim_flag << 4); sbp_send_msg(SBP_MSG_POS_LLH, sizeof(pos_llh), (u8 *) &pos_llh); /* Position in ECEF. */ msg_pos_ecef_t pos_ecef; - sbp_make_pos_ecef(&pos_ecef, soln, 0); + sbp_make_pos_ecef(&pos_ecef, soln, soln->raim_flag << 3); sbp_send_msg(SBP_MSG_POS_ECEF, sizeof(pos_ecef), (u8 *) &pos_ecef); /* Velocity in NED. */ @@ -179,15 +179,18 @@ void solution_send_baseline(const gps_time_t *t, u8 n_sats, double b_ecef[3], /* TODO: Don't fake DOP!! */ nmea_gpgga(pseudo_absolute_llh, t, n_sats, fix_mode, 1.5); /* now send pseudo absolute sbp message */ - /* Flag in message is defined as follows :float->2, fixed->1 */ - /* We defined the flags for the SBP protocol to be spp->0, fixed->1, float->2 */ - /* TODO: Define these flags from the yaml and remove hardcoding */ - u8 sbp_flags = (flags == 1) ? 1 : 2; + /* TODO: Derive these flag functions from the yaml and remove hardcoding */ msg_pos_llh_t pos_llh; - sbp_make_pos_llh_vect(&pos_llh, pseudo_absolute_llh, t, n_sats, sbp_flags); + 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); + 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; - sbp_make_pos_ecef_vect(&pos_ecef, pseudo_absolute_ecef, t, n_sats, sbp_flags); + 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); } chMtxUnlock(); @@ -208,28 +211,31 @@ static void output_baseline(u8 num_sdiffs, const sdiff_t *sdiffs, &amb_state, &num_used, b, disable_raim, DEFAULT_RAIM_THRESHOLD); chMtxUnlock(); - if (ret > 0) { - /* ret is <0 on error, 2 if float, 1 if fixed */ - flags = (ret == 1) ? 1 : 0; - } else { + if (ret < 0) { log_warn("dgnss_baseline returned error: %d", ret); return; } + + /* dgnss_baseline return value is sbp baseline flag format, + * if non-negative. */ + flags = ret; break; case FILTER_FLOAT: - flags = 0; chMtxLock(&amb_state_lock); ret = baseline(num_sdiffs, sdiffs, position_solution.pos_ecef, &amb_state.float_ambs, &num_used, b, disable_raim, DEFAULT_RAIM_THRESHOLD); chMtxUnlock(); if (ret == 1) + /* dgnss_baseline logs a warning on raim repair; do the same here */ log_warn("output_baseline: Float baseline RAIM repair"); if (ret < 0) { log_warn("dgnss_float_baseline returned error: %d", ret); return; } + /* baseline_flag(baseline return code, float mode); */ + flags = baseline_flag(ret, false); break; } @@ -341,6 +347,7 @@ static void solution_simulation() if (simulation_enabled_for(SIMULATION_MODE_FLOAT) || simulation_enabled_for(SIMULATION_MODE_RTK)) { + /* raim is "unavailable" */ u8 flags = simulation_enabled_for(SIMULATION_MODE_RTK) ? 1 : 0; solution_send_baseline(&(soln->time), From 323990c47a63d1e083591455cd4c490bd82fb34f Mon Sep 17 00:00:00 2001 From: Scott Kovach Date: Fri, 31 Jul 2015 14:40:53 -0700 Subject: [PATCH 2/3] move flag manipulation to sbp_utils, update for dgnss_baseline_t --- src/sbp_utils.c | 21 +++++++++++++++++++++ src/sbp_utils.h | 5 +++++ src/solution.c | 48 ++++++++++++++++++++++-------------------------- src/solution.h | 5 +++-- 4 files changed, 51 insertions(+), 28 deletions(-) diff --git a/src/sbp_utils.c b/src/sbp_utils.c index 88435791..ff390214 100644 --- a/src/sbp_utils.c +++ b/src/sbp_utils.c @@ -294,5 +294,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 f7a3c6e0..4218ede6 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); @@ -60,6 +61,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 e1096502..6eeeaf37 100644 --- a/src/solution.c +++ b/src/solution.c @@ -138,11 +138,15 @@ double calc_heading(const double b_ned[3]) * 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); @@ -175,21 +179,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); } @@ -199,8 +200,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) { @@ -208,23 +208,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", 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) @@ -235,11 +231,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) @@ -348,12 +344,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 62120c9f..c5c790a3 100644 --- a/src/solution.h +++ b/src/solution.h @@ -18,6 +18,7 @@ #include #include #include +#include typedef enum { SOLN_MODE_LOW_LATENCY, @@ -48,8 +49,8 @@ void solution_send_nmea(gnss_solution *soln, dops_t *dops, u8 n, navigation_measurement_t *nm, u8 fix_type); double calc_heading(const double b_ned[3]); -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 From 4f8ac8b81f16d01c583d592bc54963c70aa38cc1 Mon Sep 17 00:00:00 2001 From: Scott Kovach Date: Wed, 4 Nov 2015 17:18:14 -0800 Subject: [PATCH 3/3] submodule update --- libswiftnav | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/libswiftnav b/libswiftnav index 6800cad6..d15cf638 160000 --- a/libswiftnav +++ b/libswiftnav @@ -1 +1 @@ -Subproject commit 6800cad662abc894bab33b1200caa8c7c5d10484 +Subproject commit d15cf6380aabaae79696fcf35c6ec10c4180281b