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