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

---
 AntennaTracker/GCS_Mavlink.cpp | 43 +++++++++++++++++++++++++++++++++-
 AntennaTracker/GCS_Mavlink.h   |  4 ++++
 AntennaTracker/mode.h          |  8 +++++++
 3 files changed, 54 insertions(+), 1 deletion(-)

diff --git a/AntennaTracker/GCS_Mavlink.cpp b/AntennaTracker/GCS_Mavlink.cpp
index 93e6638304..c9ec2050c7 100644
--- a/AntennaTracker/GCS_Mavlink.cpp
+++ b/AntennaTracker/GCS_Mavlink.cpp
@@ -321,7 +321,8 @@ static const ap_message STREAM_EXTRA3_msgs[] = {
     MSG_BATTERY_STATUS,
 };
 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[] = {
@@ -649,3 +650,43 @@ void GCS_MAVLINK_Tracker::send_global_position_int()
         0,                        // Z speed cm/s (+ve Down)
         tracker.ahrs.yaw_sensor); // compass heading in 1/100 degree
 }
+
+// Send the mode with the given index (not mode number!) return the total number of modes
+// Index starts at 1
+uint8_t GCS_MAVLINK_Tracker::send_available_mode(uint8_t index) const
+{
+    const Mode* modes[] {
+        &tracker.mode_manual,
+        &tracker.mode_stop,
+        &tracker.mode_scan,
+        &tracker.mode_guided,
+        &tracker.mode_servotest,
+        &tracker.mode_auto,
+        &tracker.mode_initialising,
+    };
+
+    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/AntennaTracker/GCS_Mavlink.h b/AntennaTracker/GCS_Mavlink.h
index a431f5217e..af98ee5a39 100644
--- a/AntennaTracker/GCS_Mavlink.h
+++ b/AntennaTracker/GCS_Mavlink.h
@@ -30,6 +30,10 @@ protected:
     void send_nav_controller_output() const override;
     void send_pid_tuning() 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 packetReceived(const mavlink_status_t &status, const mavlink_message_t &msg) override;
diff --git a/AntennaTracker/mode.h b/AntennaTracker/mode.h
index d6a10022dc..bfdcad9ac7 100644
--- a/AntennaTracker/mode.h
+++ b/AntennaTracker/mode.h
@@ -22,6 +22,7 @@ public:
 
     // returns a unique number specific to this mode
     virtual Mode::Number number() const = 0;
+    virtual const char* name() const = 0;
 
     virtual bool requires_armed_servos() const = 0;
 
@@ -41,6 +42,7 @@ protected:
 class ModeAuto : public Mode {
 public:
     Mode::Number number() const override { return Mode::Number::AUTO; }
+    const char* name() const override { return "Auto"; }
     bool requires_armed_servos() const override { return true; }
     void update() override;
 };
@@ -48,6 +50,7 @@ public:
 class ModeGuided : public Mode {
 public:
     Mode::Number number() const override { return Mode::Number::GUIDED; }
+    const char* name() const override { return "Guided"; }
     bool requires_armed_servos() const override { return true; }
     void update() override;
 
@@ -66,6 +69,7 @@ private:
 class ModeInitialising : public Mode {
 public:
     Mode::Number number() const override { return Mode::Number::INITIALISING; }
+    const char* name() const override { return "Initialising"; }
     bool requires_armed_servos() const override { return false; }
     void update() override {};
 };
@@ -73,6 +77,7 @@ public:
 class ModeManual : public Mode {
 public:
     Mode::Number number() const override { return Mode::Number::MANUAL; }
+    const char* name() const override { return "Manual"; }
     bool requires_armed_servos() const override { return true; }
     void update() override;
 };
@@ -80,6 +85,7 @@ public:
 class ModeScan : public Mode {
 public:
     Mode::Number number() const override { return Mode::Number::SCAN; }
+    const char* name() const override { return "Scan"; }
     bool requires_armed_servos() const override { return true; }
     void update() override;
 };
@@ -87,6 +93,7 @@ public:
 class ModeServoTest : public Mode {
 public:
     Mode::Number number() const override { return Mode::Number::SERVOTEST; }
+    const char* name() const override { return "ServoTest"; }
     bool requires_armed_servos() const override { return true; }
     void update() override {};
 
@@ -96,6 +103,7 @@ public:
 class ModeStop : public Mode {
 public:
     Mode::Number number() const override { return Mode::Number::STOP; }
+    const char* name() const override { return "Stop"; }
     bool requires_armed_servos() const override { return false; }
     void update() override {};
 };