Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-03 06:28:27 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_GPS: Add SBP EXT_EVENT Message to dataflash log

Browse Source
This commit is contained in:
ebethon 2017-06-29 15:17:10 +01:00 committed by Francisco Ferreira
parent 526d5a4c44
commit 005dbf41a7
2 changed files with 39 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

25
libraries/AP_GPS/AP_GPS_SBP2.cpp
View File

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

15
libraries/AP_GPS/AP_GPS_SBP2.h
View File

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