Browse Source

Fix altitude slider and guidedactions for speed action:

There were some errors where fixed wing would not show the action,
also unit conversion support for speed was added
QGC4.4
davidsastresas 2 years ago committed by Don Gagne
parent
commit
8bffc3fbd6
  1. 24
      src/FlightDisplay/GuidedActionsController.qml
  2. 15
      src/FlightDisplay/GuidedValueSlider.qml

24
src/FlightDisplay/GuidedActionsController.qml

@ -190,22 +190,24 @@ Item {
function setupSlider(actionCode) { function setupSlider(actionCode) {
// generic defaults // generic defaults
guidedValueSlider.configureAsLinearSlider() guidedValueSlider.configureAsLinearSlider()
guidedValueSlider.setIsSpeedSlider(false)
if (actionCode === actionTakeoff) { if (actionCode === actionTakeoff) {
guidedValueSlider.setMinVal(_activeVehicle.minimumTakeoffAltitude()) guidedValueSlider.setMinVal(_activeVehicle.minimumTakeoffAltitude())
guidedValueSlider.setValue(_activeVehicle ? _activeVehicle.minimumTakeoffAltitude() : 0) guidedValueSlider.setValue(_activeVehicle ? _activeVehicle.minimumTakeoffAltitude() : 0)
guidedValueSlider.setDisplayText("Height") guidedValueSlider.setDisplayText("Height")
} else if (actionCode === actionChangeSpeed) { } else if (actionCode === actionChangeSpeed) {
if (_activeVehicle.vtolInFwdFlight) { guidedValueSlider.setIsSpeedSlider(true)
if (_fixedWing) {
guidedValueSlider.setDisplayText("Set Airspeed") guidedValueSlider.setDisplayText("Set Airspeed")
guidedValueSlider.setMinVal(_activeVehicle.minimumEquivalentAirspeed()) guidedValueSlider.setMinVal(QGroundControl.unitsConversion.metersSecondToAppSettingsSpeedUnits(_activeVehicle.minimumEquivalentAirspeed()).toFixed(1))
guidedValueSlider.setMaxVal(_activeVehicle.maximumEquivalentAirspeed()) guidedValueSlider.setMaxVal(QGroundControl.unitsConversion.metersSecondToAppSettingsSpeedUnits(_activeVehicle.maximumEquivalentAirspeed()).toFixed(1))
guidedValueSlider.setValue(_activeVehicle.airSpeedSetpoint.rawValue) guidedValueSlider.setValue(_activeVehicle.airSpeed.value)
} else { } else if (!_fixedWing && _activeVehicle.haveMRSpeedLimits) {
guidedValueSlider.setDisplayText("Set Speed") guidedValueSlider.setDisplayText("Set Speed")
guidedValueSlider.setMinVal(0.1) guidedValueSlider.setMinVal(QGroundControl.unitsConversion.metersSecondToAppSettingsSpeedUnits(0.1).toFixed(1))
guidedValueSlider.setMaxVal(_activeVehicle.maximumHorizontalSpeedMultirotor()) guidedValueSlider.setMaxVal(QGroundControl.unitsConversion.metersSecondToAppSettingsSpeedUnits(_activeVehicle.maximumHorizontalSpeedMultirotor()).toFixed(1))
guidedValueSlider.setValue(_activeVehicle.maximumHorizontalSpeedMultirotor()/2) guidedValueSlider.setValue(QGroundControl.unitsConversion.metersSecondToAppSettingsSpeedUnits(_activeVehicle.maximumHorizontalSpeedMultirotor()/2).toFixed(1))
} }
} else if (actionCode === actionChangeAlt || actionCode === actionOrbit || actionCode === actionGoto || actionCode === actionPause) { } else if (actionCode === actionChangeAlt || actionCode === actionOrbit || actionCode === actionGoto || actionCode === actionPause) {
guidedValueSlider.setDisplayText("New Alt(rel)") guidedValueSlider.setDisplayText("New Alt(rel)")
@ -599,10 +601,12 @@ Item {
break break
case actionChangeSpeed: case actionChangeSpeed:
if (_activeVehicle) { 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) { if (_activeVehicle.vtolInFwdFlight || _activeVehicle.fixedWing) {
_activeVehicle.guidedModeChangeEquivalentAirspeed(sliderOutputValue) _activeVehicle.guidedModeChangeEquivalentAirspeed(metersSecondSpeed)
} else { } else {
_activeVehicle.guidedModeChangeGroundSpeed(sliderOutputValue) _activeVehicle.guidedModeChangeGroundSpeed(metersSecondSpeed)
} }
} }
break break

15
src/FlightDisplay/GuidedValueSlider.qml

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

Loading…
Cancel
Save