GCS_MAVLink: add support for AVAILABLE_MODES msg

ArduPilot · Nov 10, 2024 · b3eeb20 · b3eeb20
1 parent b0f361c
commit b3eeb20
Show file tree

Hide file tree

Showing 4 changed files with 66 additions and 0 deletions.
diff --git a/libraries/GCS_MAVLink/GCS.h b/libraries/GCS_MAVLink/GCS.h
@@ -412,6 +412,10 @@ class GCS_MAVLINK
     void send_uavionix_adsb_out_status() const;
     void send_autopilot_state_for_gimbal_device() const;
 
+    // Send the mode with the given index (not mode number!) return the total number of modes
+    // Index starts at 1
+    virtual uint8_t send_available_mode(uint8_t index) const = 0;
+
     // lock a channel, preventing use by MAVLink
     void lock(bool _lock) {
         _locked = _lock;
@@ -1126,6 +1130,16 @@ class GCS_MAVLINK
     // true if we should NOT do MAVLink on this port (usually because
     // someone's doing SERIAL_CONTROL over mavlink)
     bool _locked;
+
+    // Handling of AVAILABLE_MODES
+    struct {
+        bool should_send;
+        // Note these start at 1
+        uint8_t requested_index;
+        uint8_t next_index;
+    } available_modes;
+    bool send_available_modes();
+
 };
 
 /// @class GCS

diff --git a/libraries/GCS_MAVLink/GCS_Common.cpp b/libraries/GCS_MAVLink/GCS_Common.cpp
@@ -1148,6 +1148,7 @@ ap_message GCS_MAVLINK::mavlink_id_to_ap_message_id(const uint32_t mavlink_id) c
 #if AP_AIRSPEED_ENABLED
         { MAVLINK_MSG_ID_AIRSPEED, MSG_AIRSPEED},
 #endif
+        { MAVLINK_MSG_ID_AVAILABLE_MODES, MSG_AVAILABLE_MODES},
             };
 
     for (uint8_t i=0; i<ARRAY_SIZE(map); i++) {
@@ -3199,6 +3200,18 @@ MAV_RESULT GCS_MAVLINK::handle_command_request_message(const mavlink_command_int
     if (id == MSG_LAST) {
         return MAV_RESULT_FAILED;
     }
+
+    switch(id) {
+    case MSG_AVAILABLE_MODES:
+        available_modes.should_send = true;
+        available_modes.next_index = 1;
+        available_modes.requested_index = (uint8_t)packet.param2;
+        break;
+
+    default:
+        break;
+    }
+
     send_message(id);
     return MAV_RESULT_ACCEPTED;
 }
@@ -6092,6 +6105,39 @@ void GCS_MAVLINK::send_received_message_deprecation_warning(const char * message
     send_text(MAV_SEVERITY_INFO, "Received message (%s) is deprecated", message);
 }
 
+bool GCS_MAVLINK::send_available_modes()
+{
+    if (!available_modes.should_send) {
+        // must only return false if out of space
+        return true;
+    }
+
+    CHECK_PAYLOAD_SIZE(AVAILABLE_MODES);
+
+    // Zero is a special case for send all.
+    const bool send_all = available_modes.requested_index == 0;
+    uint8_t request_index;
+    if (!send_all) {
+        // Single request
+        request_index = available_modes.requested_index;
+        available_modes.should_send = false;
+
+    } else {
+        // Request all modes
+        request_index = available_modes.next_index;
+        available_modes.next_index += 1;
+    }
+
+    const uint8_t mode_count = send_available_mode(request_index);
+
+    if (send_all && (available_modes.next_index > mode_count)) {
+        // Sending all and just sent the last
+        available_modes.should_send = false;
+    }
+
+    return true;
+}
+
 bool GCS_MAVLINK::try_send_message(const enum ap_message id)
 {
     bool ret = true;
@@ -6518,6 +6564,10 @@ bool GCS_MAVLINK::try_send_message(const enum ap_message id)
         break;
 #endif
 
+    case MSG_AVAILABLE_MODES:
+        ret = send_available_modes();
+        break;
+
     default:
         // try_send_message must always at some stage return true for
         // a message, or we will attempt to infinitely retry the

diff --git a/libraries/GCS_MAVLink/GCS_Dummy.h b/libraries/GCS_MAVLink/GCS_Dummy.h
@@ -35,6 +35,7 @@ class GCS_MAVLINK_Dummy : public GCS_MAVLINK
 
     void send_nav_controller_output() const override {};
     void send_pid_tuning() override {};
+    uint8_t send_available_mode(uint8_t index) const override { return 0; }
 };
 
 /*

diff --git a/libraries/GCS_MAVLink/ap_message.h b/libraries/GCS_MAVLink/ap_message.h
@@ -103,5 +103,6 @@ enum ap_message : uint8_t {
     MSG_HIGHRES_IMU,
 #endif
     MSG_AIRSPEED,
+    MSG_AVAILABLE_MODES,
     MSG_LAST // MSG_LAST must be the last entry in this enum
 };