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