Skip to content
This repository has been archived by the owner on Apr 13, 2021. It is now read-only.

update flag output for raim changes to four sbp messages #528

Open
wants to merge 3 commits into
base: master
Choose a base branch
from
+60 −30
Open

update flag output for raim changes to four sbp messages #528

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
3 commits
Select commit Hold shift + click to select a range
c52a1fd
update flag output for raim changes to four sbp messages
kovach Jul 21, 2015
323990c
move flag manipulation to sbp_utils, update for dgnss_baseline_t
kovach Jul 31, 2015
4f8ac8b
submodule update
kovach Nov 5, 2015
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
2 changes: 1 addition & 1 deletion libswiftnav
Open in desktop
Submodule libswiftnav updated from 0a1ac4 to d15cf6
21 changes: 21 additions & 0 deletions src/sbp_utils.c
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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)
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Pity these bit offsets for the flag fields aren't exported in the libsbp generated C headers. I suppose one could #define them here to avoid repeating magic numbers, but that would only be marginally prettier.

| (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);
}

/** \} */
/** \} */
5 changes: 5 additions & 0 deletions src/sbp_utils.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -18,6 +18,7 @@
#include <libsbp/observation.h>
#include <libswiftnav/gpstime.h>
#include <libswiftnav/pvt.h>
#include <libswiftnav/dgnss_management.h>

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);
Expand Down Expand Up @@ -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 */
Expand Down
57 changes: 30 additions & 27 deletions src/solution.c
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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. */
Expand Down Expand Up @@ -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);

Expand Down Expand Up @@ -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();
Expand All @@ -196,44 +200,42 @@ 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) {
default:
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)
Expand Down Expand Up @@ -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) {
Expand Down
5 changes: 3 additions & 2 deletions src/solution.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -18,6 +18,7 @@
#include <libswiftnav/pvt.h>
#include <libswiftnav/track.h>
#include <libswiftnav/gpstime.h>
#include <libswiftnav/dgnss_management.h>

typedef enum {
SOLN_MODE_LOW_LATENCY,
Expand Down Expand Up @@ -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
Expand Down