From c1bbb844adccdbc590e701bb1506ecaae848fc95 Mon Sep 17 00:00:00 2001
From: Peter Barker <pbarker@barker.dropbear.id.au>
Date: Thu, 9 May 2024 17:23:42 +1000
Subject: [PATCH] FirmwwarePlugin: ArduPilot: add support for changing
 multicopter groundspeed

---
 src/FirmwarePlugin/APM/APMFirmwarePlugin.cc | 29 +++++++++++++++++++++
 src/FirmwarePlugin/APM/APMFirmwarePlugin.h  |  5 ++++
 2 files changed, 34 insertions(+)

diff --git a/src/FirmwarePlugin/APM/APMFirmwarePlugin.cc b/src/FirmwarePlugin/APM/APMFirmwarePlugin.cc
index a68f3d9b1f6..4fff703aa67 100644
--- a/src/FirmwarePlugin/APM/APMFirmwarePlugin.cc
+++ b/src/FirmwarePlugin/APM/APMFirmwarePlugin.cc
@@ -897,6 +897,35 @@ void APMFirmwarePlugin::guidedModeChangeAltitude(Vehicle* vehicle, double altitu
     }
 }
 
+bool APMFirmwarePlugin::mulirotorSpeedLimitsAvailable(Vehicle* vehicle)
+{
+    return vehicle->parameterManager()->parameterExists(FactSystem::defaultComponentId, "WPNAV_SPEED");
+}
+
+double APMFirmwarePlugin::maximumHorizontalSpeedMultirotor(Vehicle* vehicle)
+{
+    QString speedParam("WPNAV_SPEED");
+
+    if (vehicle->parameterManager()->parameterExists(FactSystem::defaultComponentId, speedParam)) {
+        return vehicle->parameterManager()->getParameter(FactSystem::defaultComponentId, speedParam)->rawValue().toDouble() * 0.01;  // note cm/s -> m/s
+    }
+
+    return FirmwarePlugin::maximumHorizontalSpeedMultirotor(vehicle);
+}
+
+void APMFirmwarePlugin::guidedModeChangeGroundSpeedMetersSecond(Vehicle *vehicle, double groundspeed)
+{
+    vehicle->sendMavCommand(
+        vehicle->defaultComponentId(),
+        MAV_CMD_DO_CHANGE_SPEED,
+        true,                                   // show error is fails
+        1,                                     // 0: airspeed, 1: groundspeed
+        static_cast<float>(groundspeed),       // groundspeed setpoint
+        -1,                                   // throttle
+        0,                                    // 0: absolute speed, 1: relative to current
+        NAN, NAN,NAN);                        // param 5-7 unused
+}
+
 void APMFirmwarePlugin::guidedModeTakeoff(Vehicle* vehicle, double altitudeRel)
 {
     _guidedModeTakeoff(vehicle, altitudeRel);
diff --git a/src/FirmwarePlugin/APM/APMFirmwarePlugin.h b/src/FirmwarePlugin/APM/APMFirmwarePlugin.h
index 37b89f8bf03..ffc3e658712 100644
--- a/src/FirmwarePlugin/APM/APMFirmwarePlugin.h
+++ b/src/FirmwarePlugin/APM/APMFirmwarePlugin.h
@@ -87,6 +87,11 @@ class APMFirmwarePlugin : public FirmwarePlugin
     void                guidedModeChangeEquivalentAirspeedMetersSecond(Vehicle* vehicle, double airspeed_equiv) override;
     QVariant            mainStatusIndicatorContentItem  (const Vehicle* vehicle) const override;
 
+    // support for changing speed in Copter guide mode:
+    bool mulirotorSpeedLimitsAvailable(Vehicle* vehicle);
+    double maximumHorizontalSpeedMultirotor(Vehicle* vehicle);
+    void guidedModeChangeGroundSpeedMetersSecond(Vehicle *vehicle, double speed);
+
 protected:
     /// All access to singleton is through stack specific implementation
     APMFirmwarePlugin(void);