From e9688caac09dd3892e921798bfea5f13127af371 Mon Sep 17 00:00:00 2001
From: Iampete1 <iampete@hotmail.co.uk>
Date: Sat, 9 Nov 2024 12:35:04 +0000
Subject: [PATCH] Blimp: add support for `AVAILABLE_MODES` msg

---
 Blimp/GCS_Mavlink.cpp | 41 ++++++++++++++++++++++++++++++++++++++++-
 Blimp/GCS_Mavlink.h   |  4 ++++
 Blimp/mode.h          | 14 ++++++++++++++
 3 files changed, 58 insertions(+), 1 deletion(-)

diff --git a/Blimp/GCS_Mavlink.cpp b/Blimp/GCS_Mavlink.cpp
index b132d80160a5c..09b355b8cafc8 100644
--- a/Blimp/GCS_Mavlink.cpp
+++ b/Blimp/GCS_Mavlink.cpp
@@ -395,7 +395,8 @@ static const ap_message STREAM_EXTRA3_msgs[] = {
 #endif
 };
 static const ap_message STREAM_PARAMS_msgs[] = {
-    MSG_NEXT_PARAM
+    MSG_NEXT_PARAM,
+    MSG_AVAILABLE_MODES
 };
 static const ap_message STREAM_ADSB_msgs[] = {
     MSG_ADSB_VEHICLE,
@@ -608,3 +609,41 @@ uint8_t GCS_MAVLINK_Blimp::high_latency_wind_direction() const
     return wrap_360(degrees(atan2f(-wind.y, -wind.x))) / 2;
 }
 #endif // HAL_HIGH_LATENCY2_ENABLED
+
+// Send the mode with the given index (not mode number!) return the total number of modes
+// Index starts at 1
+uint8_t GCS_MAVLINK_Blimp::send_available_mode(uint8_t index) const
+{
+    const Mode* modes[] {
+        &blimp.mode_land,
+        &blimp.mode_manual,
+        &blimp.mode_velocity,
+        &blimp.mode_loiter,
+        &blimp.mode_rtl,
+    };
+
+    const uint8_t mode_count = ARRAY_SIZE(modes);
+
+    // Convert to zero indexed
+    const uint8_t index_zero = index - 1;
+    if (index_zero >= mode_count) {
+        // Mode does not exist!?
+        return mode_count;
+    }
+
+    // Ask the mode for its name and number
+    const char* name = modes[index_zero]->name();
+    const uint8_t mode_number = (uint8_t)modes[index_zero]->number();
+
+    mavlink_msg_available_modes_send(
+        chan,
+        mode_count,
+        index,
+        MAV_STANDARD_MODE::MAV_STANDARD_MODE_NON_STANDARD,
+        mode_number,
+        0, // MAV_MODE_PROPERTY bitmask
+        name
+    );
+
+    return mode_count;
+}
diff --git a/Blimp/GCS_Mavlink.h b/Blimp/GCS_Mavlink.h
index 35276b03a46d5..90c4881b2adf4 100644
--- a/Blimp/GCS_Mavlink.h
+++ b/Blimp/GCS_Mavlink.h
@@ -48,6 +48,10 @@ class GCS_MAVLINK_Blimp : public GCS_MAVLINK
     uint32_t log_radio_bit() const override { return MASK_LOG_PM; }
 #endif
 
+    // Send the mode with the given index (not mode number!) return the total number of modes
+    // Index starts at 1
+    uint8_t send_available_mode(uint8_t index) const override;
+
 private:
 
     void handle_message(const mavlink_message_t &msg) override;
diff --git a/Blimp/mode.h b/Blimp/mode.h
index 8a990fbb31b30..8718d47fa144e 100644
--- a/Blimp/mode.h
+++ b/Blimp/mode.h
@@ -52,6 +52,9 @@ class Mode
     virtual const char *name() const = 0;
     virtual const char *name4() const = 0;
 
+    // returns a unique number specific to this mode
+    virtual Mode::Number number() const = 0;
+
     virtual bool is_landing() const
     {
         return false;
@@ -159,6 +162,8 @@ class ModeManual : public Mode
         return "MANU";
     }
 
+    Mode::Number number() const override { return Mode::Number::MANUAL; }
+
 private:
 
 };
@@ -201,6 +206,8 @@ class ModeVelocity : public Mode
         return "VELY";
     }
 
+    Mode::Number number() const override { return Mode::Number::VELOCITY; }
+
 private:
 
 };
@@ -244,6 +251,8 @@ class ModeLoiter : public Mode
         return "LOIT";
     }
 
+    Mode::Number number() const override { return Mode::Number::LOITER; }
+
 private:
     Vector3f target_pos;
     float target_yaw;
@@ -286,6 +295,8 @@ class ModeLand : public Mode
         return "LAND";
     }
 
+    Mode::Number number() const override { return Mode::Number::LAND; }
+
 private:
 
 };
@@ -328,4 +339,7 @@ class ModeRTL : public Mode
     {
         return "RTL";
     }
+
+    Mode::Number number() const override { return Mode::Number::RTL; }
+
 };