diff --git a/libraries/AP_OpenDroneID/AP_OpenDroneID.cpp b/libraries/AP_OpenDroneID/AP_OpenDroneID.cpp index 1dd5e5e178..ddbecdebbd 100644 --- a/libraries/AP_OpenDroneID/AP_OpenDroneID.cpp +++ b/libraries/AP_OpenDroneID/AP_OpenDroneID.cpp @@ -171,7 +171,8 @@ void AP_OpenDroneID::send_static_out() const uint32_t now_ms = AP_HAL::millis(); // we need to notify user if we lost the transmitter - if (now_ms - last_arm_status_ms > 5000) { + if (now_ms - last_arm_status_ms > 5000 && now_ms - last_lost_tx_ms > 5000) { + last_lost_tx_ms = now_ms; GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "ODID: lost transmitter"); } diff --git a/libraries/AP_OpenDroneID/AP_OpenDroneID.h b/libraries/AP_OpenDroneID/AP_OpenDroneID.h index 48c520a8f3..4a4c850b1f 100644 --- a/libraries/AP_OpenDroneID/AP_OpenDroneID.h +++ b/libraries/AP_OpenDroneID/AP_OpenDroneID.h @@ -142,6 +142,9 @@ private: mavlink_open_drone_id_arm_status_t arm_status; uint32_t last_arm_status_ms; + // last time we sent a lost transmitter message + uint32_t last_lost_tx_ms; + // transmit functions to manually send a static MAVLink message void send_dynamic_out(); void send_static_out();