diff --git a/libswiftnav b/libswiftnav index 0a1ac496..d15cf638 160000 --- a/libswiftnav +++ b/libswiftnav @@ -1 +1 @@ -Subproject commit 0a1ac49650bb0149a8a17f322e6ebac3e1695a24 +Subproject commit d15cf6380aabaae79696fcf35c6ec10c4180281b 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 9bc1494e..6eeeaf37 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. */ @@ -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,19 +179,19 @@ 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 */ - /* 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; + + /* now send pseudo absolute sbp messages */ msg_pos_llh_t pos_llh; - sbp_make_pos_llh_vect(&pos_llh, pseudo_absolute_llh, t, n_sats, sbp_flags); + 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; - sbp_make_pos_ecef_vect(&pos_ecef, pseudo_absolute_ecef, t, n_sats, sbp_flags); + 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); } chMtxUnlock(); @@ -196,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) { @@ -205,35 +208,34 @@ 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) { - /* 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; } 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, + &amb_state.float_ambs, &solution.num_used, solution.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); */ + 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) @@ -341,12 +343,13 @@ static void solution_simulation() if (simulation_enabled_for(SIMULATION_MODE_FLOAT) || simulation_enabled_for(SIMULATION_MODE_RTK)) { - u8 flags = simulation_enabled_for(SIMULATION_MODE_RTK) ? 1 : 0; + /* raim is "unavailable" */ + 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