From b6186db24e59b3e319a3b8fc2e76e32232ad5281 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Mon, 15 Aug 2022 09:02:59 +1000 Subject: [PATCH] AP_OpenDroneID: report if we lose operator location --- libraries/AP_OpenDroneID/AP_OpenDroneID.cpp | 6 ++++++ libraries/AP_OpenDroneID/AP_OpenDroneID.h | 3 +++ 2 files changed, 9 insertions(+) diff --git a/libraries/AP_OpenDroneID/AP_OpenDroneID.cpp b/libraries/AP_OpenDroneID/AP_OpenDroneID.cpp index f86e6ec3f7..24a4ceab98 100644 --- a/libraries/AP_OpenDroneID/AP_OpenDroneID.cpp +++ b/libraries/AP_OpenDroneID/AP_OpenDroneID.cpp @@ -195,6 +195,12 @@ void AP_OpenDroneID::send_static_out() GCS_SEND_TEXT(MAV_SEVERITY_INFO, "ODID: transmitter OK"); } + // 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_static_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 diff --git a/libraries/AP_OpenDroneID/AP_OpenDroneID.h b/libraries/AP_OpenDroneID/AP_OpenDroneID.h index 105c9fd943..54e0e9e034 100644 --- a/libraries/AP_OpenDroneID/AP_OpenDroneID.h +++ b/libraries/AP_OpenDroneID/AP_OpenDroneID.h @@ -150,6 +150,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();