From 231f300e7f51bcbd7f59e5902b05c7c375732d74 Mon Sep 17 00:00:00 2001
From: Peter Barker <pbarker@barker.dropbear.id.au>
Date: Mon, 10 Jul 2023 12:24:31 +1000
Subject: [PATCH] Use MAV_CMD_DO_SET_MODE to change modes on ArduPilot-base
 vehicles

ArduPilot has supported this command for 5.5 years now.  Probably time to start using it.  Copter 3.6.0 has it, 3.5.x does not.
---
 src/FirmwarePlugin/APM/APMFirmwarePlugin.h |  1 +
 src/FirmwarePlugin/FirmwarePlugin.h        |  3 +++
 src/Vehicle/Vehicle.cc                     | 26 +++++++++++++++++---------
 3 files changed, 21 insertions(+), 9 deletions(-)

diff --git a/src/FirmwarePlugin/APM/APMFirmwarePlugin.h b/src/FirmwarePlugin/APM/APMFirmwarePlugin.h
index bbef05f..89a3dbf 100644
--- a/src/FirmwarePlugin/APM/APMFirmwarePlugin.h
+++ b/src/FirmwarePlugin/APM/APMFirmwarePlugin.h
@@ -56,6 +56,7 @@ public:
     QStringList         flightModes                     (Vehicle* vehicle) override;
     QString             flightMode                      (uint8_t base_mode, uint32_t custom_mode) const override;
     bool                setFlightMode                   (const QString& flightMode, uint8_t* base_mode, uint32_t* custom_mode) override;
+    bool MAV_CMD_DO_SET_MODE_is_supported() const override { return true; }
     bool                isGuidedMode                    (const Vehicle* vehicle) const override;
     QString             gotoFlightMode                  (void) const override { return QStringLiteral("Guided"); }
     QString             rtlFlightMode                   (void) const override { return QString("RTL"); }
diff --git a/src/FirmwarePlugin/FirmwarePlugin.h b/src/FirmwarePlugin/FirmwarePlugin.h
index a0bb21e..a395674 100644
--- a/src/FirmwarePlugin/FirmwarePlugin.h
+++ b/src/FirmwarePlugin/FirmwarePlugin.h
@@ -102,6 +102,9 @@ public:
     ///     @param[out] custom_mode Custom mode for SET_MODE mavlink message
     virtual bool setFlightMode(const QString& flightMode, uint8_t* base_mode, uint32_t* custom_mode);
 
+    /// returns true if this flight stack supports MAV_CMD_DO_SET_MODE
+    virtual bool MAV_CMD_DO_SET_MODE_is_supported() const { return false; }
+
     /// Returns The flight mode which indicates the vehicle is paused
     virtual QString pauseFlightMode(void) const { return QString(); }
 
diff --git a/src/Vehicle/Vehicle.cc b/src/Vehicle/Vehicle.cc
index b8dfb92..a287e3b 100644
--- a/src/Vehicle/Vehicle.cc
+++ b/src/Vehicle/Vehicle.cc
@@ -2202,15 +2202,23 @@ void Vehicle::setFlightMode(const QString& flightMode)
         // states.
         newBaseMode |= base_mode;
 
-        mavlink_message_t msg;
-        mavlink_msg_set_mode_pack_chan(_mavlink->getSystemId(),
-                                       _mavlink->getComponentId(),
-                                       sharedLink->mavlinkChannel(),
-                                       &msg,
-                                       id(),
-                                       newBaseMode,
-                                       custom_mode);
-        sendMessageOnLinkThreadSafe(sharedLink.get(), msg);
+        if (_firmwarePlugin->MAV_CMD_DO_SET_MODE_is_supported()) {
+            sendMavCommand(defaultComponentId(),
+                           MAV_CMD_DO_SET_MODE,
+                           true,    // show error if fails
+                           MAV_MODE_FLAG_CUSTOM_MODE_ENABLED,
+                           custom_mode);
+        } else {
+            mavlink_message_t msg;
+            mavlink_msg_set_mode_pack_chan(_mavlink->getSystemId(),
+                                           _mavlink->getComponentId(),
+                                           sharedLink->mavlinkChannel(),
+                                           &msg,
+                                           id(),
+                                           newBaseMode,
+                                           custom_mode);
+            sendMessageOnLinkThreadSafe(sharedLink.get(), msg);
+        }
     } else {
         qCWarning(VehicleLog) << "FirmwarePlugin::setFlightMode failed, flightMode:" << flightMode;
     }