diff --git a/ChangeLog.md b/ChangeLog.md index 45f44c40f81beaa4f2f9b6d01ca98c13a67142a7..347325f47e4fffcf518759337c1f3bdce7d1702c 100644 --- a/ChangeLog.md +++ b/ChangeLog.md @@ -11,6 +11,7 @@ Note: This file only contains high level features or important fixes. ## 3.4 ### 3.4.4 - Not yet released +* Multi-Vehicle Start Mission and Pause now work correctly. Issue #6864. ### 3.4.3 * Fix bug where Resume Mission would not display correctly in some cases. Issue #6835. @@ -22,3 +23,4 @@ Note: This file only contains high level features or important fixes. ### 3.4.1 * Fix crash when Survery with terrain follow is moved quickly * Fix terrain follow climb/descent rate fields swapped in ui + diff --git a/src/FlightDisplay/FlightDisplayView.qml b/src/FlightDisplay/FlightDisplayView.qml index dcc9e410292361e229ce7be7c11a759d11d4e6bb..745134895cdc9bd790fbd40fb6554eb7d67603af 100644 --- a/src/FlightDisplay/FlightDisplayView.qml +++ b/src/FlightDisplay/FlightDisplayView.qml @@ -470,13 +470,14 @@ QGCView { } MultiVehicleList { - anchors.margins: _margins - anchors.top: singleMultiSelector.bottom - anchors.right: parent.right - anchors.bottom: parent.bottom - width: ScreenTools.defaultFontPixelWidth * 30 - visible: !singleVehicleView.checked && !QGroundControl.videoManager.fullScreen - z: _panel.z + 4 + anchors.margins: _margins + anchors.top: singleMultiSelector.bottom + anchors.right: parent.right + anchors.bottom: parent.bottom + width: ScreenTools.defaultFontPixelWidth * 30 + visible: !singleVehicleView.checked && !QGroundControl.videoManager.fullScreen + z: _panel.z + 4 + guidedActionsController: _guidedController } //-- Virtual Joystick diff --git a/src/FlightDisplay/GuidedActionsController.qml b/src/FlightDisplay/GuidedActionsController.qml index 829dbaaa4033f528773f9b02c35b46ef8b9ff9fe..aae0ed9e42f1d1bbbc3f8a72084ea52a00e99cc7 100644 --- a/src/FlightDisplay/GuidedActionsController.qml +++ b/src/FlightDisplay/GuidedActionsController.qml @@ -40,10 +40,12 @@ Item { readonly property string takeoffTitle: qsTr("Takeoff") readonly property string landTitle: qsTr("Land") readonly property string startMissionTitle: qsTr("Start Mission") + readonly property string mvStartMissionTitle: qsTr("Start Mission (MV)") readonly property string continueMissionTitle: qsTr("Continue Mission") readonly property string resumeMissionTitle: qsTr("Resume Mission") readonly property string resumeMissionUploadFailTitle: qsTr("Resume FAILED") readonly property string pauseTitle: qsTr("Pause") + readonly property string mvPauseTitle: qsTr("Pause (MV)") readonly property string changeAltTitle: qsTr("Change Altitude") readonly property string orbitTitle: qsTr("Orbit") readonly property string landAbortTitle: qsTr("Land Abort") @@ -101,18 +103,19 @@ Item { property bool showTakeoff: _guidedActionsEnabled && _activeVehicle.takeoffVehicleSupported && !_vehicleFlying property bool showLand: _guidedActionsEnabled && _activeVehicle.guidedModeSupported && _vehicleArmed && !_activeVehicle.fixedWing && !_vehicleInLandMode property bool showStartMission: _guidedActionsEnabled && _missionAvailable && !_missionActive && !_vehicleFlying - property bool showContinueMission: _guidedActionsEnabled && _missionAvailable && !_missionActive && _vehicleArmed && _vehicleFlying && (_currentMissionIndex < missionController.visualItems.count - 1) + property bool showContinueMission: _guidedActionsEnabled && _missionAvailable && !_missionActive && _vehicleArmed && _vehicleFlying && (_currentMissionIndex < _missionItemCount - 1) property bool showPause: _guidedActionsEnabled && _vehicleArmed && _activeVehicle.pauseVehicleSupported && _vehicleFlying && !_vehiclePaused property bool showChangeAlt: _guidedActionsEnabled && _vehicleFlying && _activeVehicle.guidedModeSupported && _vehicleArmed && !_missionActive property bool showOrbit: _guidedActionsEnabled && !_hideOrbit && _vehicleFlying && _activeVehicle.orbitModeSupported && !_missionActive property bool showLandAbort: _guidedActionsEnabled && _vehicleFlying && _activeVehicle.fixedWing && _vehicleLanding property bool showGotoLocation: _guidedActionsEnabled && _vehicleFlying - // Note: The 'missionController.visualItems.count - 3' 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 < missionController.visualItems.count - 3) + // 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) property bool guidedUIVisible: guidedActionConfirm.visible || guidedActionList.visible + property var _corePlugin: QGroundControl.corePlugin property bool _guidedActionsEnabled: (!ScreenTools.isDebug && QGroundControl.corePlugin.options.guidedActionsRequireRCRSSI && _activeVehicle) ? _rcRSSIAvailable : _activeVehicle property var _activeVehicle: QGroundControl.multiVehicleManager.activeVehicle property string _flightMode: _activeVehicle ? _activeVehicle.flightMode : "" @@ -125,6 +128,7 @@ Item { property bool _vehicleInMissionMode: false property bool _vehicleInRTLMode: false property bool _vehicleInLandMode: false + property int _missionItemCount: missionController.missionItemCount property int _currentMissionIndex: missionController.currentMissionIndex property int _resumeMissionIndex: missionController.resumeMissionIndex property bool _hideEmergenyStop: !QGroundControl.corePlugin.options.guidedBarShowEmergencyStop @@ -132,15 +136,14 @@ Item { property bool _vehicleWasFlying: false property bool _rcRSSIAvailable: _activeVehicle ? _activeVehicle.rcRSSI > 0 && _activeVehicle.rcRSSI <= 100 : false - //Handy code for debugging state problems - property bool __debugGuidedStates: false + // You can turn on log output for GuidedActionsController by turning on GuidedActionsControllerLog category property bool __guidedModeSupported: _activeVehicle ? _activeVehicle.guidedModeSupported : false property bool __pauseVehicleSupported: _activeVehicle ? _activeVehicle.pauseVehicleSupported : false property bool __flightMode: _flightMode function _outputState() { - if (__debugGuidedStates) { - console.log(qsTr("_activeVehicle(%1) _vehicleArmed(%2) guidedModeSupported(%3) _vehicleFlying(%4) _vehicleInRTLMode(%5) pauseVehicleSupported(%6) _vehiclePaused(%7) _flightMode(%8)").arg(_activeVehicle ? 1 : 0).arg(_vehicleArmed ? 1 : 0).arg(__guidedModeSupported ? 1 : 0).arg(_vehicleFlying ? 1 : 0).arg(_vehicleInRTLMode ? 1 : 0).arg(__pauseVehicleSupported ? 1 : 0).arg(_vehiclePaused ? 1 : 0).arg(_flightMode)) + if (_corePlugin.guidedActionsControllerLogging()) { + console.log(qsTr("_activeVehicle(%1) _vehicleArmed(%2) guidedModeSupported(%3) _vehicleFlying(%4) _vehicleWasFlying(%5) _vehicleInRTLMode(%6) pauseVehicleSupported(%7) _vehiclePaused(%8) _flightMode(%9) _missionItemCount(%10)").arg(_activeVehicle ? 1 : 0).arg(_vehicleArmed ? 1 : 0).arg(__guidedModeSupported ? 1 : 0).arg(_vehicleFlying ? 1 : 0).arg(_vehicleWasFlying ? 1 : 0).arg(_vehicleInRTLMode ? 1 : 0).arg(__pauseVehicleSupported ? 1 : 0).arg(_vehiclePaused ? 1 : 0).arg(_flightMode).arg(_missionItemCount)) } } @@ -152,31 +155,32 @@ Item { on__FlightModeChanged: _outputState() on__GuidedModeSupportedChanged: _outputState() on__PauseVehicleSupportedChanged: _outputState() + on_MissionItemCountChanged: _outputState() on_CurrentMissionIndexChanged: { - if (__debugGuidedStates) { + if (_corePlugin.guidedActionsControllerLogging()) { console.log("_currentMissionIndex", _currentMissionIndex) } } on_ResumeMissionIndexChanged: { - if (__debugGuidedStates) { + if (_corePlugin.guidedActionsControllerLogging()) { console.log("_resumeMissionIndex", _resumeMissionIndex) } } onShowResumeMissionChanged: { - if (__debugGuidedStates) { + if (_corePlugin.guidedActionsControllerLogging()) { console.log("showResumeMission", showResumeMission) } _outputState() } onShowStartMissionChanged: { - if (__debugGuidedStates) { + if (_corePlugin.guidedActionsControllerLogging()) { console.log("showStartMission", showStartMission) } _outputState() } onShowContinueMissionChanged: { - if (__debugGuidedStates) { + if (_corePlugin.guidedActionsControllerLogging()) { console.log("showContinueMission", showContinueMission) } _outputState() @@ -245,7 +249,7 @@ Item { confirmDialog.hideTrigger = Qt.binding(function() { return !showStartMission }) break; case actionMVStartMission: - confirmDialog.title = startMissionTitle + confirmDialog.title = mvStartMissionTitle confirmDialog.message = startMissionMessage confirmDialog.hideTrigger = true break; @@ -317,7 +321,7 @@ Item { altitudeSlider.visible = true break; case actionMVPause: - confirmDialog.title = pauseTitle + confirmDialog.title = mvPauseTitle confirmDialog.message = mvPauseMessage confirmDialog.hideTrigger = true break; diff --git a/src/MissionManager/MissionController.cc b/src/MissionManager/MissionController.cc index 3b6a28843450adde9c96dd69019db44709b3fb51..5245141214dc18249333f7d4189a032cc58ef0fe 100644 --- a/src/MissionManager/MissionController.cc +++ b/src/MissionManager/MissionController.cc @@ -58,6 +58,7 @@ const int MissionController::_missionFileVersion = 2; MissionController::MissionController(PlanMasterController* masterController, QObject *parent) : PlanElementController (masterController, parent) , _missionManager (_managerVehicle->missionManager()) + , _missionItemCount (0) , _visualItems (nullptr) , _settingsItem (nullptr) , _firstItemsFromVehicle (false) @@ -163,6 +164,9 @@ void MissionController::_newMissionItemsAvailableFromVehicle(bool removeAllReque const QList& newMissionItems = _missionManager->missionItems(); qCDebug(MissionControllerLog) << "loading from vehicle: count"<< newMissionItems.count(); + _missionItemCount = newMissionItems.count(); + emit missionItemCountChanged(_missionItemCount); + int i = 0; if (_controllerVehicle->firmwarePlugin()->sendHomePositionToVehicle() && newMissionItems.count() != 0) { // First item is fake home position diff --git a/src/MissionManager/MissionController.h b/src/MissionManager/MissionController.h index 55387398fdfbe874ffd5251a3896c6f4e18dd90f..293807f6ffd390c9fa01187c69dd529eeeff33e4 100644 --- a/src/MissionManager/MissionController.h +++ b/src/MissionManager/MissionController.h @@ -75,6 +75,7 @@ public: Q_PROPERTY(double progressPct READ progressPct NOTIFY progressPctChanged) + Q_PROPERTY(int missionItemCount READ missionItemCount NOTIFY missionItemCountChanged) ///< True mission item command count (only valid in Fly View) Q_PROPERTY(int currentMissionIndex READ currentMissionIndex NOTIFY currentMissionIndexChanged) Q_PROPERTY(int resumeMissionIndex READ resumeMissionIndex NOTIFY resumeMissionIndexChanged) ///< Returns the item index two which a mission should be resumed. -1 indicates resume mission not available. @@ -176,6 +177,7 @@ public: QString corridorScanComplexItemName (void) const { return _corridorScanMissionItemName; } QString structureScanComplexItemName(void) const { return _structureScanMissionItemName; } + int missionItemCount (void) const { return _missionItemCount; } int currentMissionIndex (void) const; int resumeMissionIndex (void) const; int currentPlanViewIndex (void) const; @@ -215,6 +217,7 @@ signals: void currentPlanViewIndexChanged (void); void currentPlanViewItemChanged (void); void missionBoundingCubeChanged (void); + void missionItemCountChanged (int missionItemCount); private slots: void _newMissionItemsAvailableFromVehicle(bool removeAllRequested); @@ -269,6 +272,7 @@ private: private: MissionManager* _missionManager; + int _missionItemCount; QmlObjectListModel* _visualItems; MissionSettingsItem* _settingsItem; QmlObjectListModel _waypointLines; diff --git a/src/QGCLoggingCategory.cc b/src/QGCLoggingCategory.cc index fc1670c34af1debd52768fa35000074adb818afe..c39126f02145bea0749cc6e7bef9f01246014631 100644 --- a/src/QGCLoggingCategory.cc +++ b/src/QGCLoggingCategory.cc @@ -16,13 +16,14 @@ #include // Add Global logging categories (not class specific) here using QGC_LOGGING_CATEGORY -QGC_LOGGING_CATEGORY(FirmwareUpgradeLog, "FirmwareUpgradeLog") -QGC_LOGGING_CATEGORY(FirmwareUpgradeVerboseLog, "FirmwareUpgradeVerboseLog") -QGC_LOGGING_CATEGORY(MissionCommandsLog, "MissionCommandsLog") -QGC_LOGGING_CATEGORY(MissionItemLog, "MissionItemLog") -QGC_LOGGING_CATEGORY(ParameterManagerLog, "ParameterManagerLog") -QGC_LOGGING_CATEGORY(GeotaggingLog, "GeotaggingLog") -QGC_LOGGING_CATEGORY(RTKGPSLog, "RTKGPSLog") +QGC_LOGGING_CATEGORY(FirmwareUpgradeLog, "FirmwareUpgradeLog") +QGC_LOGGING_CATEGORY(FirmwareUpgradeVerboseLog, "FirmwareUpgradeVerboseLog") +QGC_LOGGING_CATEGORY(MissionCommandsLog, "MissionCommandsLog") +QGC_LOGGING_CATEGORY(MissionItemLog, "MissionItemLog") +QGC_LOGGING_CATEGORY(ParameterManagerLog, "ParameterManagerLog") +QGC_LOGGING_CATEGORY(GeotaggingLog, "GeotaggingLog") +QGC_LOGGING_CATEGORY(RTKGPSLog, "RTKGPSLog") +QGC_LOGGING_CATEGORY(GuidedActionsControllerLog, "GuidedActionsControllerLog") QGCLoggingCategoryRegister* _instance = NULL; const char* QGCLoggingCategoryRegister::_filterRulesSettingsGroup = "LoggingFilters"; diff --git a/src/QGCLoggingCategory.h b/src/QGCLoggingCategory.h index 15ba4c6bc695e608bc4eaef293ee7e84046f696a..15683c9a54d34ff6749f0062278677cb99a7ccda 100644 --- a/src/QGCLoggingCategory.h +++ b/src/QGCLoggingCategory.h @@ -25,6 +25,7 @@ Q_DECLARE_LOGGING_CATEGORY(MissionItemLog) Q_DECLARE_LOGGING_CATEGORY(ParameterManagerLog) Q_DECLARE_LOGGING_CATEGORY(GeotaggingLog) Q_DECLARE_LOGGING_CATEGORY(RTKGPSLog) +Q_DECLARE_LOGGING_CATEGORY(GuidedActionsControllerLog) /// @def QGC_LOGGING_CATEGORY /// This is a QGC specific replacement for Q_LOGGING_CATEGORY. It will register the category name into a diff --git a/src/api/QGCCorePlugin.cc b/src/api/QGCCorePlugin.cc index 5302be57df48df13d0105017301ea448cc7d4597..b9e45ae45d7230785aff37d9ab4cc7bb567e5212 100644 --- a/src/api/QGCCorePlugin.cc +++ b/src/api/QGCCorePlugin.cc @@ -16,6 +16,7 @@ #include "AppMessages.h" #include "QmlObjectListModel.h" #include "VideoReceiver.h" +#include "QGCLoggingCategory.h" #include #include @@ -313,3 +314,8 @@ VideoReceiver* QGCCorePlugin::createVideoReceiver(QObject* parent) { return new VideoReceiver(parent); } + +bool QGCCorePlugin::guidedActionsControllerLogging(void) const +{ + return GuidedActionsControllerLog().isDebugEnabled(); +} diff --git a/src/api/QGCCorePlugin.h b/src/api/QGCCorePlugin.h index 392db3c88e524e43f9c6a5c505463c7acf4d79c5..5fb249353c1b52576e533bc22af25b83e0ff7264 100644 --- a/src/api/QGCCorePlugin.h +++ b/src/api/QGCCorePlugin.h @@ -52,6 +52,8 @@ public: Q_PROPERTY(QString brandImageOutdoor READ brandImageOutdoor CONSTANT) Q_PROPERTY(QmlObjectListModel* customMapItems READ customMapItems CONSTANT) + Q_INVOKABLE bool guidedActionsControllerLogging(void) const; + /// The list of settings under the Settings Menu /// @return A list of QGCSettings virtual QVariantList& settingsPages(void); diff --git a/src/comm/LogReplayLink.cc b/src/comm/LogReplayLink.cc index 7c2cc4f9b0902d71a8c5e24662cce93e6d2b83dc..a97a84b33b045c408bfba35fc6f895b95dadf921 100644 --- a/src/comm/LogReplayLink.cc +++ b/src/comm/LogReplayLink.cc @@ -269,7 +269,7 @@ bool LogReplayLink::_loadLogFile(void) // timestamp size. This guarantees that we will hit a MAVLink packet before // the end of the file. Unfortunately, it basically guarantees that we will // hit more than one. This is why we have to search for a bit. - qint64 fileLoc = _logFile.size() - MAVLINK_MAX_PACKET_LEN - cbTimestamp; + qint64 fileLoc = _logFile.size() - ((MAVLINK_MAX_PACKET_LEN - cbTimestamp) * 2); _logFile.seek(fileLoc); quint64 endTimeUSecs = startTimeUSecs; // Set a sane default for the endtime mavlink_message_t msg;