Add airspeed guided action support for Ardupilot fixed wing, fix speed change slider

src/FactSystem/FactMetaData.cc

@@ -1200,6 +1200,26 @@ QVariant FactMetaData::appSettingsWeightUnitsToGrams(const QVariant& weight) {
     }
 }
 
+QVariant FactMetaData::metersSecondToAppSettingsSpeedUnits(const QVariant& metersSecond)
+{
+    const AppSettingsTranslation_s* pAppSettingsTranslation = _findAppSettingsUnitsTranslation("m/s", UnitSpeed);
+    if (pAppSettingsTranslation) {
+        return pAppSettingsTranslation->rawTranslator(metersSecond);
+    } else {
+        return metersSecond;
+    }
+}
+
+QVariant FactMetaData::appSettingsSpeedUnitsToMetersSecond(const QVariant& speed)
+{
+    const AppSettingsTranslation_s* pAppSettingsTranslation = _findAppSettingsUnitsTranslation("m/s", UnitSpeed);
+    if (pAppSettingsTranslation) {
+        return pAppSettingsTranslation->cookedTranslator(speed);
+    } else {
+        return speed;
+    }
+}
+
 QString FactMetaData::appSettingsSpeedUnitsString()
 {
     const AppSettingsTranslation_s* pAppSettingsTranslation = _findAppSettingsUnitsTranslation("m/s", UnitSpeed);

src/FactSystem/FactMetaData.h

@@ -99,6 +99,12 @@ class FactMetaData : public QObject
     /// Returns the string for distance units which has configued by user
     static QString appSettingsAreaUnitsString(void);
 
+    /// Converts from meters/second to the user specified speed unit
+    static QVariant metersSecondToAppSettingsSpeedUnits(const QVariant& metersSecond);
+
+    /// Converts from user specified speed unit to meters/second
+    static QVariant appSettingsSpeedUnitsToMetersSecond(const QVariant& speed);
+
     /// Returns the string for speed units which has configued by user
     static QString appSettingsSpeedUnitsString();
 

src/FirmwarePlugin/APM/APMFirmwarePlugin.cc

@@ -1133,3 +1133,45 @@ QMutex& APMFirmwarePlugin::_reencodeMavlinkChannelMutex()
     static QMutex _mutex{};
     return _mutex;
 }
+
+double APMFirmwarePlugin::maximumEquivalentAirspeed(Vehicle* vehicle)
+{
+    QString airspeedMax("ARSPD_FBW_MAX");
+
+    if (vehicle->parameterManager()->parameterExists(FactSystem::defaultComponentId, airspeedMax)) {
+        return vehicle->parameterManager()->getParameter(FactSystem::defaultComponentId, airspeedMax)->rawValue().toDouble();
+    }
+
+    return FirmwarePlugin::maximumEquivalentAirspeed(vehicle);
+}
+
+double APMFirmwarePlugin::minimumEquivalentAirspeed(Vehicle* vehicle)
+{
+    QString airspeedMin("ARSPD_FBW_MIN");
+
+    if (vehicle->parameterManager()->parameterExists(FactSystem::defaultComponentId, airspeedMin)) {
+        return vehicle->parameterManager()->getParameter(FactSystem::defaultComponentId, airspeedMin)->rawValue().toDouble();
+    }
+
+    return FirmwarePlugin::minimumEquivalentAirspeed(vehicle);
+}
+
+bool APMFirmwarePlugin::fixedWingAirSpeedLimitsAvailable(Vehicle* vehicle)
+{
+    return vehicle->parameterManager()->parameterExists(FactSystem::defaultComponentId, "ARSPD_FBW_MIN") &&
+           vehicle->parameterManager()->parameterExists(FactSystem::defaultComponentId, "ARSPD_FBW_MAX");
+}
+
+void APMFirmwarePlugin::guidedModeChangeEquivalentAirspeedMetersSecond(Vehicle* vehicle, double airspeed_equiv)
+{
+
+    vehicle->sendMavCommand(
+        vehicle->defaultComponentId(),
+        MAV_CMD_DO_CHANGE_SPEED,
+        true,                                 // show error is fails
+        0,                                    // 0: airspeed, 1: groundspeed
+        static_cast<float>(airspeed_equiv),   // speed setpoint
+        -1,                                   // throttle, no change
+        0                                     // 0: absolute speed, 1: relative to current
+        );                                    // param 5-7 unused
+}

src/FirmwarePlugin/APM/APMFirmwarePlugin.h

@@ -80,6 +80,10 @@ class APMFirmwarePlugin : public FirmwarePlugin
     QString             getHobbsMeter                   (Vehicle* vehicle) override; 
     bool                hasGripper                      (const Vehicle* vehicle) const override;
     const QVariantList& toolIndicators                  (const Vehicle* vehicle) override;
+    double              maximumEquivalentAirspeed       (Vehicle* vehicle) override;
+    double              minimumEquivalentAirspeed       (Vehicle* vehicle) override;
+    bool                fixedWingAirSpeedLimitsAvailable(Vehicle* vehicle) override;
+    void                guidedModeChangeEquivalentAirspeedMetersSecond(Vehicle* vehicle, double airspeed_equiv) override;
 
 protected:
     /// All access to singleton is through stack specific implementation

src/FirmwarePlugin/FirmwarePlugin.cc

@@ -276,14 +276,14 @@ void FirmwarePlugin::guidedModeChangeAltitude(Vehicle*, double, bool pauseVehicl
 }
 
 void
-FirmwarePlugin::guidedModeChangeGroundSpeed(Vehicle*, double)
+FirmwarePlugin::guidedModeChangeGroundSpeedMetersSecond(Vehicle*, double)
 {
     // Not supported by generic vehicle
     qgcApp()->showAppMessage(guided_mode_not_supported_by_vehicle);
 }
 
 void
-FirmwarePlugin::guidedModeChangeEquivalentAirspeed(Vehicle*, double)
+FirmwarePlugin::guidedModeChangeEquivalentAirspeedMetersSecond(Vehicle*, double)
 {
     // Not supported by generic vehicle
     qgcApp()->showAppMessage(guided_mode_not_supported_by_vehicle);

src/FirmwarePlugin/FirmwarePlugin.h

@@ -180,11 +180,11 @@ class FirmwarePlugin : public QObject
 
         /// Command vehicle to change groundspeed
     ///     @param groundspeed Groundspeed in m/s
-    virtual void guidedModeChangeGroundSpeed(Vehicle* vehicle, double groundspeed);
+    virtual void guidedModeChangeGroundSpeedMetersSecond(Vehicle* vehicle, double groundspeed);
 
     /// Command vehicle to change equivalent airspeed
     ///     @param airspeed_equiv Equivalent airspeed in m/s
-    virtual void guidedModeChangeEquivalentAirspeed(Vehicle* vehicle, double airspeed_equiv);
+    virtual void guidedModeChangeEquivalentAirspeedMetersSecond(Vehicle* vehicle, double airspeed_equiv);
 
     /// Default tx mode to apply to joystick axes
     /// TX modes are as outlined here: http://www.rc-airplane-world.com/rc-transmitter-modes.html

src/FirmwarePlugin/PX4/PX4FirmwarePlugin.cc

@@ -572,7 +572,7 @@ void PX4FirmwarePlugin::guidedModeChangeAltitude(Vehicle* vehicle, double altitu
     }
 }
 
-void PX4FirmwarePlugin::guidedModeChangeGroundSpeed(Vehicle* vehicle, double groundspeed)
+void PX4FirmwarePlugin::guidedModeChangeGroundSpeedMetersSecond(Vehicle* vehicle, double groundspeed)
 {
 
     vehicle->sendMavCommand(
@@ -586,7 +586,7 @@ void PX4FirmwarePlugin::guidedModeChangeGroundSpeed(Vehicle* vehicle, double gro
         NAN, NAN,NAN);                        // param 5-7 unused
 }
 
-void PX4FirmwarePlugin::guidedModeChangeEquivalentAirspeed(Vehicle* vehicle, double airspeed_equiv)
+void PX4FirmwarePlugin::guidedModeChangeEquivalentAirspeedMetersSecond(Vehicle* vehicle, double airspeed_equiv)
 {
 
     vehicle->sendMavCommand(

src/FirmwarePlugin/PX4/PX4FirmwarePlugin.h

@@ -53,8 +53,8 @@ class PX4FirmwarePlugin : public FirmwarePlugin
     bool                fixedWingAirSpeedLimitsAvailable(Vehicle* vehicle) override;
     void                guidedModeGotoLocation          (Vehicle* vehicle, const QGeoCoordinate& gotoCoord) override;
     void                guidedModeChangeAltitude        (Vehicle* vehicle, double altitudeRel, bool pauseVehicle) override;
-    void                guidedModeChangeGroundSpeed      (Vehicle* vehicle, double groundspeed) override;
-    void                guidedModeChangeEquivalentAirspeed(Vehicle* vehicle, double airspeed_equiv) override;
+    void                guidedModeChangeGroundSpeedMetersSecond(Vehicle* vehicle, double groundspeed) override;
+    void                guidedModeChangeEquivalentAirspeedMetersSecond(Vehicle* vehicle, double airspeed_equiv) override;
     void                startMission                    (Vehicle* vehicle) override;
     bool                isGuidedMode                    (const Vehicle* vehicle) const override;
     void                initializeVehicle               (Vehicle* vehicle) override;

src/FlightDisplay/GuidedActionsController.qml

@@ -190,22 +190,24 @@ Item {
     function setupSlider(actionCode) {
         // generic defaults
         guidedValueSlider.configureAsLinearSlider()
+        guidedValueSlider.setIsSpeedSlider(false)
 
         if (actionCode === actionTakeoff) {
                 guidedValueSlider.setMinVal(_activeVehicle.minimumTakeoffAltitude())
                 guidedValueSlider.setValue(_activeVehicle ? _activeVehicle.minimumTakeoffAltitude() : 0)
                 guidedValueSlider.setDisplayText("Height")
         } else if (actionCode === actionChangeSpeed) {
-            if (_activeVehicle.vtolInFwdFlight) {
+            guidedValueSlider.setIsSpeedSlider(true)
+            if (_fixedWing) {
                 guidedValueSlider.setDisplayText("Set Airspeed")
-                guidedValueSlider.setMinVal(_activeVehicle.minimumEquivalentAirspeed())
-                guidedValueSlider.setMaxVal(_activeVehicle.maximumEquivalentAirspeed())
-                guidedValueSlider.setValue(_activeVehicle.airSpeedSetpoint.rawValue)
-            } else {
+                guidedValueSlider.setMinVal(QGroundControl.unitsConversion.metersSecondToAppSettingsSpeedUnits(_activeVehicle.minimumEquivalentAirspeed()).toFixed(1))
+                guidedValueSlider.setMaxVal(QGroundControl.unitsConversion.metersSecondToAppSettingsSpeedUnits(_activeVehicle.maximumEquivalentAirspeed()).toFixed(1))
+                guidedValueSlider.setValue(_activeVehicle.airSpeed.value)
+            } else if (!_fixedWing && _activeVehicle.haveMRSpeedLimits) {
                 guidedValueSlider.setDisplayText("Set Speed")
-                guidedValueSlider.setMinVal(0.1)
-                guidedValueSlider.setMaxVal(_activeVehicle.maximumHorizontalSpeedMultirotor())
-                guidedValueSlider.setValue(_activeVehicle.maximumHorizontalSpeedMultirotor()/2)
+                guidedValueSlider.setMinVal(QGroundControl.unitsConversion.metersSecondToAppSettingsSpeedUnits(0.1).toFixed(1))
+                guidedValueSlider.setMaxVal(QGroundControl.unitsConversion.metersSecondToAppSettingsSpeedUnits(_activeVehicle.maximumHorizontalSpeedMultirotor()).toFixed(1))
+                guidedValueSlider.setValue(QGroundControl.unitsConversion.metersSecondToAppSettingsSpeedUnits(_activeVehicle.maximumHorizontalSpeedMultirotor()/2).toFixed(1))
             }
         } else if (actionCode === actionChangeAlt || actionCode === actionOrbit || actionCode === actionGoto || actionCode === actionPause) {
             guidedValueSlider.setDisplayText("New Alt(rel)")
@@ -599,10 +601,12 @@ Item {
             break
         case actionChangeSpeed:
             if (_activeVehicle) {
+                // We need to convert back to m/s as that is what mavlink standard uses for MAV_CMD_DO_CHANGE_SPEED
+                var metersSecondSpeed = QGroundControl.unitsConversion.appSettingsSpeedUnitsToMetersSecond(sliderOutputValue).toFixed(1)
                 if (_activeVehicle.vtolInFwdFlight || _activeVehicle.fixedWing) {
-                   _activeVehicle.guidedModeChangeEquivalentAirspeed(sliderOutputValue)
+                   _activeVehicle.guidedModeChangeEquivalentAirspeedMetersSecond(metersSecondSpeed)
                 } else {
-                    _activeVehicle.guidedModeChangeGroundSpeed(sliderOutputValue)
+                    _activeVehicle.guidedModeChangeGroundSpeedMetersSecond(metersSecondSpeed)
                 }
             }
             break

src/FlightDisplay/GuidedValueSlider.qml

@@ -26,6 +26,7 @@ Rectangle {
     property real _sliderCenterValue:   _vehicleAltitude
     property string _displayText:       ""
     property bool _altSlider:         true
+    property bool _speedSlider:       false
 
     property var sliderValue : valueSlider.value
 
@@ -75,6 +76,10 @@ Rectangle {
         _displayText = text
     }
 
+    function setIsSpeedSlider(isSpeed) {
+        _speedSlider = isSpeed
+    }
+
     function getOutputValue() {
         if (_altSlider) {
             return valueField.newValue - _sliderCenterValue
@@ -101,7 +106,8 @@ Rectangle {
         QGCLabel {
             id:                         valueField
             anchors.horizontalCenter:   parent.horizontalCenter
-            text:                       newValueAppUnits + " " + QGroundControl.unitsConversion.appSettingsHorizontalDistanceUnitsString
+            text:                       newValueAppUnits + " " + (_speedSlider ? QGroundControl.unitsConversion.appSettingsSpeedUnitsString
+                                                                               : QGroundControl.unitsConversion.appSettingsHorizontalDistanceUnitsString)
 
             property real   newValue
             property string newValueAppUnits
@@ -120,7 +126,12 @@ Rectangle {
             function updateLinear(value) {
                 // value is between -1 and 1
                 newValue = _sliderMinVal + (value + 1) * 0.5 * (_sliderMaxVal - _sliderMinVal)
-                newValueAppUnits = QGroundControl.unitsConversion.metersToAppSettingsHorizontalDistanceUnits(newValue).toFixed(1)
+                if (_speedSlider) {
+                    // Already working in converted units
+                    newValueAppUnits = newValue.toFixed(1)
+                } else {
+                    newValueAppUnits = QGroundControl.unitsConversion.metersToAppSettingsHorizontalDistanceUnits(newValue).toFixed(1)
+                }
             }
 
             function getSliderValueFromOutputLinear(val) {

src/QmlControls/QmlUnitsConversion.h

@@ -59,6 +59,12 @@ class QmlUnitsConversion : public QObject
 
     QString appSettingsAreaUnitsString(void) const { return FactMetaData::appSettingsAreaUnitsString(); }
 
+    /// Converts from meters/second to the user specified speed unit
+    Q_INVOKABLE QVariant metersSecondToAppSettingsSpeedUnits(const QVariant& metersSecond) const { return FactMetaData::metersSecondToAppSettingsSpeedUnits(metersSecond); }
+
+    /// Converts from user specified speed unit to meters/second
+    Q_INVOKABLE QVariant appSettingsSpeedUnitsToMetersSecond(const QVariant& speed) const { return FactMetaData::appSettingsSpeedUnitsToMetersSecond(speed); }
+
     /// Returns the string for speed units which has configued by user
     QString appSettingsSpeedUnitsString() { return FactMetaData::appSettingsSpeedUnitsString(); }
 

src/Vehicle/Vehicle.cc

@@ -2775,23 +2775,23 @@ void Vehicle::guidedModeChangeAltitude(double altitudeChange, bool pauseVehicle)
 }
 
 void
-Vehicle::guidedModeChangeGroundSpeed(double groundspeed)
+Vehicle::guidedModeChangeGroundSpeedMetersSecond(double groundspeed)
 {
     if (!guidedModeSupported()) {
         qgcApp()->showAppMessage(guided_mode_not_supported_by_vehicle);
         return;
     }
-    _firmwarePlugin->guidedModeChangeGroundSpeed(this, groundspeed);
+    _firmwarePlugin->guidedModeChangeGroundSpeedMetersSecond(this, groundspeed);
 }
 
 void
-Vehicle::guidedModeChangeEquivalentAirspeed(double airspeed)
+Vehicle::guidedModeChangeEquivalentAirspeedMetersSecond(double airspeed)
 {
     if (!guidedModeSupported()) {
         qgcApp()->showAppMessage(guided_mode_not_supported_by_vehicle);
         return;
     }
-    _firmwarePlugin->guidedModeChangeEquivalentAirspeed(this, airspeed);
+    _firmwarePlugin->guidedModeChangeEquivalentAirspeedMetersSecond(this, airspeed);
 }
 
 void Vehicle::guidedModeOrbit(const QGeoCoordinate& centerCoord, double radius, double amslAltitude)

src/Vehicle/Vehicle.h

@@ -378,10 +378,10 @@ class Vehicle : public FactGroup
 
     /// Command vehicle to change groundspeed
     ///     @param groundspeed Target horizontal groundspeed
-    Q_INVOKABLE void guidedModeChangeGroundSpeed   (double groundspeed);
+    Q_INVOKABLE void guidedModeChangeGroundSpeedMetersSecond(double groundspeed);
     /// Command vehicle to change equivalent airspeed
     ///     @param airspeed Target equivalent airspeed
-    Q_INVOKABLE void guidedModeChangeEquivalentAirspeed   (double airspeed);
+    Q_INVOKABLE void guidedModeChangeEquivalentAirspeedMetersSecond(double airspeed);
 
     /// Command vehicle to orbit given center point
     ///     @param centerCoord Orit around this point