void solution_send_sbp(gnss_solution *soln, dops_t *dops, bool clock_jump)
{
if (soln) {
/* Send GPS_TIME message first. */
msg_gps_time_t gps_time;
sbp_make_gps_time(&gps_time, &soln->time, 0);
sbp_send_msg(SBP_MSG_GPS_TIME, sizeof(gps_time), (u8 *) &gps_time);
if (chVTTimeElapsedSinceX(last_dgnss) > DGNSS_TIMEOUT(soln_freq)) {
/* Position in LLH. */
msg_pos_llh_t pos_llh;
sbp_make_pos_llh(&pos_llh, soln, 0);
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_send_msg(SBP_MSG_POS_ECEF, sizeof(pos_ecef), (u8 *) &pos_ecef);
}
/* Velocity in NED. */
/* Do not send if there has been a clock jump. Velocity may be unreliable.*/
if (!clock_jump) {
msg_vel_ned_t vel_ned;
sbp_make_vel_ned(&vel_ned, soln, 0);
sbp_send_msg(SBP_MSG_VEL_NED, sizeof(vel_ned), (u8 *) &vel_ned);
/* Velocity in ECEF. */
msg_vel_ecef_t vel_ecef;
sbp_make_vel_ecef(&vel_ecef, soln, 0);
sbp_send_msg(SBP_MSG_VEL_ECEF, sizeof(vel_ecef), (u8 *) &vel_ecef);
}
}
if (dops) {
DO_EVERY(10,
msg_dops_t sbp_dops;
sbp_make_dops(&sbp_dops, dops, &(soln->time));
sbp_send_msg(SBP_MSG_DOPS, sizeof(msg_dops_t), (u8 *) &sbp_dops);
);
}
void solution_send_sbp(gnss_solution *soln, dops_t *dops)
{
if (soln) {
/* Send GPS_TIME message first. */
sbp_gps_time_t gps_time;
sbp_make_gps_time(&gps_time, &soln->time, 0);
sbp_send_msg(SBP_GPS_TIME, sizeof(gps_time), (u8 *) &gps_time);
/* Position in LLH. */
sbp_pos_llh_t pos_llh;
sbp_make_pos_llh(&pos_llh, soln, 0);
sbp_send_msg(SBP_POS_LLH, sizeof(pos_llh), (u8 *) &pos_llh);
/* Position in ECEF. */
sbp_pos_ecef_t pos_ecef;
sbp_make_pos_ecef(&pos_ecef, soln, 0);
sbp_send_msg(SBP_POS_ECEF, sizeof(pos_ecef), (u8 *) &pos_ecef);
/* Velocity in NED. */
sbp_vel_ned_t vel_ned;
sbp_make_vel_ned(&vel_ned, soln, 0);
sbp_send_msg(SBP_VEL_NED, sizeof(vel_ned), (u8 *) &vel_ned);
/* Velocity in ECEF. */
sbp_vel_ecef_t vel_ecef;
sbp_make_vel_ecef(&vel_ecef, soln, 0);
sbp_send_msg(SBP_VEL_ECEF, sizeof(vel_ecef), (u8 *) &vel_ecef);
}
if (dops) {
DO_EVERY(10,
sbp_dops_t sbp_dops;
sbp_make_dops(&sbp_dops, dops);
sbp_send_msg(SBP_DOPS, sizeof(sbp_dops_t), (u8 *) &sbp_dops);
);
}