mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-01-24 17:48:35 -04:00
AP_OpenDroneID: report if we lose operator location
This commit is contained in:
parent
8018cfe6a4
commit
d503af2b9e
@ -176,6 +176,12 @@ void AP_OpenDroneID::send_static_out()
|
||||
GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "ODID: lost transmitter");
|
||||
}
|
||||
|
||||
// we need to notify user if we lost system msg with operator location
|
||||
if (now_ms - last_system_ms > 5000 && now_ms - last_lost_operator_msg_ms > 5000) {
|
||||
last_lost_operator_msg_ms = now_ms;
|
||||
GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "ODID: lost operator location");
|
||||
}
|
||||
|
||||
const uint32_t msg_spacing_ms = _mavlink_dynamic_period_ms / 4;
|
||||
if (now_ms - last_msg_send_ms >= msg_spacing_ms) {
|
||||
// allow update of channel during setup, this makes it easy to debug with a GCS
|
||||
|
@ -145,6 +145,9 @@ private:
|
||||
// last time we sent a lost transmitter message
|
||||
uint32_t last_lost_tx_ms;
|
||||
|
||||
// last time we sent a lost operator location notice
|
||||
uint32_t last_lost_operator_msg_ms;
|
||||
|
||||
// transmit functions to manually send a static MAVLink message
|
||||
void send_dynamic_out();
|
||||
void send_static_out();
|
||||
|
Loading…
Reference in New Issue
Block a user