diff --git a/src/MissionManager/FixedWingLandingComplexItem.cc b/src/MissionManager/FixedWingLandingComplexItem.cc index 464adb679e8..9228601861e 100644 --- a/src/MissionManager/FixedWingLandingComplexItem.cc +++ b/src/MissionManager/FixedWingLandingComplexItem.cc @@ -149,9 +149,9 @@ bool FixedWingLandingComplexItem::_isValidLandItem(const MissionItem& missionIte } } -bool FixedWingLandingComplexItem::scanForItem(QmlObjectListModel* visualItems, bool flyView, PlanMasterController* masterController) +bool FixedWingLandingComplexItem::scanForItems(QmlObjectListModel* visualItems, bool flyView, PlanMasterController* masterController) { - return _scanForItem(visualItems, flyView, masterController, _isValidLandItem, _createItem); + return _scanForItems(visualItems, flyView, masterController, _isValidLandItem, _createItem); } // Never call this method directly. If you want to update the flight segments you emit _updateFlightPathSegmentsSignal() diff --git a/src/MissionManager/FixedWingLandingComplexItem.h b/src/MissionManager/FixedWingLandingComplexItem.h index a9487664e50..3cd8fc3d2de 100644 --- a/src/MissionManager/FixedWingLandingComplexItem.h +++ b/src/MissionManager/FixedWingLandingComplexItem.h @@ -37,7 +37,7 @@ class FixedWingLandingComplexItem : public LandingComplexItem Fact* valueSetIsDistance (void) { return &_valueSetIsDistanceFact; } /// Scans the loaded items for a landing pattern complex item - static bool scanForItem(QmlObjectListModel* visualItems, bool flyView, PlanMasterController* masterController); + static bool scanForItems(QmlObjectListModel* visualItems, bool flyView, PlanMasterController* masterController); // Overrides from ComplexMissionItem QString patternName (void) const final { return name; } diff --git a/src/MissionManager/LandingComplexItem.cc b/src/MissionManager/LandingComplexItem.cc index a30e1662bd7..9c9befd29e9 100644 --- a/src/MissionManager/LandingComplexItem.cc +++ b/src/MissionManager/LandingComplexItem.cc @@ -363,7 +363,7 @@ MissionItem* LandingComplexItem::_createFinalApproachItem(int seqNum, QObject* p } } -bool LandingComplexItem::_scanForItem(QmlObjectListModel* visualItems, bool flyView, PlanMasterController* masterController, IsLandItemFunc isLandItemFunc, CreateItemFunc createItemFunc) +bool LandingComplexItem::_scanForItems(QmlObjectListModel* visualItems, bool flyView, PlanMasterController* masterController, IsLandItemFunc isLandItemFunc, CreateItemFunc createItemFunc) { qCDebug(LandingComplexItemLog) << "VTOLLandingComplexItem::scanForItem count" << visualItems->count(); @@ -371,6 +371,23 @@ bool LandingComplexItem::_scanForItem(QmlObjectListModel* visualItems, bool flyV return false; } + // Start looking for the commands in reverse order from the end of the list + int startIndex = visualItems->count(); + bool foundAny = false; + + while (startIndex >= 0) { + if (_scanForItem(visualItems, startIndex, flyView, masterController, isLandItemFunc, createItemFunc)) { + foundAny = true; + } else { + startIndex--; + } + } + + return foundAny; +} + +bool LandingComplexItem::_scanForItem(QmlObjectListModel* visualItems, int& startIndex, bool flyView, PlanMasterController* masterController, IsLandItemFunc isLandItemFunc, CreateItemFunc createItemFunc) +{ // A valid landing pattern is comprised of the follow commands in this order at the end of the item list: // MAV_CMD_DO_LAND_START - required // MAV_CMD_DO_CHANGE_SPEED - optional @@ -379,9 +396,7 @@ bool LandingComplexItem::_scanForItem(QmlObjectListModel* visualItems, bool flyV // MAV_CMD_NAV_LOITER_TO_ALT or MAV_CMD_NAV_WAYPOINT // MAV_CMD_NAV_LAND or MAV_CMD_NAV_VTOL_LAND - // Start looking for the commands in reverse order from the end of the list - - int scanIndex = visualItems->count() - 1; + int scanIndex = startIndex - 1; if (scanIndex < 0 || scanIndex > visualItems->count() - 1) { return false; @@ -476,7 +491,6 @@ bool LandingComplexItem::_scanForItem(QmlObjectListModel* visualItems, bool flyV } // We made it this far so we do have a Fixed Wing Landing Pattern item at the end of the mission. - // Since we have scanned it we need to remove the items for it fromt the list int deleteCount = 3; if (stopTakingPhotos) { deleteCount += CameraSection::stopTakingPhotosCommandCount(); @@ -487,7 +501,7 @@ bool LandingComplexItem::_scanForItem(QmlObjectListModel* visualItems, bool flyV if (useDoChangeSpeed) { deleteCount++; } - int firstItem = visualItems->count() - deleteCount; + int firstItem = startIndex - deleteCount; while (deleteCount--) { visualItems->removeAt(firstItem)->deleteLater(); } @@ -526,7 +540,8 @@ bool LandingComplexItem::_scanForItem(QmlObjectListModel* visualItems, bool flyV complexItem->_recalcFromCoordinateChange(); complexItem->setDirty(false); - visualItems->append(complexItem); + visualItems->insert(firstItem, complexItem); + startIndex = firstItem; return true; } diff --git a/src/MissionManager/LandingComplexItem.h b/src/MissionManager/LandingComplexItem.h index 52208d1f450..52e5c56c2d4 100644 --- a/src/MissionManager/LandingComplexItem.h +++ b/src/MissionManager/LandingComplexItem.h @@ -170,7 +170,8 @@ protected slots: typedef bool (*IsLandItemFunc)(const MissionItem& missionItem); typedef LandingComplexItem* (*CreateItemFunc)(PlanMasterController* masterController, bool flyView); - static bool _scanForItem(QmlObjectListModel* visualItems, bool flyView, PlanMasterController* masterController, IsLandItemFunc isLandItemFunc, CreateItemFunc createItemFunc); + static bool _scanForItems(QmlObjectListModel* visualItems, bool flyView, PlanMasterController* masterController, IsLandItemFunc isLandItemFunc, CreateItemFunc createItemFunc); + static bool _scanForItem(QmlObjectListModel* visualItems, int& startIndex, bool flyView, PlanMasterController* masterController, IsLandItemFunc isLandItemFunc, CreateItemFunc createItemFunc); int _sequenceNumber = 0; bool _dirty = false; diff --git a/src/MissionManager/MissionController.cc b/src/MissionManager/MissionController.cc index 15c41911bc3..99e5fb17155 100644 --- a/src/MissionManager/MissionController.cc +++ b/src/MissionManager/MissionController.cc @@ -53,9 +53,11 @@ MissionController::MissionController(PlanMasterController* masterController, QOb _updateTimer.setSingleShot(true); - connect(&_updateTimer, &QTimer::timeout, this, &MissionController::_updateTimeout); - connect(_planViewSettings->takeoffItemNotRequired(), &Fact::rawValueChanged, this, &MissionController::_takeoffItemNotRequiredChanged); - connect(this, &MissionController::missionDistanceChanged, this, &MissionController::recalcTerrainProfile); + connect(&_updateTimer, &QTimer::timeout, this, &MissionController::_updateTimeout); + connect(_planViewSettings->takeoffItemNotRequired(), &Fact::rawValueChanged, this, &MissionController::_forceRecalcOfAllowedBits); + connect( + _planViewSettings->allowMultipleLandingPatterns(), &Fact::rawValueChanged, this, &MissionController::_forceRecalcOfAllowedBits); + connect(this, &MissionController::missionPlannedDistanceChanged, this, &MissionController::recalcTerrainProfile); // The follow is used to compress multiple recalc calls in a row to into a single call. connect(this, &MissionController::_recalcMissionFlightStatusSignal, this, &MissionController::_recalcMissionFlightStatus, Qt::QueuedConnection); @@ -72,7 +74,8 @@ MissionController::~MissionController() void MissionController::_resetMissionFlightStatus(void) { - _missionFlightStatus.totalDistance = 0.0; + _missionFlightStatus.totalDistance = 0.0; + _missionFlightStatus.plannedDistance = 0.0; _missionFlightStatus.maxTelemetryDistance = 0.0; _missionFlightStatus.totalTime = 0.0; _missionFlightStatus.hoverTime = 0.0; @@ -101,7 +104,7 @@ void MissionController::_resetMissionFlightStatus(void) _missionFlightStatus.ampMinutesAvailable = static_cast(_missionFlightStatus.mAhBattery) / 1000.0 * 60.0 * ((100.0 - batteryPercentRemainingAnnounce) / 100.0); } - emit missionDistanceChanged(_missionFlightStatus.totalDistance); + emit missionPlannedDistanceChanged(_missionFlightStatus.plannedDistance); emit missionTimeChanged(); emit missionHoverDistanceChanged(_missionFlightStatus.hoverDistance); emit missionCruiseDistanceChanged(_missionFlightStatus.cruiseDistance); @@ -1328,6 +1331,12 @@ void MissionController::_recalcFlightPathSegments(void) break; } + // Don't draw segments immediately after a landing item + if (lastFlyThroughVI->isLandCommand()) { + lastFlyThroughVI = visualItem; + continue; + } + if (visualItem->specifiesCoordinate() && !visualItem->isStandaloneCoordinate()) { // Incomplete items are complex items which are waiting for the user to complete setup before there visuals can become valid. // They may not yet have valid entry/exit coordinates associated with them while in the incomplete state. @@ -1448,7 +1457,7 @@ void MissionController::_addHoverTime(double hoverTime, double hoverDistance, in _missionFlightStatus.totalTime += hoverTime; _missionFlightStatus.hoverTime += hoverTime; _missionFlightStatus.hoverDistance += hoverDistance; - _missionFlightStatus.totalDistance += hoverDistance; + _missionFlightStatus.plannedDistance += hoverDistance; _updateBatteryInfo(waypointIndex); } @@ -1457,7 +1466,7 @@ void MissionController::_addCruiseTime(double cruiseTime, double cruiseDistance, _missionFlightStatus.totalTime += cruiseTime; _missionFlightStatus.cruiseTime += cruiseTime; _missionFlightStatus.cruiseDistance += cruiseDistance; - _missionFlightStatus.totalDistance += cruiseDistance; + _missionFlightStatus.plannedDistance += cruiseDistance; _updateBatteryInfo(waypointIndex); } @@ -1517,6 +1526,7 @@ void MissionController::_recalcMissionFlightStatus() bool linkStartToHome = false; bool foundRTL = false; + bool pastLandCommand = false; double totalHorizontalDistance = 0; for (int i=0; i<_visualItems->count(); i++) { @@ -1582,7 +1592,8 @@ void MissionController::_recalcMissionFlightStatus() } } - _addTimeDistance(_missionFlightStatus.vtolMode == QGCMAVLink::VehicleClassMultiRotor, 0, 0, item->additionalTimeDelay(), 0, -1); + if (!pastLandCommand) + _addTimeDistance(_missionFlightStatus.vtolMode == QGCMAVLink::VehicleClassMultiRotor, 0, 0, item->additionalTimeDelay(), 0, -1); if (item->specifiesCoordinate()) { @@ -1621,18 +1632,25 @@ void MissionController::_recalcMissionFlightStatus() double azimuth, distance, altDifference; _calcPrevWaypointValues(item, lastFlyThroughVI, &azimuth, &distance, &altDifference); - totalHorizontalDistance += distance; + + // If the last waypoint was a land command, there's a discontinuity at this point + if (!lastFlyThroughVI->isLandCommand()) { + totalHorizontalDistance += distance; + item->setDistance(distance); + + if (!pastLandCommand) { + // Calculate time/distance + double hoverTime = distance / _missionFlightStatus.hoverSpeed; + double cruiseTime = distance / _missionFlightStatus.cruiseSpeed; + _addTimeDistance(_missionFlightStatus.vtolMode == QGCMAVLink::VehicleClassMultiRotor, hoverTime, cruiseTime, 0, distance, item->sequenceNumber()); + } + } + item->setAltDifference(altDifference); item->setAzimuth(azimuth); - item->setDistance(distance); item->setDistanceFromStart(totalHorizontalDistance); _missionFlightStatus.maxTelemetryDistance = qMax(_missionFlightStatus.maxTelemetryDistance, _calcDistanceToHome(item, _settingsItem)); - - // Calculate time/distance - double hoverTime = distance / _missionFlightStatus.hoverSpeed; - double cruiseTime = distance / _missionFlightStatus.cruiseSpeed; - _addTimeDistance(_missionFlightStatus.vtolMode == QGCMAVLink::VehicleClassMultiRotor, hoverTime, cruiseTime, 0, distance, item->sequenceNumber()); } if (complexItem) { @@ -1640,9 +1658,11 @@ void MissionController::_recalcMissionFlightStatus() double distance = complexItem->complexDistance(); _missionFlightStatus.maxTelemetryDistance = qMax(_missionFlightStatus.maxTelemetryDistance, complexItem->greatestDistanceTo(complexItem->exitCoordinate())); - double hoverTime = distance / _missionFlightStatus.hoverSpeed; - double cruiseTime = distance / _missionFlightStatus.cruiseSpeed; - _addTimeDistance(_missionFlightStatus.vtolMode == QGCMAVLink::VehicleClassMultiRotor, hoverTime, cruiseTime, 0, distance, item->sequenceNumber()); + if (!pastLandCommand) { + double hoverTime = distance / _missionFlightStatus.hoverSpeed; + double cruiseTime = distance / _missionFlightStatus.cruiseSpeed; + _addTimeDistance(_missionFlightStatus.vtolMode == QGCMAVLink::VehicleClassMultiRotor, hoverTime, cruiseTime, 0, distance, item->sequenceNumber()); + } totalHorizontalDistance += distance; } @@ -1696,6 +1716,10 @@ void MissionController::_recalcMissionFlightStatus() break; } } + + if (item->isLandCommand()) { + pastLandCommand = true; + } } lastFlyThroughVI->setMissionVehicleYaw(_missionFlightStatus.vehicleYaw); @@ -1704,13 +1728,17 @@ void MissionController::_recalcMissionFlightStatus() double azimuth, distance, altDifference; _calcPrevWaypointValues(lastFlyThroughVI, _settingsItem, &azimuth, &distance, &altDifference); - // Calculate time/distance - double hoverTime = distance / _missionFlightStatus.hoverSpeed; - double cruiseTime = distance / _missionFlightStatus.cruiseSpeed; - double landTime = qAbs(altDifference) / _appSettings->offlineEditingDescentSpeed()->rawValue().toDouble(); - _addTimeDistance(_missionFlightStatus.vtolMode == QGCMAVLink::VehicleClassMultiRotor, hoverTime, cruiseTime, distance, landTime, -1); + if (!pastLandCommand) { + // Calculate time/distance + double hoverTime = distance / _missionFlightStatus.hoverSpeed; + double cruiseTime = distance / _missionFlightStatus.cruiseSpeed; + double landTime = qAbs(altDifference) / _appSettings->offlineEditingDescentSpeed()->rawValue().toDouble(); + _addTimeDistance(_missionFlightStatus.vtolMode == QGCMAVLink::VehicleClassMultiRotor, hoverTime, cruiseTime, distance, landTime, -1); + } } + _missionFlightStatus.totalDistance = totalHorizontalDistance; + if (_missionFlightStatus.mAhBattery != 0 && _missionFlightStatus.batteryChangePoint == -1) { _missionFlightStatus.batteryChangePoint = 0; } @@ -1722,7 +1750,8 @@ void MissionController::_recalcMissionFlightStatus() } emit missionMaxTelemetryChanged (_missionFlightStatus.maxTelemetryDistance); - emit missionDistanceChanged (_missionFlightStatus.totalDistance); + emit missionTotalDistanceChanged (_missionFlightStatus.totalDistance); + emit missionPlannedDistanceChanged (_missionFlightStatus.plannedDistance); emit missionHoverDistanceChanged (_missionFlightStatus.hoverDistance); emit missionCruiseDistanceChanged (_missionFlightStatus.cruiseDistance); emit missionTimeChanged (); @@ -2165,9 +2194,9 @@ void MissionController::setDirty(bool dirty) void MissionController::_scanForAdditionalSettings(QmlObjectListModel* visualItems, PlanMasterController* masterController) { - // First we look for a Landing Patterns which are at the end - if (!FixedWingLandingComplexItem::scanForItem(visualItems, _flyView, masterController)) { - VTOLLandingComplexItem::scanForItem(visualItems, _flyView, masterController); + // First we look for Landing Patterns which are at the end + if (!FixedWingLandingComplexItem::scanForItems(visualItems, _flyView, masterController)) { + VTOLLandingComplexItem::scanForItems(visualItems, _flyView, masterController); } int scanIndex = 0; @@ -2473,8 +2502,8 @@ void MissionController::setCurrentPlanViewSeqNum(int sequenceNumber, bool force) } } - if (foundLand) { - // Can't have more than one land sequence + if (foundLand && !_planViewSettings->allowMultipleLandingPatterns()->rawValue().toBool()) { + // Can't have more than one land sequence unless allowed in settings _isInsertLandValid = false; if (sequenceNumber >= landSeqNum) { // Can't have fly through commands after a land item @@ -2482,6 +2511,12 @@ void MissionController::setCurrentPlanViewSeqNum(int sequenceNumber, bool force) } } + if (_hasLandItem != foundLand) { + // Update hasLandItem if the presence of a landing item has changed + _hasLandItem = foundLand; + emit hasLandItemChanged(); + } + // These are not valid when only takeoff is allowed _isInsertLandValid = _isInsertLandValid && !_onlyInsertTakeoffValid; _flyThroughCommandsAllowed = _flyThroughCommandsAllowed && !_onlyInsertTakeoffValid; @@ -2594,7 +2629,7 @@ bool MissionController::isEmpty(void) const return _visualItems->count() <= 1; } -void MissionController::_takeoffItemNotRequiredChanged(void) +void MissionController::_forceRecalcOfAllowedBits(void) { // Force a recalc of allowed bits setCurrentPlanViewSeqNum(_currentPlanViewSeqNum, true /* force */); diff --git a/src/MissionManager/MissionController.h b/src/MissionManager/MissionController.h index ace2ef5d870..b5a6ffa7e00 100644 --- a/src/MissionManager/MissionController.h +++ b/src/MissionManager/MissionController.h @@ -51,6 +51,7 @@ class MissionController : public PlanElementController typedef struct { double maxTelemetryDistance; double totalDistance; + double plannedDistance; double totalTime; double hoverDistance; double hoverTime; @@ -90,7 +91,8 @@ class MissionController : public PlanElementController Q_PROPERTY(int currentPlanViewVIIndex READ currentPlanViewVIIndex NOTIFY currentPlanViewVIIndexChanged) Q_PROPERTY(VisualMissionItem* currentPlanViewItem READ currentPlanViewItem NOTIFY currentPlanViewItemChanged) Q_PROPERTY(TakeoffMissionItem* takeoffMissionItem READ takeoffMissionItem NOTIFY takeoffMissionItemChanged) - Q_PROPERTY(double missionDistance READ missionDistance NOTIFY missionDistanceChanged) + Q_PROPERTY(double missionTotalDistance READ missionTotalDistance NOTIFY missionTotalDistanceChanged) + Q_PROPERTY(double missionPlannedDistance READ missionPlannedDistance NOTIFY missionPlannedDistanceChanged) Q_PROPERTY(double missionTime READ missionTime NOTIFY missionTimeChanged) Q_PROPERTY(double missionHoverDistance READ missionHoverDistance NOTIFY missionHoverDistanceChanged) Q_PROPERTY(double missionCruiseDistance READ missionCruiseDistance NOTIFY missionCruiseDistanceChanged) @@ -106,6 +108,7 @@ class MissionController : public PlanElementController Q_PROPERTY(bool onlyInsertTakeoffValid MEMBER _onlyInsertTakeoffValid NOTIFY onlyInsertTakeoffValidChanged) Q_PROPERTY(bool isInsertTakeoffValid MEMBER _isInsertTakeoffValid NOTIFY isInsertTakeoffValidChanged) Q_PROPERTY(bool isInsertLandValid MEMBER _isInsertLandValid NOTIFY isInsertLandValidChanged) + Q_PROPERTY(bool hasLandItem MEMBER _hasLandItem NOTIFY hasLandItemChanged) Q_PROPERTY(bool isROIActive MEMBER _isROIActive NOTIFY isROIActiveChanged) Q_PROPERTY(bool isROIBeginCurrentItem MEMBER _isROIBeginCurrentItem NOTIFY isROIBeginCurrentItemChanged) Q_PROPERTY(bool flyThroughCommandsAllowed MEMBER _flyThroughCommandsAllowed NOTIFY flyThroughCommandsAllowedChanged) @@ -243,7 +246,8 @@ class MissionController : public PlanElementController int currentPlanViewSeqNum (void) const { return _currentPlanViewSeqNum; } int currentPlanViewVIIndex (void) const { return _currentPlanViewVIIndex; } - double missionDistance (void) const { return _missionFlightStatus.totalDistance; } + double missionTotalDistance (void) const { return _missionFlightStatus.totalDistance; } + double missionPlannedDistance (void) const { return _missionFlightStatus.plannedDistance; } double missionTime (void) const { return _missionFlightStatus.totalTime; } double missionHoverDistance (void) const { return _missionFlightStatus.hoverDistance; } double missionHoverTime (void) const { return _missionFlightStatus.hoverTime; } @@ -264,7 +268,8 @@ class MissionController : public PlanElementController void visualItemsChanged (void); void splitSegmentChanged (void); void newItemsFromVehicle (void); - void missionDistanceChanged (double missionDistance); + void missionTotalDistanceChanged (double missionTotalDistance); + void missionPlannedDistanceChanged (double missionPlannedDistance); void missionTimeChanged (void); void missionHoverDistanceChanged (double missionHoverDistance); void missionHoverTimeChanged (void); @@ -289,6 +294,7 @@ class MissionController : public PlanElementController void onlyInsertTakeoffValidChanged (void); void isInsertTakeoffValidChanged (void); void isInsertLandValidChanged (void); + void hasLandItemChanged (void); void isROIActiveChanged (void); void isROIBeginCurrentItemChanged (void); void flyThroughCommandsAllowedChanged (void); @@ -316,7 +322,7 @@ private slots: void _complexBoundingBoxChanged (void); void _recalcAll (void); void _managerVehicleChanged (Vehicle* managerVehicle); - void _takeoffItemNotRequiredChanged (void); + void _forceRecalcOfAllowedBits (void); private: void _init (void); @@ -392,6 +398,7 @@ private slots: bool _onlyInsertTakeoffValid = true; bool _isInsertTakeoffValid = true; bool _isInsertLandValid = false; + bool _hasLandItem = false; bool _isROIActive = false; bool _flyThroughCommandsAllowed = false; bool _isROIBeginCurrentItem = false; diff --git a/src/MissionManager/VTOLLandingComplexItem.cc b/src/MissionManager/VTOLLandingComplexItem.cc index 8780d1f8afa..9de7e08ac82 100644 --- a/src/MissionManager/VTOLLandingComplexItem.cc +++ b/src/MissionManager/VTOLLandingComplexItem.cc @@ -118,9 +118,9 @@ bool VTOLLandingComplexItem::_isValidLandItem(const MissionItem& missionItem) } } -bool VTOLLandingComplexItem::scanForItem(QmlObjectListModel* visualItems, bool flyView, PlanMasterController* masterController) +bool VTOLLandingComplexItem::scanForItems(QmlObjectListModel* visualItems, bool flyView, PlanMasterController* masterController) { - return _scanForItem(visualItems, flyView, masterController, _isValidLandItem, _createItem); + return _scanForItems(visualItems, flyView, masterController, _isValidLandItem, _createItem); } // Never call this method directly. If you want to update the flight segments you emit _updateFlightPathSegmentsSignal() diff --git a/src/MissionManager/VTOLLandingComplexItem.h b/src/MissionManager/VTOLLandingComplexItem.h index 089b09c61e1..528e29d7e47 100644 --- a/src/MissionManager/VTOLLandingComplexItem.h +++ b/src/MissionManager/VTOLLandingComplexItem.h @@ -28,7 +28,7 @@ class VTOLLandingComplexItem : public LandingComplexItem VTOLLandingComplexItem(PlanMasterController* masterController, bool flyView); /// Scans the loaded items for a landing pattern complex item - static bool scanForItem(QmlObjectListModel* visualItems, bool flyView, PlanMasterController* masterController); + static bool scanForItems(QmlObjectListModel* visualItems, bool flyView, PlanMasterController* masterController); // Overrides from ComplexMissionItem QString patternName (void) const final { return name; } diff --git a/src/PlanView/PlanToolBarIndicators.qml b/src/PlanView/PlanToolBarIndicators.qml index 93fdd84b129..a3b9aca31de 100644 --- a/src/PlanView/PlanToolBarIndicators.qml +++ b/src/PlanView/PlanToolBarIndicators.qml @@ -20,7 +20,7 @@ Item { property var _currentMissionItem: _planMasterController.missionController.currentPlanViewItem ///< Mission item to display status for property var missionItems: _controllerValid ? _planMasterController.missionController.visualItems : undefined - property real missionDistance: _controllerValid ? _planMasterController.missionController.missionDistance : NaN + property real missionPlannedDistance: _controllerValid ? _planMasterController.missionController.missionPlannedDistance : NaN property real missionTime: _controllerValid ? _planMasterController.missionController.missionTime : 0 property real missionMaxTelemetry: _controllerValid ? _planMasterController.missionController.missionMaxTelemetry : NaN property bool missionDirty: _controllerValid ? _planMasterController.missionController.dirty : false @@ -45,7 +45,7 @@ Item { 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 _missionPlannedDistance: _missionValid ? missionPlannedDistance : NaN property real _missionMaxTelemetry: _missionValid ? missionMaxTelemetry : NaN property real _missionTime: _missionValid ? missionTime : 0 property int _batteryChangePoint: _controllerValid ? _planMasterController.missionController.batteryChangePoint : -1 @@ -57,15 +57,15 @@ Item { (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.metersToAppSettingsVerticalDistanceUnits(_altDifference).toFixed(1) + " " + QGroundControl.unitsConversion.appSettingsVerticalDistanceUnitsString - property string _gradientText: isNaN(_gradient) ? "-.-" : _gradient.toFixed(0) + qsTr(" deg") - property string _azimuthText: isNaN(_azimuth) ? "-.-" : Math.round(_azimuth) % 360 - property string _headingText: isNaN(_azimuth) ? "-.-" : Math.round(_heading) % 360 - property string _missionDistanceText: isNaN(_missionDistance) ? "-.-" : QGroundControl.unitsConversion.metersToAppSettingsHorizontalDistanceUnits(_missionDistance).toFixed(0) + " " + QGroundControl.unitsConversion.appSettingsHorizontalDistanceUnitsString - property string _missionMaxTelemetryText: isNaN(_missionMaxTelemetry) ? "-.-" : QGroundControl.unitsConversion.metersToAppSettingsHorizontalDistanceUnits(_missionMaxTelemetry).toFixed(0) + " " + QGroundControl.unitsConversion.appSettingsHorizontalDistanceUnitsString - property string _batteryChangePointText: _batteryChangePoint < 0 ? qsTr("N/A") : _batteryChangePoint - property string _batteriesRequiredText: _batteriesRequired < 0 ? qsTr("N/A") : _batteriesRequired + property string _distanceText: isNaN(_distance) ? "-.-" : QGroundControl.unitsConversion.metersToAppSettingsHorizontalDistanceUnits(_distance).toFixed(1) + " " + QGroundControl.unitsConversion.appSettingsHorizontalDistanceUnitsString + property string _altDifferenceText: isNaN(_altDifference) ? "-.-" : QGroundControl.unitsConversion.metersToAppSettingsVerticalDistanceUnits(_altDifference).toFixed(1) + " " + QGroundControl.unitsConversion.appSettingsVerticalDistanceUnitsString + property string _gradientText: isNaN(_gradient) ? "-.-" : _gradient.toFixed(0) + qsTr(" deg") + property string _azimuthText: isNaN(_azimuth) ? "-.-" : Math.round(_azimuth) % 360 + property string _headingText: isNaN(_azimuth) ? "-.-" : Math.round(_heading) % 360 + property string _missionPlannedDistanceText: isNaN(_missionPlannedDistance) ? "-.-" : QGroundControl.unitsConversion.metersToAppSettingsHorizontalDistanceUnits(_missionPlannedDistance).toFixed(0) + " " + QGroundControl.unitsConversion.appSettingsHorizontalDistanceUnitsString + property string _missionMaxTelemetryText: isNaN(_missionMaxTelemetry) ? "-.-" : QGroundControl.unitsConversion.metersToAppSettingsHorizontalDistanceUnits(_missionMaxTelemetry).toFixed(0) + " " + QGroundControl.unitsConversion.appSettingsHorizontalDistanceUnitsString + property string _batteryChangePointText: _batteryChangePoint < 0 ? qsTr("N/A") : _batteryChangePoint + property string _batteriesRequiredText: _batteriesRequired < 0 ? qsTr("N/A") : _batteriesRequired readonly property real _margins: ScreenTools.defaultFontPixelWidth @@ -189,7 +189,7 @@ Item { QGCLabel { text: qsTr("Distance:"); font.pointSize: _dataFontSize; } QGCLabel { - text: _missionDistanceText + text: _missionPlannedDistanceText font.pointSize: _dataFontSize Layout.minimumWidth: _largeValueWidth } diff --git a/src/PlanView/PlanView.qml b/src/PlanView/PlanView.qml index 12cc2e3a27a..7d9c74569d4 100644 --- a/src/PlanView/PlanView.qml +++ b/src/PlanView/PlanView.qml @@ -632,7 +632,11 @@ Item { } }, ToolStripAction { - text: _planMasterController.controllerVehicle.multiRotor ? qsTr("Return") : qsTr("Land") + text: _planMasterController.controllerVehicle.multiRotor + ? qsTr("Return") + : _missionController.isInsertLandValid && _missionController.hasLandItem + ? qsTr("Alt Land") + : qsTr("Land") iconSource: "/res/rtl.svg" enabled: _missionController.isInsertLandValid visible: toolStrip._isMissionLayer || toolStrip._isUtmspLayer diff --git a/src/PlanView/TerrainStatus.qml b/src/PlanView/TerrainStatus.qml index 5f8eb843441..13eff397032 100644 --- a/src/PlanView/TerrainStatus.qml +++ b/src/PlanView/TerrainStatus.qml @@ -26,14 +26,14 @@ Rectangle { signal setCurrentSeqNum(int seqNum) - property real _margins: ScreenTools.defaultFontPixelWidth / 2 - property var _visualItems: missionController.visualItems - property real _altRange: _maxAMSLAltitude - _minAMSLAltitude - property real _indicatorSpacing: 5 - property real _minAMSLAltitude: isNaN(terrainProfile.minAMSLAlt) ? 0 : terrainProfile.minAMSLAlt - property real _maxAMSLAltitude: isNaN(terrainProfile.maxAMSLAlt) ? 100 : terrainProfile.maxAMSLAlt - property real _missionDistance: isNaN(missionController.missionDistance) ? 100 : missionController.missionDistance - property var _unitsConversion: QGroundControl.unitsConversion + property real _margins: ScreenTools.defaultFontPixelWidth / 2 + property var _visualItems: missionController.visualItems + property real _altRange: _maxAMSLAltitude - _minAMSLAltitude + property real _indicatorSpacing: 5 + property real _minAMSLAltitude: isNaN(terrainProfile.minAMSLAlt) ? 0 : terrainProfile.minAMSLAlt + property real _maxAMSLAltitude: isNaN(terrainProfile.maxAMSLAlt) ? 100 : terrainProfile.maxAMSLAlt + property real _missionTotalDistance: isNaN(missionController.missionTotalDistance) ? 100 : missionController.missionTotalDistance + property var _unitsConversion: QGroundControl.unitsConversion QGCPalette { id: qgcPal } @@ -75,7 +75,7 @@ Rectangle { ValueAxis { id: axisX min: 0 - max: _unitsConversion.metersToAppSettingsHorizontalDistanceUnits(missionController.missionDistance) + max: _unitsConversion.metersToAppSettingsHorizontalDistanceUnits(missionController.missionTotalDistance) lineVisible: true labelsFont.family: ScreenTools.fixedFontFamily labelsFont.pointSize: ScreenTools.smallFontPointSize @@ -103,7 +103,7 @@ Rectangle { visible: true XYPoint { x: 0; y: _unitsConversion.metersToAppSettingsVerticalDistanceUnits(_minAMSLAltitude) } - XYPoint { x: _unitsConversion.metersToAppSettingsHorizontalDistanceUnits(_missionDistance); y: _unitsConversion.metersToAppSettingsVerticalDistanceUnits(_maxAMSLAltitude) } + XYPoint { x: _unitsConversion.metersToAppSettingsHorizontalDistanceUnits(_missionTotalDistance); y: _unitsConversion.metersToAppSettingsVerticalDistanceUnits(_maxAMSLAltitude) } } } diff --git a/src/QmlControls/TerrainProfile.cc b/src/QmlControls/TerrainProfile.cc index c0ad72fad85..275ccf2308a 100644 --- a/src/QmlControls/TerrainProfile.cc +++ b/src/QmlControls/TerrainProfile.cc @@ -266,7 +266,7 @@ QSGNode* TerrainProfile::updatePaintNode(QSGNode* oldNode, QQuickItem::UpdatePai qCDebug(TerrainProfileLog) << QStringLiteral("updatePaintNode counter:%1 cFlightProfileSegments:%2 cTerrainProfilePoints:%3 cMissingTerrainSegments:%4 cTerrainCollisionSegments:%5 _minAMSLAlt:%6 _maxAMSLAlt:%7 maxTerrainHeight:%8") .arg(counter++).arg(cFlightProfileSegments).arg(cTerrainProfilePoints).arg(cMissingTerrainSegments).arg(cTerrainCollisionSegments).arg(_minAMSLAlt).arg(_maxAMSLAlt).arg(maxTerrainHeight); - _pixelsPerMeter = _visibleWidth / _missionController->missionDistance(); + _pixelsPerMeter = _visibleWidth / _missionController->missionTotalDistance(); // Instantiate nodes if (!rootNode) { diff --git a/src/Settings/PlanView.SettingsGroup.json b/src/Settings/PlanView.SettingsGroup.json index 3f3557d86e1..b3e5b4ae73d 100644 --- a/src/Settings/PlanView.SettingsGroup.json +++ b/src/Settings/PlanView.SettingsGroup.json @@ -21,6 +21,12 @@ "type": "bool", "default": false }, +{ + "name": "allowMultipleLandingPatterns", + "shortDesc": "Allow configuring multiple landing sequences. The first one will be used for the mission, but in the event of an RTL, the one that is closest will be used instead.", + "type": "bool", + "default": true +}, { "name": "useConditionGate", "shortDesc": "Use MAV_CMD_CONDITION_GATE for pattern generation", diff --git a/src/Settings/PlanViewSettings.cc b/src/Settings/PlanViewSettings.cc index 07d6abd7b29..6c9512abafe 100644 --- a/src/Settings/PlanViewSettings.cc +++ b/src/Settings/PlanViewSettings.cc @@ -20,5 +20,6 @@ DECLARE_SETTINGSFACT(PlanViewSettings, displayPresetsTabFirst) DECLARE_SETTINGSFACT(PlanViewSettings, showMissionItemStatus) DECLARE_SETTINGSFACT(PlanViewSettings, useConditionGate) DECLARE_SETTINGSFACT(PlanViewSettings, takeoffItemNotRequired) +DECLARE_SETTINGSFACT(PlanViewSettings, allowMultipleLandingPatterns) DECLARE_SETTINGSFACT(PlanViewSettings, showGimbalOnlyWhenSet) DECLARE_SETTINGSFACT(PlanViewSettings, vtolTransitionDistance) diff --git a/src/Settings/PlanViewSettings.h b/src/Settings/PlanViewSettings.h index 60ef0c329ef..4e8c5fd60a0 100644 --- a/src/Settings/PlanViewSettings.h +++ b/src/Settings/PlanViewSettings.h @@ -24,6 +24,7 @@ class PlanViewSettings : public SettingsGroup DEFINE_SETTINGFACT(showMissionItemStatus) DEFINE_SETTINGFACT(useConditionGate) DEFINE_SETTINGFACT(takeoffItemNotRequired) + DEFINE_SETTINGFACT(allowMultipleLandingPatterns) DEFINE_SETTINGFACT(showGimbalOnlyWhenSet) DEFINE_SETTINGFACT(vtolTransitionDistance) }; diff --git a/src/UI/preferences/PlanViewSettings.qml b/src/UI/preferences/PlanViewSettings.qml index dc0980205a9..52475f9378d 100644 --- a/src/UI/preferences/PlanViewSettings.qml +++ b/src/UI/preferences/PlanViewSettings.qml @@ -47,9 +47,16 @@ SettingsPage { FactCheckBoxSlider { Layout.fillWidth: true - text: qsTr("Missions Do Not Require Takeoff Item") + text: qsTr("Missions do not require takeoff item") fact: _planViewSettings.takeoffItemNotRequired visible: fact.visible } + + FactCheckBoxSlider { + Layout.fillWidth: true + text: qsTr("Allow configuring multiple landing sequences") + fact: _planViewSettings.allowMultipleLandingPatterns + visible: fact.visible + } } } diff --git a/test/MissionManager/LandingComplexItemTest.cc b/test/MissionManager/LandingComplexItemTest.cc index a081258538e..215c9078b8b 100644 --- a/test/MissionManager/LandingComplexItemTest.cc +++ b/test/MissionManager/LandingComplexItemTest.cc @@ -249,7 +249,7 @@ void LandingComplexItemTest::_testScanForItems(void) visualItems->append(simpleItem); } - QVERIFY(LandingComplexItem::_scanForItem(visualItems, false /* flyView */, _masterController, &SimpleLandingComplexItem::_isValidLandItem, &SimpleLandingComplexItem::_createItem)); + QVERIFY(LandingComplexItem::_scanForItems(visualItems, false /* flyView */, _masterController, &SimpleLandingComplexItem::_isValidLandItem, &SimpleLandingComplexItem::_createItem)); QCOMPARE(visualItems->count(), 1); SimpleLandingComplexItem* scannedItem = visualItems->value(0); QVERIFY(scannedItem);