From 109d6a59e8e14906fe298ebe0175b4e78f3ccb40 Mon Sep 17 00:00:00 2001 From: DonLakeFlyer Date: Tue, 4 Aug 2020 12:21:00 -0700 Subject: [PATCH] VTOL Takeof/Landing Pattern work --- ChangeLog.md | 2 + .../FixedWingLandingComplexItem.cc | 12 ++--- src/MissionManager/SimpleMissionItem.cc | 2 +- src/MissionManager/TakeoffMissionItem.cc | 4 +- src/MissionManager/VTOLLandingComplexItem.cc | 29 +++++------ .../VTOLLandingPattern.FactMetaData.json | 36 ++++++------- src/PlanView/FWLandingPatternEditor.qml | 2 - src/PlanView/PlanToolBarIndicators.qml | 18 ++++--- src/PlanView/SimpleItemEditor.qml | 8 +-- src/PlanView/TakeoffItemMapVisual.qml | 1 - src/PlanView/VTOLLandingPatternEditor.qml | 25 +++++++-- src/Settings/PlanView.SettingsGroup.json | 52 +++++++++++-------- src/Settings/PlanViewSettings.cc | 1 + src/Settings/PlanViewSettings.h | 1 + src/ui/preferences/GeneralSettings.qml | 13 +++-- 15 files changed, 119 insertions(+), 87 deletions(-) diff --git a/ChangeLog.md b/ChangeLog.md index 917c53789..b4f43e044 100644 --- a/ChangeLog.md +++ b/ChangeLog.md @@ -4,6 +4,8 @@ Note: This file only contains high level features or important fixes. ## 4.1 - Daily build +* VTOL: General setting for transition distance which affects Plan takeoff, landing pattern creation +* VTOL: Much better VTOL support throughout QGC * Maps: Support zoom up to level 23 even if map provider doesn't provide tiles that high * Settings/Mavlink: Add ability to forward mavlink traffic out specified UDP port * Support mavlink terrain protocol which queries gcs for terrain height information. Allows planning missions with TERRAIN\_FRAME. diff --git a/src/MissionManager/FixedWingLandingComplexItem.cc b/src/MissionManager/FixedWingLandingComplexItem.cc index 72c39af78..5224f882a 100644 --- a/src/MissionManager/FixedWingLandingComplexItem.cc +++ b/src/MissionManager/FixedWingLandingComplexItem.cc @@ -574,17 +574,11 @@ void FixedWingLandingComplexItem::_recalcFromHeadingAndDistanceChange(void) double landToTangentDistance = _landingDistanceFact.rawValue().toDouble(); double heading = _landingHeadingFact.rawValue().toDouble(); - // Calculate loiter tangent coordinate + // Heading is from loiter to land, hence +180 _loiterTangentCoordinate = _landingCoordinate.atDistanceAndAzimuth(landToTangentDistance, heading + 180); - // Calculate the distance and angle to the loiter coordinate - QGeoCoordinate tangent = _landingCoordinate.atDistanceAndAzimuth(landToTangentDistance, 0); - QGeoCoordinate loiter = tangent.atDistanceAndAzimuth(radius, 90); - double loiterDistance = _landingCoordinate.distanceTo(loiter); - double loiterAzimuth = _landingCoordinate.azimuthTo(loiter) * (_loiterClockwise ? -1 : 1); - - // Use those values to get the new loiter point which takes heading into acount - _loiterCoordinate = _landingCoordinate.atDistanceAndAzimuth(loiterDistance, heading + 180 + loiterAzimuth); + // Loiter coord is 90 degrees counter clockwise from tangent coord + _loiterCoordinate = _loiterTangentCoordinate.atDistanceAndAzimuth(radius, heading - 180 - 90); _loiterCoordinate.setAltitude(_loiterAltitudeFact.rawValue().toDouble()); _ignoreRecalcSignals = true; diff --git a/src/MissionManager/SimpleMissionItem.cc b/src/MissionManager/SimpleMissionItem.cc index 53195b1d9..5f4c507ab 100644 --- a/src/MissionManager/SimpleMissionItem.cc +++ b/src/MissionManager/SimpleMissionItem.cc @@ -399,7 +399,7 @@ QString SimpleMissionItem::abbreviation() const case MAV_CMD_NAV_LAND: return tr("Land"); case MAV_CMD_NAV_VTOL_TAKEOFF: - return tr("VTOL Takeoff"); + return tr("Transition Direction"); case MAV_CMD_NAV_VTOL_LAND: return tr("VTOL Land"); case MAV_CMD_DO_SET_ROI: diff --git a/src/MissionManager/TakeoffMissionItem.cc b/src/MissionManager/TakeoffMissionItem.cc index aa8b9cd1a..90ca24416 100644 --- a/src/MissionManager/TakeoffMissionItem.cc +++ b/src/MissionManager/TakeoffMissionItem.cc @@ -175,12 +175,12 @@ void TakeoffMissionItem::setLaunchCoordinate(const QGeoCoordinate& launchCoordin if (_launchTakeoffAtSameLocation) { takeoffCoordinate = launchCoordinate; } else { - double distance = 30.0; // Default distance is VTOL transition to takeoff point distance + double distance = qgcApp()->toolbox()->settingsManager()->planViewSettings()->vtolTransitionDistance()->rawValue().toDouble(); // Default distance is VTOL transition to takeoff point distance if (_controllerVehicle->fixedWing()) { double altitude = this->altitude()->rawValue().toDouble(); if (altitudeMode() == QGroundControlQmlGlobal::AltitudeModeRelative) { - // Offset for fixed wing climb out of 30 degrees + // Offset for fixed wing climb out of 30 degrees to specified altitude if (altitude != 0.0) { distance = altitude / tan(qDegreesToRadians(30.0)); } diff --git a/src/MissionManager/VTOLLandingComplexItem.cc b/src/MissionManager/VTOLLandingComplexItem.cc index 9b1f9caca..efcf3a714 100644 --- a/src/MissionManager/VTOLLandingComplexItem.cc +++ b/src/MissionManager/VTOLLandingComplexItem.cc @@ -14,6 +14,7 @@ #include "SimpleMissionItem.h" #include "PlanMasterController.h" #include "FlightPathSegment.h" +#include "QGC.h" #include @@ -54,9 +55,13 @@ VTOLLandingComplexItem::VTOLLandingComplexItem(PlanMasterController* masterContr _editorQml = "qrc:/qml/VTOLLandingPatternEditor.qml"; _isIncomplete = false; - QGeoCoordinate homePositionCoordinate = masterController->missionController()->plannedHomePosition(); - if (homePositionCoordinate.isValid()) { - setLandingCoordinate(homePositionCoordinate); + // We adjust landing distance meta data to Plan View settings unless there was a custom build override + if (QGC::fuzzyCompare(_landingDistanceFact.rawValue().toDouble(), _landingDistanceFact.rawDefaultValue().toDouble())) { + Fact* vtolTransitionDistanceFact = qgcApp()->toolbox()->settingsManager()->planViewSettings()->vtolTransitionDistance(); + double vtolTransitionDistance = vtolTransitionDistanceFact->rawValue().toDouble(); + _landingDistanceFact.metaData()->setRawDefaultValue(vtolTransitionDistance); + _landingDistanceFact.setRawValue(vtolTransitionDistance); + _landingDistanceFact.metaData()->setRawMin(vtolTransitionDistanceFact->metaData()->rawMin()); } connect(&_loiterAltitudeFact, &Fact::valueChanged, this, &VTOLLandingComplexItem::_updateLoiterCoodinateAltitudeFromFact); @@ -535,21 +540,15 @@ void VTOLLandingComplexItem::_recalcFromHeadingAndDistanceChange(void) if (!_ignoreRecalcSignals && _landingCoordSet) { // These are our known values - double radius = _loiterRadiusFact.rawValue().toDouble(); - double landToTangentDistance = _landingDistanceFact.rawValue().toDouble(); - double heading = _landingHeadingFact.rawValue().toDouble(); + double radius = _loiterRadiusFact.rawValue().toDouble(); + double landToTangentDistance = _landingDistanceFact.rawValue().toDouble(); + double heading = _landingHeadingFact.rawValue().toDouble(); - // Calculate loiter tangent coordinate + // Heading is from loiter to land, hence +180 _loiterTangentCoordinate = _landingCoordinate.atDistanceAndAzimuth(landToTangentDistance, heading + 180); - // Calculate the distance and angle to the loiter coordinate - QGeoCoordinate tangent = _landingCoordinate.atDistanceAndAzimuth(landToTangentDistance, 0); - QGeoCoordinate loiter = tangent.atDistanceAndAzimuth(radius, 90); - double loiterDistance = _landingCoordinate.distanceTo(loiter); - double loiterAzimuth = _landingCoordinate.azimuthTo(loiter) * (_loiterClockwise ? -1 : 1); - - // Use those values to get the new loiter point which takes heading into acount - _loiterCoordinate = _landingCoordinate.atDistanceAndAzimuth(loiterDistance, heading + 180 + loiterAzimuth); + // Loiter coord is 90 degrees counter clockwise from tangent coord + _loiterCoordinate = _loiterTangentCoordinate.atDistanceAndAzimuth(radius, heading - 180 - 90); _loiterCoordinate.setAltitude(_loiterAltitudeFact.rawValue().toDouble()); _ignoreRecalcSignals = true; diff --git a/src/MissionManager/VTOLLandingPattern.FactMetaData.json b/src/MissionManager/VTOLLandingPattern.FactMetaData.json index 7f6ad8752..f17902902 100644 --- a/src/MissionManager/VTOLLandingPattern.FactMetaData.json +++ b/src/MissionManager/VTOLLandingPattern.FactMetaData.json @@ -5,59 +5,59 @@ [ { "name": "LandingDistance", - "shortDesc": "Distance between landing and loiter points.", + "shortDesc": "Distance between landing and loiter points.", "type": "double", "units": "m", "min": 10, "decimalPlaces": 1, - "default": 30.0 + "default": 300.0 }, { "name": "LandingHeading", - "shortDesc": "Heading from loiter point to land point.", + "shortDesc": "Heading from loiter point to land point.", "type": "double", "units": "deg", "min": 0.0, "max": 360.0, "decimalPlaces": 0, - "default": 270.0 + "default": 270.0 }, { "name": "LoiterAltitude", - "shortDesc": "Aircraft will proceed to the loiter point and loiter downwards until it reaches this approach altitude. Once altitude is reached the aircraft will fly to land point at current altitude.", + "shortDesc": "Aircraft will proceed to the loiter point and loiter downwards until it reaches this approach altitude. Once altitude is reached the aircraft will fly to land point at current altitude.", "type": "double", "units": "m", "decimalPlaces": 1, - "default": 40.0 + "default": 30.0 }, { "name": "LoiterRadius", - "shortDesc": "Loiter radius.", + "shortDesc": "Loiter radius.", "type": "double", "decimalPlaces": 1, "min": 1, "units": "m", - "default": 75.0 + "default": 75.0 }, { "name": "LandingAltitude", - "shortDesc": "Altitude for landing point on ground.", + "shortDesc": "Altitude for landing point on ground.", "type": "double", "units": "m", "decimalPlaces": 1, - "default": 0.0 + "default": 0.0 }, { - "name": "StopTakingPhotos", - "shortDesc": "Stop taking photos", - "type": "bool", - "default": true + "name": "StopTakingPhotos", + "shortDesc": "Stop taking photos", + "type": "bool", + "default": true }, { - "name": "StopTakingVideo", - "shortDesc": "Stop taking video", - "type": "bool", - "default": true + "name": "StopTakingVideo", + "shortDesc": "Stop taking video", + "type": "bool", + "default": true } ] } diff --git a/src/PlanView/FWLandingPatternEditor.qml b/src/PlanView/FWLandingPatternEditor.qml index 9f6534e5e..62695cbd5 100644 --- a/src/PlanView/FWLandingPatternEditor.qml +++ b/src/PlanView/FWLandingPatternEditor.qml @@ -317,8 +317,6 @@ Rectangle { onClicked: { missionItem.wizardMode = false missionItem.landingDragAngleOnly = false - // Trial of no auto select next item - //editorRoot.selectNextNotReadyItem() } } } diff --git a/src/PlanView/PlanToolBarIndicators.qml b/src/PlanView/PlanToolBarIndicators.qml index e7a84990f..491aab248 100644 --- a/src/PlanView/PlanToolBarIndicators.qml +++ b/src/PlanView/PlanToolBarIndicators.qml @@ -27,7 +27,9 @@ Item { property var _controllerDirty: _controllerValid ? _planMasterController.dirty : false property var _controllerSyncInProgress: _controllerValid ? _planMasterController.syncInProgress : false - property bool _statusValid: _currentMissionItem !== undefined && _currentMissionItem !== null + property bool _currentMissionItemValid: _currentMissionItem && _currentMissionItem !== undefined && _currentMissionItem !== null + property bool _curreItemIsFlyThrough: _currentMissionItemValid && _currentMissionItem.specifiesCoordinate && !_currentMissionItem.isStandaloneCoordinate + property bool _currentItemIsVTOLTakeoff: _currentMissionItemValid && _currentMissionItem.command == 84 property bool _missionValid: missionItems !== undefined property real _dataFontSize: ScreenTools.defaultFontPointSize @@ -36,11 +38,10 @@ Item { property real _smallValueWidth: ScreenTools.defaultFontPixelWidth * 3 property real _labelToValueSpacing: ScreenTools.defaultFontPixelWidth property real _rowSpacing: ScreenTools.isMobile ? 1 : 0 - property real _distance: _statusValid && _currentMissionItem ? _currentMissionItem.distance : NaN - property real _altDifference: _statusValid && _currentMissionItem ? _currentMissionItem.altDifference : NaN - property real _gradient: _statusValid && _currentMissionItem && _currentMissionItem.distance > 0 ? (Math.atan(_currentMissionItem.altDifference / _currentMissionItem.distance) * (180.0/Math.PI)) : NaN - property real _azimuth: _statusValid && _currentMissionItem ? _currentMissionItem.azimuth : NaN - property real _heading: _statusValid && _currentMissionItem ? _currentMissionItem.missionVehicleYaw : NaN + property real _distance: _currentMissionItemValid ? _currentMissionItem.distance : NaN + property real _altDifference: _currentMissionItemValid ? _currentMissionItem.altDifference : NaN + property real _azimuth: _currentMissionItemValid ? _currentMissionItem.azimuth : NaN + property real _heading: _currentMissionItemValid ? _currentMissionItem.missionVehicleYaw : NaN property real _missionDistance: _missionValid ? missionDistance : NaN property real _missionMaxTelemetry: _missionValid ? missionMaxTelemetry : NaN property real _missionTime: _missionValid ? missionTime : NaN @@ -49,6 +50,11 @@ Item { property bool _batteryInfoAvailable: _batteryChangePoint >= 0 || _batteriesRequired >= 0 property real _controllerProgressPct: _controllerValid ? _planMasterController.missionController.progressPct : 0 property bool _syncInProgress: _controllerValid ? _planMasterController.missionController.syncInProgress : false + property real _gradient: _currentMissionItemValid && _currentMissionItem.distance > 0 ? + (_currentItemIsVTOLTakeoff ? + 0 : + (Math.atan(_currentMissionItem.altDifference / _currentMissionItem.distance) * (180.0/Math.PI))) + : NaN property string _distanceText: isNaN(_distance) ? "-.-" : QGroundControl.unitsConversion.metersToAppSettingsHorizontalDistanceUnits(_distance).toFixed(1) + " " + QGroundControl.unitsConversion.appSettingsHorizontalDistanceUnitsString property string _altDifferenceText: isNaN(_altDifference) ? "-.-" : QGroundControl.unitsConversion.metersToAppSettingsHorizontalDistanceUnits(_altDifference).toFixed(1) + " " + QGroundControl.unitsConversion.appSettingsHorizontalDistanceUnitsString diff --git a/src/PlanView/SimpleItemEditor.qml b/src/PlanView/SimpleItemEditor.qml index 6b4db36d9..4a5a36d9b 100644 --- a/src/PlanView/SimpleItemEditor.qml +++ b/src/PlanView/SimpleItemEditor.qml @@ -82,7 +82,11 @@ Rectangle { visible: missionItem.isTakeoffItem && missionItem.wizardMode // Hack special case for takeoff item QGCLabel { - text: qsTr("Move '%1' Takeoff to the %2 location.").arg(_controllerVehicle.vtol ? qsTr("V") : qsTr("T")).arg(_controllerVehicle.vtol ? qsTr("desired") : qsTr("climbout")) + text: qsTr("Move '%1' %2 to the %3 location. %4") + .arg(_controllerVehicle.vtol ? qsTr("T") : qsTr("T")) + .arg(_controllerVehicle.vtol ? qsTr("Transition Direction") : qsTr("Takeoff")) + .arg(_controllerVehicle.vtol ? qsTr("desired") : qsTr("climbout")) + .arg(_controllerVehicle.vtol ? (qsTr("Ensure distance from launch to transition direction is far enough to complete transition.")) : "") Layout.fillWidth: true wrapMode: Text.WordWrap visible: !initialClickLabel.visible @@ -101,8 +105,6 @@ Rectangle { visible: !initialClickLabel.visible onClicked: { missionItem.wizardMode = false - // Trial of no auto select next item - //editorRoot.selectNextNotReadyItem() } } diff --git a/src/PlanView/TakeoffItemMapVisual.qml b/src/PlanView/TakeoffItemMapVisual.qml index f19436dc3..0cdb9666d 100644 --- a/src/PlanView/TakeoffItemMapVisual.qml +++ b/src/PlanView/TakeoffItemMapVisual.qml @@ -149,7 +149,6 @@ Item { readonly property int _decimalPlaces: 8 onClicked: { - console.log("mousearea click") var coordinate = map.toCoordinate(Qt.point(mouse.x, mouse.y), false /* clipToViewPort */) coordinate.latitude = coordinate.latitude.toFixed(_decimalPlaces) coordinate.longitude = coordinate.longitude.toFixed(_decimalPlaces) diff --git a/src/PlanView/VTOLLandingPatternEditor.qml b/src/PlanView/VTOLLandingPatternEditor.qml index 84a8a83e0..81eef00b4 100644 --- a/src/PlanView/VTOLLandingPatternEditor.qml +++ b/src/PlanView/VTOLLandingPatternEditor.qml @@ -32,9 +32,9 @@ Rectangle { //property real availableWidth ///< Width for control //property var missionItem ///< Mission Item for editor - property var _masterControler: masterController - property var _missionController: _masterControler.missionController - property var _missionVehicle: _masterControler.controllerVehicle + property var _masterControler: masterController + property var _missionController: _masterControler.missionController + property var _missionVehicle: _masterControler.controllerVehicle property real _margin: ScreenTools.defaultFontPixelWidth / 2 property real _spacer: ScreenTools.defaultFontPixelWidth / 2 property string _setToVehicleHeadingStr: qsTr("Set to vehicle heading") @@ -51,6 +51,14 @@ Rectangle { spacing: _margin visible: !editorColumnNeedLandingPoint.visible + QGCLabel { + anchors.left: parent.left + anchors.right: parent.right + wrapMode: Text.WordWrap + font.pointSize: ScreenTools.smallFontPointSize + text: qsTr("Loiter down to specified altitude. Fly to land point while transitioning. Hover straight down to land.") + } + SectionHeader { id: loiterPointSection anchors.left: parent.left @@ -216,6 +224,15 @@ Rectangle { font.pointSize: ScreenTools.smallFontPointSize text: qsTr("* Avoid tailwind from loiter to land.") } + + QGCLabel { + anchors.left: parent.left + anchors.right: parent.right + wrapMode: Text.WordWrap + color: qgcPal.warningText + font.pointSize: ScreenTools.smallFontPointSize + text: qsTr("* Ensure landing distance is enough to complete transition.") + } } } @@ -287,8 +304,6 @@ Rectangle { onClicked: { missionItem.wizardMode = false missionItem.landingDragAngleOnly = false - // Trial of no auto select next item - //editorRoot.selectNextNotReadyItem() } } } diff --git a/src/Settings/PlanView.SettingsGroup.json b/src/Settings/PlanView.SettingsGroup.json index 6201749ac..3f3557d86 100644 --- a/src/Settings/PlanView.SettingsGroup.json +++ b/src/Settings/PlanView.SettingsGroup.json @@ -1,37 +1,45 @@ { - "version": 1, - "fileType": "FactMetaData", + "version": 1, + "fileType": "FactMetaData", "QGC.MetaData.Facts": [ { - "name": "displayPresetsTabFirst", - "shortDesc": "Display the presets tab at start", - "type": "bool", - "default": false + "name": "displayPresetsTabFirst", + "shortDesc": "Display the presets tab at start", + "type": "bool", + "default": false }, { - "name": "showMissionItemStatus", - "shortDesc": "Show/Hide the mission item status display", - "type": "bool", - "default": true + "name": "showMissionItemStatus", + "shortDesc": "Show/Hide the mission item status display", + "type": "bool", + "default": true }, { - "name": "takeoffItemNotRequired", - "shortDesc": "Allow missions to not require a takeoff item", - "type": "bool", - "default": false + "name": "takeoffItemNotRequired", + "shortDesc": "Allow missions to not require a takeoff item", + "type": "bool", + "default": false }, { - "name": "useConditionGate", - "shortDesc": "Use MAV_CMD_CONDITION_GATE for pattern generation", - "type": "bool", - "default": false + "name": "useConditionGate", + "shortDesc": "Use MAV_CMD_CONDITION_GATE for pattern generation", + "type": "bool", + "default": false }, { - "name": "showGimbalOnlyWhenSet", - "shortDesc": "Show gimbal yaw visual only when set explicitly for the waypoint", - "type": "bool", - "default": false + "name": "showGimbalOnlyWhenSet", + "shortDesc": "Show gimbal yaw visual only when set explicitly for the waypoint", + "type": "bool", + "default": false +}, +{ + "name": "vtolTransitionDistance", + "shortDesc": "Amount of distance required for vehicle to complete a transition", + "type": "double", + "default": 300.0, + "units": "m", + "min": 100.0 } ] } diff --git a/src/Settings/PlanViewSettings.cc b/src/Settings/PlanViewSettings.cc index 00eb1f3c3..96750fdfb 100644 --- a/src/Settings/PlanViewSettings.cc +++ b/src/Settings/PlanViewSettings.cc @@ -22,3 +22,4 @@ DECLARE_SETTINGSFACT(PlanViewSettings, showMissionItemStatus) DECLARE_SETTINGSFACT(PlanViewSettings, useConditionGate) DECLARE_SETTINGSFACT(PlanViewSettings, takeoffItemNotRequired) DECLARE_SETTINGSFACT(PlanViewSettings, showGimbalOnlyWhenSet) +DECLARE_SETTINGSFACT(PlanViewSettings, vtolTransitionDistance) diff --git a/src/Settings/PlanViewSettings.h b/src/Settings/PlanViewSettings.h index 7e42f46cc..0acc95893 100644 --- a/src/Settings/PlanViewSettings.h +++ b/src/Settings/PlanViewSettings.h @@ -25,4 +25,5 @@ public: DEFINE_SETTINGFACT(useConditionGate) DEFINE_SETTINGFACT(takeoffItemNotRequired) DEFINE_SETTINGFACT(showGimbalOnlyWhenSet) + DEFINE_SETTINGFACT(vtolTransitionDistance) }; diff --git a/src/ui/preferences/GeneralSettings.qml b/src/ui/preferences/GeneralSettings.qml index eeed52d10..55fafcff3 100644 --- a/src/ui/preferences/GeneralSettings.qml +++ b/src/ui/preferences/GeneralSettings.qml @@ -348,15 +348,22 @@ Rectangle { anchors.horizontalCenter: parent.horizontalCenter spacing: _margins - RowLayout { - spacing: ScreenTools.defaultFontPixelWidth - visible: QGroundControl.settingsManager.appSettings.defaultMissionItemAltitude.visible + GridLayout { + columns: 2 + columnSpacing: ScreenTools.defaultFontPixelWidth + visible: QGroundControl.settingsManager.appSettings.defaultMissionItemAltitude.visible QGCLabel { text: qsTr("Default Mission Altitude") } FactTextField { Layout.preferredWidth: _valueFieldWidth fact: QGroundControl.settingsManager.appSettings.defaultMissionItemAltitude } + + QGCLabel { text: qsTr("VTOL TransitionDistance") } + FactTextField { + Layout.preferredWidth: _valueFieldWidth + fact: QGroundControl.settingsManager.planViewSettings.vtolTransitionDistance + } } FactCheckBox { -- 2.22.0