FirmwwarePlugin: ArduPilot: add support for changing multicopter grou…

…ndspeed
mavlink · May 10, 2024 · c1bbb84 · c1bbb84
1 parent d446993
commit c1bbb84
Show file tree

Hide file tree

Showing 2 changed files with 34 additions and 0 deletions.
diff --git 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
@@ -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);