diff --git a/libraries/AP_GPS/AP_GPS_SBP2.cpp b/libraries/AP_GPS/AP_GPS_SBP2.cpp index d48a9da586..ea7a861ae9 100644 --- a/libraries/AP_GPS/AP_GPS_SBP2.cpp +++ b/libraries/AP_GPS/AP_GPS_SBP2.cpp @@ -198,12 +198,17 @@ AP_GPS_SBP2::_sbp_process_message() { last_dops = *((struct sbp_dops_t*)parser_state.msg_buff); break; + case SBP_EXT_EVENT_MSGTYPE: + last_event = *((struct sbp_ext_event_t*)parser_state.msg_buff); + logging_ext_event(); + break; + default: break; } // send all messages we receive to log, even if it's an unsupported message, - // so we can do annditional post-processing from Dataflash logs. + // 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); } @@ -491,3 +496,21 @@ AP_GPS_SBP2::logging_log_raw_sbp(uint16_t msg_type, gps._DataFlash->WriteBlock(&pkt2, sizeof(pkt2)); } }; + +void +AP_GPS_SBP2::logging_ext_event() { + if (gps._DataFlash == nullptr || !gps._DataFlash->logging_started()) { + return; + } + + struct log_SbpEvent pkt = { + LOG_PACKET_HEADER_INIT(LOG_MSG_SBPEVENT), + time_us : AP_HAL::micros64(), + wn : last_event.wn, + tow : last_event.tow, + ns_residual : last_event.ns_residual, + level : last_event.flags.level, + quality : last_event.flags.quality, + }; + gps._DataFlash->WriteBlock(&pkt, sizeof(pkt)); +}; diff --git a/libraries/AP_GPS/AP_GPS_SBP2.h b/libraries/AP_GPS/AP_GPS_SBP2.h index 7e8ad457e9..3a4ad4a62a 100644 --- a/libraries/AP_GPS/AP_GPS_SBP2.h +++ b/libraries/AP_GPS/AP_GPS_SBP2.h @@ -78,6 +78,7 @@ private: static const uint16_t SBP_VEL_NED_MSGTYPE = 0x020E; static const uint16_t SBP_TRACKING_STATE_MSGTYPE = 0x0013; static const uint16_t SBP_IAR_STATE_MSGTYPE = 0x0019; + static const uint16_t SBP_EXT_EVENT_MSGTYPE = 0x0101; // Heartbeat struct PACKED sbp_heartbeat_t { @@ -148,6 +149,18 @@ private: } flags; }; // 22 bytes + // Messages reporting accurately-timestamped external events, e.g. camera shutter time. + struct PACKED sbp_ext_event_t { + uint16_t wn; //< GPS week number (unit: weeks) + uint32_t tow; //< GPS Time of Week (unit: ms) + int32_t ns_residual; //< Nanosecond residual of millisecond-rounded TOW (ranges from -500000 to 500000) + struct PACKED flags { + uint8_t level:1; //< New level of pin values (0: Low (falling edge), 1: High (rising edge)) + uint8_t quality:1; //< Time quality values (0: Unknown - don't have nav solution, 1: Good (< 1 microsecond)) + uint8_t res:6; //< Reserved + } flags; + }; // 12 bytes + void _sbp_process(); void _sbp_process_message(); bool _attempt_state_update(); @@ -163,6 +176,7 @@ private: struct sbp_dops_t last_dops; struct sbp_pos_llh_t last_pos_llh; struct sbp_vel_ned_t last_vel_ned; + struct sbp_ext_event_t last_event; uint32_t last_full_update_tow; uint16_t last_full_update_wn; @@ -178,6 +192,7 @@ private: // ************************************************************************ void logging_log_full_update(); + void logging_ext_event(); void logging_log_raw_sbp(uint16_t msg_type, uint16_t sender_id, uint8_t msg_len, uint8_t *msg_buff); int32_t distMod(int32_t tow1_ms, int32_t tow2_ms, int32_t mod);