//INVARIANT: A fully received message with correct CRC is currently in parser_state
void
AP_GPS_SBP::_sbp_process_message() {
switch (parser_state.msg_type) {
case SBP_HEARTBEAT_MSGTYPE:
last_heatbeat_received_ms = AP_HAL::millis();
break;
case SBP_GPS_TIME_MSGTYPE:
memcpy(&last_gps_time, parser_state.msg_buff, sizeof(last_gps_time));
break;
case SBP_VEL_NED_MSGTYPE:
memcpy(&last_vel_ned, parser_state.msg_buff, sizeof(last_vel_ned));
break;
case SBP_POS_LLH_MSGTYPE: {
struct sbp_pos_llh_t *pos_llh = (struct sbp_pos_llh_t*)parser_state.msg_buff;
// Check if this is a single point or RTK solution
// flags = 0 -> single point
if (pos_llh->flags == 0) {
last_pos_llh_spp = *pos_llh;
} else if (pos_llh->flags == 1 || pos_llh->flags == 2) {
last_pos_llh_rtk = *pos_llh;
}
break;
}
case SBP_DOPS_MSGTYPE:
memcpy(&last_dops, parser_state.msg_buff, sizeof(last_dops));
break;
case SBP_TRACKING_STATE_MSGTYPE:
//INTENTIONALLY BLANK
//Currently unhandled, but logged after switch statement.
break;
case SBP_IAR_STATE_MSGTYPE: {
sbp_iar_state_t *iar = (struct sbp_iar_state_t*)parser_state.msg_buff;
last_iar_num_hypotheses = iar->num_hypotheses;
break;
}
default:
// log anyway if it's an unsupported message.
// The log mask will be used to adjust or suppress logging
break;
}
logging_log_raw_sbp(parser_state.msg_type, parser_state.sender_id, parser_state.msg_len, parser_state.msg_buff);
}
//INVARIANT: A fully received message with correct CRC is currently in parser_state
void
AP_GPS_SBP::_sbp_process_message() {
switch(parser_state.msg_type) {
case SBP_HEARTBEAT_MSGTYPE:
last_heatbeat_received_ms = hal.scheduler->millis();
break;
case SBP_GPS_TIME_MSGTYPE:
last_gps_time = *(struct sbp_gps_time_t*)parser_state.msg_buff;
break;
case SBP_VEL_NED_MSGTYPE:
last_vel_ned = *(struct sbp_vel_ned_t*)parser_state.msg_buff;
break;
case SBP_POS_LLH_MSGTYPE: {
struct sbp_pos_llh_t *pos_llh = (struct sbp_pos_llh_t*)parser_state.msg_buff;
// Check if this is a single point or RTK solution
// flags = 0 -> single point
if (pos_llh->flags == 0) {
last_pos_llh_spp = *pos_llh;
} else if (pos_llh->flags == 1 || pos_llh->flags == 2) {
last_pos_llh_rtk = *pos_llh;
}
break;
}
case SBP_DOPS_MSGTYPE:
last_dops = *(struct sbp_dops_t*)parser_state.msg_buff;
break;
case SBP_TRACKING_STATE_MSGTYPE:
//INTENTIONALLY BLANK
//Currenly unhandled, but logged after switch statement.
break;
case SBP_IAR_STATE_MSGTYPE: {
sbp_iar_state_t *iar = (struct sbp_iar_state_t*)parser_state.msg_buff;
last_iar_num_hypotheses = iar->num_hypotheses;
break;
}
default:
// Break out of any logging if it's an unsupported message
return;
}
logging_log_raw_sbp(parser_state.msg_type, parser_state.sender_id, parser_state.msg_len, parser_state.msg_buff);
}
//INVARIANT: A fully received message with correct CRC is currently in parser_state
void
AP_GPS_SBP2::_sbp_process_message() {
//Here, we copy messages into local structs.
switch (parser_state.msg_type) {
case SBP_HEARTBEAT_MSGTYPE:
memcpy(&last_heartbeat, parser_state.msg_buff, sizeof(struct sbp_heartbeat_t));
last_heartbeat_received_ms = AP_HAL::millis();
break;
case SBP_GPS_TIME_MSGTYPE:
memcpy(&last_gps_time, parser_state.msg_buff, sizeof(struct sbp_gps_time_t));
check_new_itow(last_gps_time.tow, parser_state.msg_len);
break;
case SBP_VEL_NED_MSGTYPE:
memcpy(&last_vel_ned, parser_state.msg_buff, sizeof(struct sbp_vel_ned_t));
check_new_itow(last_vel_ned.tow, parser_state.msg_len);
break;
case SBP_POS_LLH_MSGTYPE:
memcpy(&last_pos_llh, parser_state.msg_buff, sizeof(struct sbp_pos_llh_t));
check_new_itow(last_pos_llh.tow, parser_state.msg_len);
break;
case SBP_DOPS_MSGTYPE:
memcpy(&last_dops, parser_state.msg_buff, sizeof(struct sbp_dops_t));
check_new_itow(last_dops.tow, parser_state.msg_len);
break;
case SBP_EXT_EVENT_MSGTYPE:
memcpy(&last_event, parser_state.msg_buff, sizeof(struct sbp_ext_event_t));
check_new_itow(last_event.tow, parser_state.msg_len);
logging_ext_event();
break;
default:
break;
}
// send all messages we receive to log, even if it's an unsupported message,
// so we can do additional post-processing from Dataflash logs.
// The log mask will be used to adjust or suppress logging
logging_log_raw_sbp(parser_state.msg_type, parser_state.sender_id, parser_state.msg_len, parser_state.msg_buff);
}
