diff --git a/src/FactSystem/FactMetaData.cc b/src/FactSystem/FactMetaData.cc index e8d8dfa5efd..b92defe4724 100644 --- a/src/FactSystem/FactMetaData.cc +++ b/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); diff --git a/src/FactSystem/FactMetaData.h b/src/FactSystem/FactMetaData.h index 6b12bc2df79..06145cce568 100644 --- a/src/FactSystem/FactMetaData.h +++ b/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(); diff --git a/src/FirmwarePlugin/APM/APMFirmwarePlugin.cc b/src/FirmwarePlugin/APM/APMFirmwarePlugin.cc index 987826a2a6e..4e0fdefe14c 100644 --- a/src/FirmwarePlugin/APM/APMFirmwarePlugin.cc +++ b/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(airspeed_equiv), // speed setpoint + -1, // throttle, no change + 0 // 0: absolute speed, 1: relative to current + ); // param 5-7 unused +} \ No newline at end of file diff --git a/src/FirmwarePlugin/APM/APMFirmwarePlugin.h b/src/FirmwarePlugin/APM/APMFirmwarePlugin.h index 4a1dd068673..1530de15258 100644 --- a/src/FirmwarePlugin/APM/APMFirmwarePlugin.h +++ b/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 diff --git a/src/FirmwarePlugin/FirmwarePlugin.cc b/src/FirmwarePlugin/FirmwarePlugin.cc index 35d44b23492..32cd3a6af86 100644 --- a/src/FirmwarePlugin/FirmwarePlugin.cc +++ b/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); diff --git a/src/FirmwarePlugin/FirmwarePlugin.h b/src/FirmwarePlugin/FirmwarePlugin.h index f4f05a0139d..f9444b4eea3 100644 --- a/src/FirmwarePlugin/FirmwarePlugin.h +++ b/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 diff --git a/src/FirmwarePlugin/PX4/PX4FirmwarePlugin.cc b/src/FirmwarePlugin/PX4/PX4FirmwarePlugin.cc index b8fab2716ff..cf76d12af23 100644 --- a/src/FirmwarePlugin/PX4/PX4FirmwarePlugin.cc +++ b/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( diff --git a/src/FirmwarePlugin/PX4/PX4FirmwarePlugin.h b/src/FirmwarePlugin/PX4/PX4FirmwarePlugin.h index ff8e9eb8c02..a0a8d62ad48 100644 --- a/src/FirmwarePlugin/PX4/PX4FirmwarePlugin.h +++ b/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; diff --git a/src/FlightDisplay/GuidedActionsController.qml b/src/FlightDisplay/GuidedActionsController.qml index f6ce0e28dfd..26985353cb2 100644 --- a/src/FlightDisplay/GuidedActionsController.qml +++ b/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 diff --git a/src/FlightDisplay/GuidedValueSlider.qml b/src/FlightDisplay/GuidedValueSlider.qml index 1548aecdeb1..a7487c613cb 100644 --- a/src/FlightDisplay/GuidedValueSlider.qml +++ b/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) { diff --git a/src/QmlControls/QmlUnitsConversion.h b/src/QmlControls/QmlUnitsConversion.h index 267503dc787..769018f6198 100644 --- a/src/QmlControls/QmlUnitsConversion.h +++ b/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(); } diff --git a/src/Vehicle/Vehicle.cc b/src/Vehicle/Vehicle.cc index 570e3ab6935..b486f06fa5d 100644 --- a/src/Vehicle/Vehicle.cc +++ b/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) diff --git a/src/Vehicle/Vehicle.h b/src/Vehicle/Vehicle.h index 5a61c786aa6..6440908a08a 100644 --- a/src/Vehicle/Vehicle.h +++ b/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