Browse Source

support speed changes from the flyview

- allow changing maximum horizontal cruise speed for multirotors
- allow changing equivalent airspeed setpoint for fixed wings

Signed-off-by: RomanBapst <bapstroman@gmail.com>
QGC4.4
RomanBapst 3 years ago committed by Ramon Roche
parent
commit
e51a859b49
  1. 2
      qgroundcontrol.qrc
  2. 14
      src/FirmwarePlugin/FirmwarePlugin.cc
  3. 17
      src/FirmwarePlugin/FirmwarePlugin.h
  4. 67
      src/FirmwarePlugin/PX4/PX4FirmwarePlugin.cc
  5. 5
      src/FirmwarePlugin/PX4/PX4FirmwarePlugin.h
  6. 14
      src/FlightDisplay/FlyView.qml
  7. 14
      src/FlightDisplay/GuidedActionConfirm.qml
  8. 10
      src/FlightDisplay/GuidedActionList.qml
  9. 80
      src/FlightDisplay/GuidedActionsController.qml
  10. 36
      src/FlightDisplay/GuidedValueSlider.qml
  11. 2
      src/FlightDisplay/TelemetryValuesBar.qml
  12. 2
      src/QmlControls/QGroundControl/FlightDisplay/qmldir
  13. 37
      src/Vehicle/Vehicle.cc
  14. 16
      src/Vehicle/Vehicle.h

2
qgroundcontrol.qrc

@ -220,7 +220,7 @@ @@ -220,7 +220,7 @@
<file alias="QGroundControl/FlightDisplay/GuidedActionTakeoff.qml">src/FlightDisplay/GuidedActionTakeoff.qml</file>
<file alias="QGroundControl/FlightDisplay/GuidedActionPause.qml">src/FlightDisplay/GuidedActionPause.qml</file>
<file alias="QGroundControl/FlightDisplay/GuidedActionRTL.qml">src/FlightDisplay/GuidedActionRTL.qml</file>
<file alias="QGroundControl/FlightDisplay/GuidedAltitudeSlider.qml">src/FlightDisplay/GuidedAltitudeSlider.qml</file>
<file alias="QGroundControl/FlightDisplay/GuidedValueSlider.qml">src/FlightDisplay/GuidedValueSlider.qml</file>
<file alias="QGroundControl/FlightDisplay/GuidedToolStripAction.qml">src/FlightDisplay/GuidedToolStripAction.qml</file>
<file alias="QGroundControl/FlightDisplay/MultiVehicleList.qml">src/FlightDisplay/MultiVehicleList.qml</file>
<file alias="QGroundControl/FlightDisplay/PreFlightBatteryCheck.qml">src/FlightDisplay/PreFlightBatteryCheck.qml</file>

14
src/FirmwarePlugin/FirmwarePlugin.cc

@ -275,6 +275,20 @@ void FirmwarePlugin::guidedModeChangeAltitude(Vehicle*, double, bool pauseVehicl @@ -275,6 +275,20 @@ void FirmwarePlugin::guidedModeChangeAltitude(Vehicle*, double, bool pauseVehicl
qgcApp()->showAppMessage(guided_mode_not_supported_by_vehicle);
}
void
FirmwarePlugin::guidedModeChangeGroundSpeed(Vehicle*, double)
{
// Not supported by generic vehicle
qgcApp()->showAppMessage(guided_mode_not_supported_by_vehicle);
}
void
FirmwarePlugin::guidedModeChangeEquivalentAirspeed(Vehicle*, double)
{
// Not supported by generic vehicle
qgcApp()->showAppMessage(guided_mode_not_supported_by_vehicle);
}
void FirmwarePlugin::startMission(Vehicle*)
{
// Not supported by generic vehicle

17
src/FirmwarePlugin/FirmwarePlugin.h

@ -150,6 +150,15 @@ public: @@ -150,6 +150,15 @@ public:
/// @return The minimum takeoff altitude (relative) for guided takeoff.
virtual double minimumTakeoffAltitude(Vehicle* /*vehicle*/) { return 10; }
/// @return The maximum horizontal groundspeed for a multirotor.
virtual double maximumHorizontalSpeedMultirotor(Vehicle* /*vehicle*/) { return 10; }
/// @return The maximum equivalent airspeed setpoint.
virtual double maximumEquivalentAirspeed(Vehicle* /*vehicle*/) { return 25; }
/// @return The minimum equivalent airspeed setpoint
virtual double minimumEquivalentAirspeed(Vehicle* /*vehicle*/) { return 12; }
/// Command the vehicle to start the mission
virtual void startMission(Vehicle* vehicle);
@ -161,6 +170,14 @@ public: @@ -161,6 +170,14 @@ public:
/// @param pauseVehicle true: pause vehicle prior to altitude change
virtual void guidedModeChangeAltitude(Vehicle* vehicle, double altitudeChange, bool pauseVehicle);
/// Command vehicle to change groundspeed
/// @param groundspeed Groundspeed in m/s
virtual void guidedModeChangeGroundSpeed(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);
/// Default tx mode to apply to joystick axes
/// TX modes are as outlined here: http://www.rc-airplane-world.com/rc-transmitter-modes.html
virtual int defaultJoystickTXMode(void);

67
src/FirmwarePlugin/PX4/PX4FirmwarePlugin.cc

@ -409,6 +409,39 @@ void PX4FirmwarePlugin::guidedModeTakeoff(Vehicle* vehicle, double takeoffAltRel @@ -409,6 +409,39 @@ void PX4FirmwarePlugin::guidedModeTakeoff(Vehicle* vehicle, double takeoffAltRel
static_cast<float>(takeoffAltAMSL)); // AMSL altitude
}
double PX4FirmwarePlugin::maximumHorizontalSpeedMultirotor(Vehicle* vehicle)
{
QString speedParam("MPC_XY_VEL_MAX");
if (vehicle->parameterManager()->parameterExists(FactSystem::defaultComponentId, speedParam)) {
return vehicle->parameterManager()->getParameter(FactSystem::defaultComponentId, speedParam)->rawValue().toDouble();
} else {
return FirmwarePlugin::maximumHorizontalSpeedMultirotor(vehicle);
}
}
double PX4FirmwarePlugin::maximumEquivalentAirspeed(Vehicle* vehicle)
{
QString airspeedMax("FW_AIRSPD_MAX");
if (vehicle->parameterManager()->parameterExists(FactSystem::defaultComponentId, airspeedMax)) {
return vehicle->parameterManager()->getParameter(FactSystem::defaultComponentId, airspeedMax)->rawValue().toDouble();
} else {
return FirmwarePlugin::maximumEquivalentAirspeed(vehicle);
}
}
double PX4FirmwarePlugin::minimumEquivalentAirspeed(Vehicle* vehicle)
{
QString airspeedMin("FW_AIRSPD_MIN");
if (vehicle->parameterManager()->parameterExists(FactSystem::defaultComponentId, airspeedMin)) {
return vehicle->parameterManager()->getParameter(FactSystem::defaultComponentId, airspeedMin)->rawValue().toDouble();
} else {
return FirmwarePlugin::minimumEquivalentAirspeed(vehicle);
}
}
void PX4FirmwarePlugin::guidedModeGotoLocation(Vehicle* vehicle, const QGeoCoordinate& gotoCoord)
{
if (qIsNaN(vehicle->altitudeAMSL()->rawValue().toDouble())) {
@ -525,6 +558,34 @@ void PX4FirmwarePlugin::guidedModeChangeAltitude(Vehicle* vehicle, double altitu @@ -525,6 +558,34 @@ void PX4FirmwarePlugin::guidedModeChangeAltitude(Vehicle* vehicle, double altitu
}
}
void PX4FirmwarePlugin::guidedModeChangeGroundSpeed(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 PX4FirmwarePlugin::guidedModeChangeEquivalentAirspeed(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), // groundspeed setpoint
-1, // throttle
0, // 0: absolute speed, 1: relative to current
NAN, NAN,NAN); // param 5-7 unused
}
void PX4FirmwarePlugin::startMission(Vehicle* vehicle)
{
if (_setFlightModeAndValidate(vehicle, missionFlightMode())) {
@ -633,7 +694,7 @@ bool PX4FirmwarePlugin::supportsNegativeThrust(Vehicle* vehicle) @@ -633,7 +694,7 @@ bool PX4FirmwarePlugin::supportsNegativeThrust(Vehicle* vehicle)
return ((vehicle->vehicleType() == MAV_TYPE_GROUND_ROVER) || (vehicle->vehicleType() == MAV_TYPE_SUBMARINE));
}
QString PX4FirmwarePlugin::getHobbsMeter(Vehicle* vehicle)
QString PX4FirmwarePlugin::getHobbsMeter(Vehicle* vehicle)
{
static const char* HOOBS_HI = "LND_FLIGHT_T_HI";
static const char* HOOBS_LO = "LND_FLIGHT_T_LO";
@ -645,7 +706,7 @@ QString PX4FirmwarePlugin::getHobbsMeter(Vehicle* vehicle) @@ -645,7 +706,7 @@ QString PX4FirmwarePlugin::getHobbsMeter(Vehicle* vehicle)
Fact* factLo = vehicle->parameterManager()->getParameter(FactSystem::defaultComponentId, HOOBS_LO);
hobbsTimeSeconds = ((uint64_t)factHi->rawValue().toUInt() << 32 | (uint64_t)factLo->rawValue().toUInt()) / 1000000;
qCDebug(VehicleLog) << "Hobbs Meter raw PX4:" << "(" << factHi->rawValue().toUInt() << factLo->rawValue().toUInt() << ")";
}
}
int hours = hobbsTimeSeconds / 3600;
int minutes = (hobbsTimeSeconds % 3600) / 60;
@ -653,4 +714,4 @@ QString PX4FirmwarePlugin::getHobbsMeter(Vehicle* vehicle) @@ -653,4 +714,4 @@ QString PX4FirmwarePlugin::getHobbsMeter(Vehicle* vehicle)
QString timeStr = QString::asprintf("%04d:%02d:%02d", hours, minutes, seconds);
qCDebug(VehicleLog) << "Hobbs Meter string:" << timeStr;
return timeStr;
}
}

5
src/FirmwarePlugin/PX4/PX4FirmwarePlugin.h

@ -46,8 +46,13 @@ public: @@ -46,8 +46,13 @@ public:
void guidedModeRTL (Vehicle* vehicle, bool smartRTL) override;
void guidedModeLand (Vehicle* vehicle) override;
void guidedModeTakeoff (Vehicle* vehicle, double takeoffAltRel) override;
double maximumHorizontalSpeedMultirotor(Vehicle* vehicle) override;
double maximumEquivalentAirspeed(Vehicle* vehicle) override;
double minimumEquivalentAirspeed(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 startMission (Vehicle* vehicle) override;
bool isGuidedMode (const Vehicle* vehicle) const override;
void initializeVehicle (Vehicle* vehicle) override;

14
src/FlightDisplay/FlyView.qml

@ -51,7 +51,7 @@ Item { @@ -51,7 +51,7 @@ Item {
property real _margins: ScreenTools.defaultFontPixelWidth / 2
property var _guidedController: guidedActionsController
property var _guidedActionList: guidedActionList
property var _guidedAltSlider: guidedAltSlider
property var _guidedValueSlider: guidedValueSlider
property real _toolsMargin: ScreenTools.defaultFontPixelWidth * 0.75
property rect _centerViewport: Qt.rect(0, 0, width, height)
property real _rightPanelWidth: ScreenTools.defaultFontPixelWidth * 30
@ -79,7 +79,7 @@ Item { @@ -79,7 +79,7 @@ Item {
anchors.top: parent.top
anchors.bottom: parent.bottom
anchors.left: parent.left
anchors.right: guidedAltSlider.visible ? guidedAltSlider.left : parent.right
anchors.right: guidedValueSlider.visible ? guidedValueSlider.left : parent.right
z: _fullItemZorder + 1
parentToolInsets: _toolInsets
mapControl: _mapControl
@ -99,7 +99,7 @@ Item { @@ -99,7 +99,7 @@ Item {
id: guidedActionsController
missionController: _missionController
actionList: _guidedActionList
altitudeSlider: _guidedAltSlider
guidedValueSlider: _guidedValueSlider
}
/*GuidedActionConfirm {
@ -109,7 +109,7 @@ Item { @@ -109,7 +109,7 @@ Item {
anchors.horizontalCenter: parent.horizontalCenter
z: QGroundControl.zOrderTopMost
guidedController: _guidedController
altitudeSlider: _guidedAltSlider
guidedValueSlider: _guidedValueSlider
}*/
GuidedActionList {
@ -121,9 +121,9 @@ Item { @@ -121,9 +121,9 @@ Item {
guidedController: _guidedController
}
//-- Altitude slider
GuidedAltitudeSlider {
id: guidedAltSlider
//-- Guided value slider (e.g. altitude)
GuidedValueSlider {
id: guidedValueSlider
anchors.margins: _toolsMargin
anchors.right: parent.right
anchors.top: parent.top

14
src/FlightDisplay/GuidedActionConfirm.qml

@ -25,7 +25,7 @@ Rectangle { @@ -25,7 +25,7 @@ Rectangle {
visible: false
property var guidedController
property var altitudeSlider
property var guidedValueSlider
property string title // Currently unused
property alias message: messageText.text
property int action
@ -57,7 +57,7 @@ Rectangle { @@ -57,7 +57,7 @@ Rectangle {
}
function confirmCancelled() {
altitudeSlider.visible = false
guidedValueSlider.visible = false
visible = false
hideTrigger = false
visibleTimer.stop()
@ -106,13 +106,13 @@ Rectangle { @@ -106,13 +106,13 @@ Rectangle {
onAccept: {
_root.visible = false
var altitudeChange = 0
if (altitudeSlider.visible) {
altitudeChange = altitudeSlider.getAltitudeChangeValue()
altitudeSlider.visible = false
var sliderOutputValue = 0
if (guidedValueSlider.visible) {
sliderOutputValue = guidedValueSlider.getOutputValue()
guidedValueSlider.visible = false
}
hideTrigger = false
guidedController.executeAction(_root.action, _root.actionData, altitudeChange, _root.optionChecked)
guidedController.executeAction(_root.action, _root.actionData, sliderOutputValue, _root.optionChecked)
if (mapIndicator) {
mapIndicator.actionConfirmed()
mapIndicator = undefined

10
src/FlightDisplay/GuidedActionList.qml

@ -28,7 +28,7 @@ Rectangle { @@ -28,7 +28,7 @@ Rectangle {
visible: false
property var guidedController
property var altitudeSlider
property var guidedValueSlider
function show() {
visible = true
@ -62,6 +62,12 @@ Rectangle { @@ -62,6 +62,12 @@ Rectangle {
text: guidedController.landAbortMessage,
action: guidedController.actionLandAbort,
visible: guidedController.showLandAbort
},
{
title: guidedController.changeSpeedTitle,
text: guidedController.changeSpeedMessage,
action: guidedController.actionChangeSpeed,
visible: guidedController.showChangeSpeed
}
]
@ -90,7 +96,7 @@ Rectangle { @@ -90,7 +96,7 @@ Rectangle {
Layout.minimumWidth: _width
Layout.maximumWidth: _width
property real _width: Math.min((_actionWidth * 2) + _actionHorizSpacing, actionRow.width)
property real _width: Math.min((_actionWidth * 3) + _actionHorizSpacing*2, actionRow.width)
RowLayout {
id: actionRow

80
src/FlightDisplay/GuidedActionsController.qml

@ -30,7 +30,7 @@ Item { @@ -30,7 +30,7 @@ Item {
property var missionController
property var confirmDialog
property var actionList
property var altitudeSlider
property var guidedValueSlider
property var orbitMapCircle
readonly property string emergencyStopTitle: qsTr("EMERGENCY STOP")
@ -47,6 +47,8 @@ Item { @@ -47,6 +47,8 @@ Item {
readonly property string pauseTitle: qsTr("Pause")
readonly property string mvPauseTitle: qsTr("Pause (MV)")
readonly property string changeAltTitle: qsTr("Change Altitude")
readonly property string changeCruiseSpeedTitle: qsTr("Change Cruise Speed")
readonly property string changeAirspeedTitle: qsTr("Change Airspeed")
readonly property string orbitTitle: qsTr("Orbit")
readonly property string landAbortTitle: qsTr("Land Abort")
readonly property string setWaypointTitle: qsTr("Set Waypoint")
@ -66,6 +68,8 @@ Item { @@ -66,6 +68,8 @@ Item {
readonly property string landMessage: qsTr("Land the vehicle at the current position.")
readonly property string rtlMessage: qsTr("Return to the launch position of the vehicle.")
readonly property string changeAltMessage: qsTr("Change the altitude of the vehicle up or down.")
readonly property string changeCruiseSpeedMessage: qsTr("Change the maximum horizontal cruise speed.")
readonly property string changeAirspeedMessage: qsTr("Change the equivalent airspeed setpoint")
readonly property string gotoMessage: qsTr("Move the vehicle to the specified location.")
property string setWaypointMessage: qsTr("Adjust current waypoint to %1.").arg(_actionData)
readonly property string orbitMessage: qsTr("Orbit the vehicle around the specified location.")
@ -100,6 +104,7 @@ Item { @@ -100,6 +104,7 @@ Item {
readonly property int actionROI: 22
readonly property int actionActionList: 23
readonly property int actionForceArm: 24
readonly property int actionChangeSpeed: 25
property var _activeVehicle: QGroundControl.multiVehicleManager.activeVehicle
property bool _useChecklist: QGroundControl.settingsManager.appSettings.useChecklist.rawValue && QGroundControl.corePlugin.options.preFlightChecklistUrl.toString().length
@ -117,11 +122,14 @@ Item { @@ -117,11 +122,14 @@ Item {
property bool showContinueMission: _guidedActionsEnabled && _missionAvailable && !_missionActive && _vehicleArmed && _vehicleFlying && (_currentMissionIndex < _missionItemCount - 1)
property bool showPause: _guidedActionsEnabled && _vehicleArmed && _activeVehicle.pauseVehicleSupported && _vehicleFlying && !_vehiclePaused && !_fixedWingOnApproach
property bool showChangeAlt: _guidedActionsEnabled && _vehicleFlying && _activeVehicle.guidedModeSupported && _vehicleArmed && !_missionActive
property bool showChangeSpeed: _guidedActionsEnabled && _vehicleFlying && _activeVehicle.guidedModeSupported && _vehicleArmed && !_missionActive
property bool showOrbit: _guidedActionsEnabled && _vehicleFlying && __orbitSupported && !_missionActive
property bool showROI: _guidedActionsEnabled && _vehicleFlying && __roiSupported && !_missionActive
property bool showLandAbort: _guidedActionsEnabled && _vehicleFlying && _fixedWingOnApproach
property bool showGotoLocation: _guidedActionsEnabled && _vehicleFlying
property bool showActionList: _guidedActionsEnabled && (showStartMission || showResumeMission || showChangeAlt || showLandAbort)
property string changeSpeedTitle: _fixedWing ? changeAirspeedTitle : changeCruiseSpeedTitle
property string changeSpeedMessage: _fixedWing ? changeAirspeedMessage : changeCruiseSpeedMessage
// Note: The '_missionItemCount - 2' is a hack to not trigger resume mission when a mission ends with an RTL item
property bool showResumeMission: _activeVehicle && !_vehicleArmed && _vehicleWasFlying && _missionAvailable && _resumeMissionIndex > 0 && (_resumeMissionIndex < _missionItemCount - 2)
@ -150,6 +158,7 @@ Item { @@ -150,6 +158,7 @@ Item {
property bool _vehicleWasFlying: false
property bool _rcRSSIAvailable: _activeVehicle ? _activeVehicle.rcRSSI > 0 && _activeVehicle.rcRSSI <= 100 : false
property bool _fixedWingOnApproach: _activeVehicle ? _activeVehicle.fixedWing && _vehicleLanding : false
property bool _fixedWing: _activeVehicle ? _activeVehicle.fixedWing || _activeVehicle.vtolInFwdFlight : false
// You can turn on log output for GuidedActionsController by turning on GuidedActionsControllerLog category
property bool __guidedModeSupported: _activeVehicle ? _activeVehicle.guidedModeSupported : false
@ -164,6 +173,32 @@ Item { @@ -164,6 +173,32 @@ Item {
}
}
function setupSlider(actionCode) {
// generic defaults
guidedValueSlider.configureAsLinearSlider()
if (actionCode === actionTakeoff) {
guidedValueSlider.setMinVal(_activeVehicle.minimumTakeoffAltitude())
guidedValueSlider.setValue(_activeVehicle ? _activeVehicle.minimumTakeoffAltitude() : 0)
guidedValueSlider.setDisplayText("Height")
} else if (actionCode === actionChangeSpeed) {
if (_activeVehicle.vtolInFwdFlight) {
guidedValueSlider.setDisplayText("Set Airspeed")
guidedValueSlider.setMinVal(_activeVehicle.minimumEquivalentAirspeed())
guidedValueSlider.setMaxVal(_activeVehicle.maximumEquivalentAirspeed())
guidedValueSlider.setValue(_activeVehicle.airSpeedSetpoint.rawValue)
} else {
guidedValueSlider.setDisplayText("Set Speed")
guidedValueSlider.setMinVal(0.1)
guidedValueSlider.setMaxVal(_activeVehicle.maximumHorizontalSpeedMultirotor())
guidedValueSlider.setValue(_activeVehicle.maximumHorizontalSpeedMultirotor()/2)
}
} else if (actionCode === actionChangeAlt || actionCode === actionOrbit || actionCode === actionGoto || actionCode === actionPause) {
guidedValueSlider.setDisplayText("New Alt(rel)")
guidedValueSlider.configureAsRelativeAltSliderExp()
}
}
on_ActiveVehicleChanged: _outputState()
Component.onCompleted: _outputState()
@ -308,7 +343,7 @@ Item { @@ -308,7 +343,7 @@ Item {
function closeAll() {
confirmDialog.visible = false
actionList.visible = false
altitudeSlider.visible = false
guidedValueSlider.visible = false
}
// Called when an action is about to be executed in order to confirm
@ -321,6 +356,9 @@ Item { @@ -321,6 +356,9 @@ Item {
confirmDialog.mapIndicator = mapIndicator
confirmDialog.optionText = ""
_actionData = actionData
setupSlider(actionCode)
switch (actionCode) {
case actionArm:
if (_vehicleFlying || !_guidedActionsEnabled) {
@ -352,8 +390,7 @@ Item { @@ -352,8 +390,7 @@ Item {
confirmDialog.title = takeoffTitle
confirmDialog.message = takeoffMessage
confirmDialog.hideTrigger = Qt.binding(function() { return !showTakeoff })
altitudeSlider.setToMinimumTakeoff()
altitudeSlider.visible = true
guidedValueSlider.visible = true
break;
case actionStartMission:
showImmediate = false
@ -398,8 +435,7 @@ Item { @@ -398,8 +435,7 @@ Item {
confirmDialog.title = changeAltTitle
confirmDialog.message = changeAltMessage
confirmDialog.hideTrigger = Qt.binding(function() { return !showChangeAlt })
altitudeSlider.reset()
altitudeSlider.visible = true
guidedValueSlider.visible = true
break;
case actionGoto:
confirmDialog.title = gotoTitle
@ -414,8 +450,7 @@ Item { @@ -414,8 +450,7 @@ Item {
confirmDialog.title = orbitTitle
confirmDialog.message = orbitMessage
confirmDialog.hideTrigger = Qt.binding(function() { return !showOrbit })
altitudeSlider.reset()
altitudeSlider.visible = true
guidedValueSlider.visible = true
break;
case actionLandAbort:
confirmDialog.title = landAbortTitle
@ -426,8 +461,7 @@ Item { @@ -426,8 +461,7 @@ Item {
confirmDialog.title = pauseTitle
confirmDialog.message = pauseMessage
confirmDialog.hideTrigger = Qt.binding(function() { return !showPause })
altitudeSlider.reset()
altitudeSlider.visible = true
guidedValueSlider.visible = true
break;
case actionMVPause:
confirmDialog.title = mvPauseTitle
@ -452,6 +486,13 @@ Item { @@ -452,6 +486,13 @@ Item {
case actionActionList:
actionList.show()
return
case actionChangeSpeed:
confirmDialog.title = "Change Speed"
confirmDialog.hideTrigger = true
confirmDialog.title = changeSpeedTitle
confirmDialog.message = changeSpeedMessage
guidedValueSlider.visible = true
break
default:
console.warn("Unknown actionCode", actionCode)
return
@ -460,7 +501,7 @@ Item { @@ -460,7 +501,7 @@ Item {
}
// Executes the specified action
function executeAction(actionCode, actionData, actionAltitudeChange, optionChecked) {
function executeAction(actionCode, actionData, sliderOutputValue, optionChecked) {
var i;
var rgVehicle;
switch (actionCode) {
@ -471,7 +512,7 @@ Item { @@ -471,7 +512,7 @@ Item {
_activeVehicle.guidedModeLand()
break
case actionTakeoff:
_activeVehicle.guidedModeTakeoff(actionAltitudeChange)
_activeVehicle.guidedModeTakeoff(sliderOutputValue)
break
case actionResumeMission:
case actionResumeMissionUploadFail:
@ -500,7 +541,7 @@ Item { @@ -500,7 +541,7 @@ Item {
_activeVehicle.emergencyStop()
break
case actionChangeAlt:
_activeVehicle.guidedModeChangeAltitude(actionAltitudeChange, false /* pauseVehicle */)
_activeVehicle.guidedModeChangeAltitude(sliderOutputValue, false /* pauseVehicle */)
break
case actionGoto:
_activeVehicle.guidedModeGotoLocation(actionData)
@ -509,13 +550,13 @@ Item { @@ -509,13 +550,13 @@ Item {
_activeVehicle.setCurrentMissionSequence(actionData)
break
case actionOrbit:
_activeVehicle.guidedModeOrbit(orbitMapCircle.center, orbitMapCircle.radius() * (orbitMapCircle.clockwiseRotation ? 1 : -1), _activeVehicle.altitudeAMSL.rawValue + actionAltitudeChange)
_activeVehicle.guidedModeOrbit(orbitMapCircle.center, orbitMapCircle.radius() * (orbitMapCircle.clockwiseRotation ? 1 : -1), _activeVehicle.altitudeAMSL.rawValue + sliderOutputValue)
break
case actionLandAbort:
_activeVehicle.abortLanding(50) // hardcoded value for climbOutAltitude that is currently ignored
break
case actionPause:
_activeVehicle.guidedModeChangeAltitude(actionAltitudeChange, true /* pauseVehicle */)
_activeVehicle.guidedModeChangeAltitude(sliderOutputValue, true /* pauseVehicle */)
break
case actionMVPause:
rgVehicle = QGroundControl.multiVehicleManager.vehicles
@ -532,6 +573,15 @@ Item { @@ -532,6 +573,15 @@ Item {
case actionROI:
_activeVehicle.guidedModeROI(actionData)
break
case actionChangeSpeed:
if (_activeVehicle) {
if (_activeVehicle.vtolInFwdFlight || _activeVehicle.fixedWing) {
_activeVehicle.guidedModeChangeEquivalentAirspeed(sliderOutputValue)
} else {
_activeVehicle.guidedModeChangeGroundSpeed(sliderOutputValue)
}
}
break
default:
console.warn(qsTr("Internal error: unknown actionCode"), actionCode)
break

36
src/FlightDisplay/GuidedValueSlider.qml

@ -34,22 +34,12 @@ Rectangle { @@ -34,22 +34,12 @@ Rectangle {
}
on_SliderCenterValueChanged: {
valueField.updateFunction(sliderValue)
valueField.updateFunction(sliderValue)
}
function setValue(val) {
if (valueField.updateFunction == valueField.updateExpAroundCenterValue) {
if (val >= _sliderCenterValue) {
valueSlider.value = Math.pow(val / Math.max(_sliderMaxVal - _sliderCenterValue, 0), 1.0/3.0)
} else {
valueSlider.value = -Math.pow(val / Math.max(_sliderCenterValue - _sliderMinVal, 0), 1.0/3.0)
}
} else {
valueSlider.value = valueField.getSliderValueFromOutputLinear(val)
}
valueField.updateFunction(sliderValue)
valueSlider.value = valueField.getSliderValueFromOutput(val)
valueField.updateFunction(valueSlider.value)
}
function configureAsRelativeAltSliderExp() {
@ -133,8 +123,24 @@ Rectangle { @@ -133,8 +123,24 @@ Rectangle {
newValueAppUnits = QGroundControl.unitsConversion.metersToAppSettingsHorizontalDistanceUnits(newValue).toFixed(1)
}
function getSliderValueFromOutputLinear(value) {
return 2 * (value - _sliderMinVal) / (_sliderMaxVal - _sliderMinVal) - 1
function getSliderValueFromOutputLinear(val) {
return 2 * (val - _sliderMinVal) / (_sliderMaxVal - _sliderMinVal) - 1
}
function getSliderValueFromOutputExp(val) {
if (val >= _sliderCenterValue) {
return Math.pow(val / Math.max(_sliderMaxVal - _sliderCenterValue, 0), 1.0/3.0)
} else {
return -Math.pow(val / Math.max(_sliderCenterValue - _sliderMinVal, 0), 1.0/3.0)
}
}
function getSliderValueFromOutput(output) {
if (updateFunction == updateExpAroundCenterValue) {
return getSliderValueFromOutputExp(output)
} else {
return getSliderValueFromOutputLinear(output)
}
}
}
}

2
src/FlightDisplay/TelemetryValuesBar.qml

@ -105,7 +105,7 @@ Rectangle { @@ -105,7 +105,7 @@ Rectangle {
GuidedActionConfirm {
Layout.fillWidth: true
guidedController: _guidedController
altitudeSlider: _guidedAltSlider
guidedValueSlider: _guidedValueSlider
}
}
}

2
src/QmlControls/QGroundControl/FlightDisplay/qmldir

@ -19,7 +19,7 @@ GuidedActionLand 1.0 GuidedActionLand.qml @@ -19,7 +19,7 @@ GuidedActionLand 1.0 GuidedActionLand.qml
GuidedActionList 1.0 GuidedActionList.qml
GuidedActionPause 1.0 GuidedActionPause.qml
GuidedActionRTL 1.0 GuidedActionRTL.qml
GuidedAltitudeSlider 1.0 GuidedAltitudeSlider.qml
GuidedValueSlider 1.0 GuidedValueSlider.qml
GuidedActionTakeoff 1.0 GuidedActionTakeoff.qml
GuidedToolStripAction 1.0 GuidedToolStripAction.qml
MultiVehicleList 1.0 MultiVehicleList.qml

37
src/Vehicle/Vehicle.cc

@ -2542,6 +2542,23 @@ double Vehicle::minimumTakeoffAltitude() @@ -2542,6 +2542,23 @@ double Vehicle::minimumTakeoffAltitude()
return _firmwarePlugin->minimumTakeoffAltitude(this);
}
double Vehicle::maximumHorizontalSpeedMultirotor()
{
return _firmwarePlugin->maximumHorizontalSpeedMultirotor(this);
}
double Vehicle::maximumEquivalentAirspeed()
{
return _firmwarePlugin->maximumEquivalentAirspeed(this);
}
double Vehicle::minimumEquivalentAirspeed()
{
return _firmwarePlugin->minimumEquivalentAirspeed(this);
}
void Vehicle::startMission()
{
_firmwarePlugin->startMission(this);
@ -2573,6 +2590,26 @@ void Vehicle::guidedModeChangeAltitude(double altitudeChange, bool pauseVehicle) @@ -2573,6 +2590,26 @@ void Vehicle::guidedModeChangeAltitude(double altitudeChange, bool pauseVehicle)
_firmwarePlugin->guidedModeChangeAltitude(this, altitudeChange, pauseVehicle);
}
void
Vehicle::guidedModeChangeGroundSpeed(double groundspeed)
{
if (!guidedModeSupported()) {
qgcApp()->showAppMessage(guided_mode_not_supported_by_vehicle);
return;
}
_firmwarePlugin->guidedModeChangeGroundSpeed(this, groundspeed);
}
void
Vehicle::guidedModeChangeEquivalentAirspeed(double airspeed)
{
if (!guidedModeSupported()) {
qgcApp()->showAppMessage(guided_mode_not_supported_by_vehicle);
return;
}
_firmwarePlugin->guidedModeChangeEquivalentAirspeed(this, airspeed);
}
void Vehicle::guidedModeOrbit(const QGeoCoordinate& centerCoord, double radius, double amslAltitude)
{
if (!orbitModeSupported()) {

16
src/Vehicle/Vehicle.h

@ -352,6 +352,15 @@ public: @@ -352,6 +352,15 @@ public:
/// @return The minimum takeoff altitude (relative) for guided takeoff.
Q_INVOKABLE double minimumTakeoffAltitude();
/// @return Maximum horizontal speed multirotor.
Q_INVOKABLE double maximumHorizontalSpeedMultirotor();
/// @return Maximum equivalent airspeed.
Q_INVOKABLE double maximumEquivalentAirspeed();
/// @return Minumum equivalent airspeed.
Q_INVOKABLE double minimumEquivalentAirspeed();
/// Command vehicle to move to specified location (altitude is included and relative)
Q_INVOKABLE void guidedModeGotoLocation(const QGeoCoordinate& gotoCoord);
@ -360,6 +369,13 @@ public: @@ -360,6 +369,13 @@ public:
/// @param pauseVehicle true: pause vehicle prior to altitude change
Q_INVOKABLE void guidedModeChangeAltitude(double altitudeChange, bool pauseVehicle);
/// Command vehicle to change groundspeed
/// @param groundspeed Target horizontal groundspeed
Q_INVOKABLE void guidedModeChangeGroundSpeed (double groundspeed);
/// Command vehicle to change equivalent airspeed
/// @param airspeed Target equivalent airspeed
Q_INVOKABLE void guidedModeChangeEquivalentAirspeed (double airspeed);
/// Command vehicle to orbit given center point
/// @param centerCoord Orit around this point
/// @param radius Distance from vehicle to centerCoord

Loading…
Cancel
Save