AP_OpenDroneID: check available tx space and fixed message rates

ensure we don't lose messages due to lack of serial TX space, and
fixed update rate of LOCATION and SYSTEM_UPDATE msgs

libraries/AP_OpenDroneID/AP_OpenDroneID.cpp

@@ -126,7 +126,9 @@ bool AP_OpenDroneID::pre_arm_check(char* failmsg, uint8_t failmsg_len)
         return false;
     }
 
-    if (last_system_ms == 0 || now_ms - last_system_ms > max_age_ms) {
+    if (last_system_ms == 0 ||
+        (now_ms - last_system_ms > max_age_ms &&
+         (now_ms - last_system_update_ms > max_age_ms))) {
         strncpy(failmsg, "SYSTEM not available", failmsg_len);
         return false;
     }
@@ -144,7 +146,6 @@ void AP_OpenDroneID::update()
     if (_enable == 0) {
         return;
     }
-    const uint32_t now = AP_HAL::millis();
 
     const bool armed = hal.util->get_soft_armed();
     if (armed && !_was_armed) {
@@ -153,17 +154,25 @@ void AP_OpenDroneID::update()
     }
     _was_armed = armed;
 
-    if (now - _last_send_dynamic_messages_ms >= _mavlink_dynamic_period_ms) {
+    send_dynamic_out();
-        _last_send_dynamic_messages_ms = now;
-        send_dynamic_out();
-    }
-
     send_static_out();
 }
 
 void AP_OpenDroneID::send_dynamic_out()
 {
-    send_location_message();
+    const uint32_t now = AP_HAL::millis();
+    if (now - _last_send_location_ms >= _mavlink_dynamic_period_ms &&
+        HAVE_PAYLOAD_SPACE(_chan, OPEN_DRONE_ID_LOCATION)) {
+        _last_send_location_ms = now;
+        send_location_message();
+    }
+
+    // operator location needs to be sent at the same rate as location for FAA compliance
+    if (now - _last_send_system_update_ms >= _mavlink_dynamic_period_ms &&
+        HAVE_PAYLOAD_SPACE(_chan, OPEN_DRONE_ID_SYSTEM_UPDATE)) {
+        _last_send_system_update_ms = now;
+        send_system_update_message();
+    }
 }
 
 void AP_OpenDroneID::send_static_out()
@@ -171,40 +180,60 @@ 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 && now_ms - last_lost_tx_ms > 5000) {
+    if (now_ms - last_arm_status_ms > 5000) {
-        last_lost_tx_ms = now_ms;
+        if (now_ms - last_lost_tx_ms > 5000) {
-        GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "ODID: lost transmitter");
+            last_lost_tx_ms = now_ms;
+            GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "ODID: lost transmitter");
+        }
+    } else if (last_lost_tx_ms != 0) {
+        // we're OK again
+        last_lost_tx_ms = 0;
+        GCS_SEND_TEXT(MAV_SEVERITY_INFO, "ODID: transmitter OK");
     }
 
-    const uint32_t msg_spacing_ms = _mavlink_dynamic_period_ms / 4;
+    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
         _chan = mavlink_channel_t(gcs().get_channel_from_port_number(_mav_port));
-
+        bool sent_ok = false;
-        last_msg_send_ms = now_ms;
         switch (next_msg_to_send) {
         case NEXT_MSG_BASIC_ID:
-            send_basic_id_message();
+            if (HAVE_PAYLOAD_SPACE(_chan, OPEN_DRONE_ID_BASIC_ID)) {
+                send_basic_id_message();
+                sent_ok = true;
+            }
             break;
         case NEXT_MSG_SYSTEM:
-            send_system_message();
+            if (HAVE_PAYLOAD_SPACE(_chan, OPEN_DRONE_ID_SYSTEM)) {
+                send_system_message();
+                sent_ok = true;
+            }
             break;
         case NEXT_MSG_SELF_ID:
-            send_self_id_message();
+            if (HAVE_PAYLOAD_SPACE(_chan, OPEN_DRONE_ID_SELF_ID)) {
+                send_self_id_message();
+                sent_ok = true;
+            }
             break;
         case NEXT_MSG_OPERATOR_ID:
-            send_operator_id_message();
+            if (HAVE_PAYLOAD_SPACE(_chan, OPEN_DRONE_ID_OPERATOR_ID)) {
+                send_operator_id_message();
+                sent_ok = true;
+            }
             break;
         case NEXT_MSG_ENUM_END:
             break;
         }
-        next_msg_to_send = next_msg((uint8_t(next_msg_to_send) + 1) % uint8_t(NEXT_MSG_ENUM_END));
+        if (sent_ok) {
+            last_msg_send_ms = now_ms;
+            next_msg_to_send = next_msg((uint8_t(next_msg_to_send) + 1) % uint8_t(NEXT_MSG_ENUM_END));
+        }
     }
 }
 
 // The send_location_message
 // all open_drone_id send functions use data stored in the open drone id class.
-//This location send function is an exception. It uses live location data from the ArduPilot system.
+// This location send function is an exception. It uses live location data from the ArduPilot system.
 void AP_OpenDroneID::send_location_message()
 {
     auto &ahrs = AP::ahrs();
@@ -362,12 +391,28 @@ void AP_OpenDroneID::send_system_message()
 void AP_OpenDroneID::send_self_id_message()
 {
     need_send_self_id |= dronecan_send_all;
-    // note that packet is filled in by the GCS
     if (_chan != MAV_CHAN_INVALID) {
         mavlink_msg_open_drone_id_self_id_send_struct(_chan, &pkt_self_id);
     }
 }
 
+void AP_OpenDroneID::send_system_update_message()
+{
+    need_send_system |= dronecan_send_all;
+    // note that packet is filled in by the GCS
+    if (_chan != MAV_CHAN_INVALID) {
+        const auto pkt_system_update = mavlink_open_drone_id_system_update_t {
+        operator_latitude : pkt_system.operator_latitude,
+        operator_longitude : pkt_system.operator_longitude,
+        operator_altitude_geo : pkt_system.operator_altitude_geo,
+        timestamp : pkt_system.timestamp,
+        target_system : pkt_system.target_system,
+        target_component : pkt_system.target_component,
+        };
+        mavlink_msg_open_drone_id_system_update_send_struct(_chan, &pkt_system_update);
+    }
+}
+
 void AP_OpenDroneID::send_operator_id_message()
 {
     need_send_operator_id |= dronecan_send_all;
@@ -602,10 +647,11 @@ void AP_OpenDroneID::handle_msg(mavlink_channel_t chan, const mavlink_message_t
         pkt_system.operator_longitude = pkt_system_update.operator_longitude;
         pkt_system.operator_altitude_geo = pkt_system_update.operator_altitude_geo;
         pkt_system.timestamp = pkt_system_update.timestamp;
+        last_system_update_ms = AP_HAL::millis();
         if (last_system_ms != 0) {
-            // we can only mark this as updated if we have the other
+            // we can only mark system as updated if we have the other
             // information already
-            last_system_ms = AP_HAL::millis();
+            last_system_ms = last_system_update_ms;
         }
         break;
     }

libraries/AP_OpenDroneID/AP_OpenDroneID.h

@@ -118,7 +118,8 @@ private:
 
     mavlink_channel_t _chan; // MAVLink channel that communicates with the Remote ID Transceiver
     const mavlink_channel_t MAV_CHAN_INVALID = mavlink_channel_t(255U);
-    uint32_t _last_send_dynamic_messages_ms;
+    uint32_t _last_send_location_ms;
+    uint32_t _last_send_system_update_ms;
     uint32_t _last_send_static_messages_ms;
     const uint32_t _mavlink_dynamic_period_ms = 1000; //how often are mavlink dynamic messages sent in ms. E.g. 1000 = 1 Hz
     const uint32_t _mavlink_static_period_ms = 5000; //how often are mavlink static messages sent in ms
@@ -138,6 +139,9 @@ private:
     // last time we got a SYSTEM message
     uint32_t last_system_ms;
 
+    // last time we got a SYSTEM_UPDATE message
+    uint32_t last_system_update_ms;
+
     // arm status from the transmitter
     mavlink_open_drone_id_arm_status_t arm_status;
     uint32_t last_arm_status_ms;
@@ -150,6 +154,7 @@ private:
     void send_static_out();
     void send_basic_id_message();
     void send_system_message();
+    void send_system_update_message();
     void send_self_id_message();
     void send_operator_id_message();
     void send_location_message();