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

---
 ArduSub/GCS_Mavlink.cpp | 47 ++++++++++++++++++++++++++++++++++++++++-
 ArduSub/GCS_Mavlink.h   |  4 ++++
 ArduSub/mode.h          | 15 +++++++++++++
 3 files changed, 65 insertions(+), 1 deletion(-)

diff --git a/ArduSub/GCS_Mavlink.cpp b/ArduSub/GCS_Mavlink.cpp
index c2bbf8b5c4..f17901638d 100644
--- a/ArduSub/GCS_Mavlink.cpp
+++ b/ArduSub/GCS_Mavlink.cpp
@@ -443,7 +443,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
 };
 
 const struct GCS_MAVLINK::stream_entries GCS_MAVLINK::all_stream_entries[] = {
@@ -968,3 +969,47 @@ uint8_t GCS_MAVLINK_Sub::high_latency_tgt_airspeed() const
     return 0;
 }
 #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_Sub::send_available_mode(uint8_t index) const
+{
+    const Mode* modes[] {
+        &sub.mode_manual,
+        &sub.mode_stabilize,
+        &sub.mode_acro,
+        &sub.mode_althold,
+        &sub.mode_surftrak,
+        &sub.mode_poshold,
+        &sub.mode_auto,
+        &sub.mode_guided,
+        &sub.mode_circle,
+        &sub.mode_surface,
+        &sub.mode_motordetect,
+    };
+
+    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/ArduSub/GCS_Mavlink.h b/ArduSub/GCS_Mavlink.h
index c38ec3f4ab..7dc2d0c1a2 100644
--- a/ArduSub/GCS_Mavlink.h
+++ b/ArduSub/GCS_Mavlink.h
@@ -37,6 +37,10 @@ protected:
 
     uint64_t capabilities() const override;
 
+    // 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/ArduSub/mode.h b/ArduSub/mode.h
index 11a6447167..35780cb9c0 100644
--- a/ArduSub/mode.h
+++ b/ArduSub/mode.h
@@ -72,6 +72,9 @@ public:
     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;
+
     // functions for reporting to GCS
     virtual bool get_wp(Location &loc) { return false; }
     virtual int32_t wp_bearing() const { return 0; }
@@ -202,6 +205,7 @@ protected:
 
     const char *name() const override { return "MANUAL"; }
     const char *name4() const override { return "MANU"; }
+    Mode::Number number() const override { return Mode::Number::MANUAL; }
 };
 
 
@@ -224,6 +228,7 @@ protected:
 
     const char *name() const override { return "ACRO"; }
     const char *name4() const override { return "ACRO"; }
+    Mode::Number number() const override { return Mode::Number::ACRO; }
 };
 
 
@@ -246,6 +251,7 @@ protected:
 
     const char *name() const override { return "STABILIZE"; }
     const char *name4() const override { return "STAB"; }
+    Mode::Number number() const override { return Mode::Number::STABILIZE; }
 };
 
 
@@ -272,6 +278,7 @@ protected:
 
     const char *name() const override { return "ALT_HOLD"; }
     const char *name4() const override { return "ALTH"; }
+    Mode::Number number() const override { return Mode::Number::ALT_HOLD; }
 };
 
 
@@ -293,6 +300,7 @@ protected:
 
     const char *name() const override { return "SURFTRAK"; }
     const char *name4() const override { return "STRK"; }
+    Mode::Number number() const override { return Mode::Number::SURFTRAK; }
 
 private:
 
@@ -342,6 +350,8 @@ protected:
 
     const char *name() const override { return "GUIDED"; }
     const char *name4() const override { return "GUID"; }
+    Mode::Number number() const override { return Mode::Number::GUIDED; }
+
     autopilot_yaw_mode get_default_auto_yaw_mode(bool rtl) const;
 
 private:
@@ -387,6 +397,7 @@ protected:
 
     const char *name() const override { return "AUTO"; }
     const char *name4() const override { return "AUTO"; }
+    Mode::Number number() const override { return Mode::Number::AUTO; }
 
 private:
     void auto_wp_run();
@@ -417,6 +428,7 @@ protected:
 
     const char *name() const override { return "POSHOLD"; }
     const char *name4() const override { return "POSH"; }
+    Mode::Number number() const override { return Mode::Number::POSHOLD; }
 };
 
 
@@ -439,6 +451,7 @@ protected:
 
     const char *name() const override { return "CIRCLE"; }
     const char *name4() const override { return "CIRC"; }
+    Mode::Number number() const override { return Mode::Number::CIRCLE; }
 };
 
 class ModeSurface : public Mode
@@ -460,6 +473,7 @@ protected:
 
     const char *name() const override { return "SURFACE"; }
     const char *name4() const override { return "SURF"; }
+    Mode::Number number() const override { return Mode::Number::CIRCLE; }
 };
 
 
@@ -482,4 +496,5 @@ protected:
 
     const char *name() const override { return "MOTORDETECT"; }
     const char *name4() const override { return "DETE"; }
+    Mode::Number number() const override { return Mode::Number::MOTOR_DETECT; }
 };