From 916d6ebd4b583d4d7609960f75d84b749b58e32e Mon Sep 17 00:00:00 2001 From: DonLakeFlyer Date: Sat, 29 Feb 2020 15:44:41 -0800 Subject: [PATCH] Mavlink terrain protocol support --- qgroundcontrol.pro | 4 + qgroundcontrol.qrc | 2 + src/FirmwarePlugin/APM/APMFirmwarePlugin.h | 1 - src/FirmwarePlugin/FirmwarePlugin.cc | 6 - src/FirmwarePlugin/FirmwarePlugin.h | 3 - src/FirmwarePlugin/PX4/PX4FirmwarePlugin.h | 1 - src/FlightDisplay/TerrainProgress.qml | 93 +++++++++++ src/FlightMap/Widgets/QGCInstrumentWidget.qml | 95 +++++------ src/MissionManager/CameraCalc.cc | 8 +- src/MissionManager/CameraCalc.h | 5 +- src/MissionManager/CameraCalcTest.cc | 8 +- src/MissionManager/CameraCalcTest.h | 8 +- src/MissionManager/CameraSection.cc | 17 +- src/MissionManager/CameraSection.h | 4 +- src/MissionManager/CameraSectionTest.cc | 84 +++++----- src/MissionManager/CameraSectionTest.h | 7 +- src/MissionManager/ComplexMissionItem.cc | 5 +- src/MissionManager/ComplexMissionItem.h | 9 +- src/MissionManager/CorridorScanComplexItem.cc | 5 +- src/MissionManager/CorridorScanComplexItem.h | 3 +- .../CorridorScanComplexItemTest.cc | 7 +- .../CorridorScanComplexItemTest.h | 8 +- src/MissionManager/FWLandingPatternTest.cc | 30 ++-- src/MissionManager/FWLandingPatternTest.h | 16 +- .../FixedWingLandingComplexItem.cc | 11 +- .../FixedWingLandingComplexItem.h | 5 +- src/MissionManager/GeoFenceController.cc | 14 +- src/MissionManager/GeoFenceController.h | 13 +- src/MissionManager/MissionController.cc | 94 +++++------ src/MissionManager/MissionController.h | 33 ++-- src/MissionManager/MissionItemTest.cc | 13 +- src/MissionManager/MissionItemTest.h | 11 +- src/MissionManager/MissionSettingsItem.cc | 14 +- src/MissionManager/MissionSettingsItem.h | 5 +- src/MissionManager/MissionSettingsTest.cc | 2 +- src/MissionManager/PlanElementController.cc | 7 - src/MissionManager/PlanElementController.h | 16 +- src/MissionManager/PlanMasterController.cc | 90 +++++++---- src/MissionManager/PlanMasterController.h | 38 +++-- src/MissionManager/RallyPointController.cc | 22 ++- src/MissionManager/RallyPointController.h | 29 ++-- src/MissionManager/Section.h | 10 +- src/MissionManager/SectionTest.cc | 6 +- src/MissionManager/SimpleMissionItem.cc | 65 +++----- src/MissionManager/SimpleMissionItem.h | 27 ++-- src/MissionManager/SimpleMissionItemTest.cc | 11 +- src/MissionManager/SpeedSection.cc | 19 +-- src/MissionManager/SpeedSection.h | 4 +- src/MissionManager/SpeedSectionTest.cc | 13 +- .../StructureScanComplexItem.cc | 7 +- src/MissionManager/StructureScanComplexItem.h | 5 +- .../StructureScanComplexItemTest.cc | 10 +- .../StructureScanComplexItemTest.h | 8 +- src/MissionManager/SurveyComplexItem.cc | 7 +- src/MissionManager/SurveyComplexItem.h | 5 +- src/MissionManager/SurveyComplexItemTest.cc | 7 +- src/MissionManager/SurveyComplexItemTest.h | 10 +- src/MissionManager/TakeoffMissionItem.cc | 15 +- src/MissionManager/TakeoffMissionItem.h | 8 +- .../TransectStyleComplexItem.cc | 10 +- src/MissionManager/TransectStyleComplexItem.h | 4 +- .../TransectStyleComplexItemTest.cc | 11 +- .../TransectStyleComplexItemTest.h | 10 +- src/MissionManager/VisualMissionItem.cc | 17 +- src/MissionManager/VisualMissionItem.h | 51 +++--- src/MissionManager/VisualMissionItemTest.cc | 11 +- src/MissionManager/VisualMissionItemTest.h | 5 +- src/PlanView/SimpleItemEditor.qml | 9 +- .../QGroundControl/FlightDisplay/qmldir | 1 + src/Terrain/TerrainQuery.cc | 23 +-- src/Terrain/TerrainQuery.h | 11 +- src/Vehicle/TerrainFactGroup.cc | 22 +++ src/Vehicle/TerrainFactGroup.h | 35 ++++ src/Vehicle/TerrainFactGroup.json | 16 ++ src/Vehicle/TerrainProtocolHandler.cc | 152 ++++++++++++++++++ src/Vehicle/TerrainProtocolHandler.h | 46 ++++++ src/Vehicle/Vehicle.cc | 25 ++- src/Vehicle/Vehicle.h | 10 +- src/comm/MockLink.cc | 2 +- 79 files changed, 988 insertions(+), 566 deletions(-) create mode 100644 src/FlightDisplay/TerrainProgress.qml create mode 100644 src/Vehicle/TerrainFactGroup.cc create mode 100644 src/Vehicle/TerrainFactGroup.h create mode 100644 src/Vehicle/TerrainFactGroup.json create mode 100644 src/Vehicle/TerrainProtocolHandler.cc create mode 100644 src/Vehicle/TerrainProtocolHandler.h diff --git a/qgroundcontrol.pro b/qgroundcontrol.pro index b6ca3355d..8bbccce11 100644 --- a/qgroundcontrol.pro +++ b/qgroundcontrol.pro @@ -669,6 +669,8 @@ HEADERS += \ src/Vehicle/GPSRTKFactGroup.h \ src/Vehicle/MAVLinkLogManager.h \ src/Vehicle/MultiVehicleManager.h \ + src/Vehicle/TerrainFactGroup.h \ + src/Vehicle/TerrainProtocolHandler.h \ src/Vehicle/TrajectoryPoints.h \ src/Vehicle/Vehicle.h \ src/Vehicle/VehicleObjectAvoidance.h \ @@ -872,6 +874,8 @@ SOURCES += \ src/Vehicle/GPSRTKFactGroup.cc \ src/Vehicle/MAVLinkLogManager.cc \ src/Vehicle/MultiVehicleManager.cc \ + src/Vehicle/TerrainFactGroup.cc \ + src/Vehicle/TerrainProtocolHandler.cc \ src/Vehicle/TrajectoryPoints.cc \ src/Vehicle/Vehicle.cc \ src/Vehicle/VehicleObjectAvoidance.cc \ diff --git a/qgroundcontrol.qrc b/qgroundcontrol.qrc index 181621741..f36381509 100644 --- a/qgroundcontrol.qrc +++ b/qgroundcontrol.qrc @@ -197,6 +197,7 @@ src/FlightDisplay/PreFlightRCCheck.qml src/FlightDisplay/PreFlightSensorsHealthCheck.qml src/FlightDisplay/PreFlightSoundCheck.qml + src/FlightDisplay/TerrainProgress.qml src/QmlControls/QGroundControl/FlightDisplay/qmldir src/FlightMap/MapItems/CameraTriggerIndicator.qml src/FlightMap/Widgets/CenterMapDropButton.qml @@ -285,6 +286,7 @@ src/Vehicle/SetpointFact.json src/Vehicle/SubmarineFact.json src/Vehicle/TemperatureFact.json + src/Vehicle/TerrainFactGroup.json src/Vehicle/VehicleFact.json src/Vehicle/VibrationFact.json src/Vehicle/WindFact.json diff --git a/src/FirmwarePlugin/APM/APMFirmwarePlugin.h b/src/FirmwarePlugin/APM/APMFirmwarePlugin.h index 5b422ca98..826ea6e13 100644 --- a/src/FirmwarePlugin/APM/APMFirmwarePlugin.h +++ b/src/FirmwarePlugin/APM/APMFirmwarePlugin.h @@ -105,7 +105,6 @@ public: QObject* loadParameterMetaData (const QString& metaDataFile) override; QString brandImageIndoor (const Vehicle* vehicle) const override { Q_UNUSED(vehicle); return QStringLiteral("/qmlimages/APM/BrandImage"); } QString brandImageOutdoor (const Vehicle* vehicle) const override { Q_UNUSED(vehicle); return QStringLiteral("/qmlimages/APM/BrandImage"); } - bool supportsTerrainFrame (void) const override { return true; } protected: /// All access to singleton is through stack specific implementation diff --git a/src/FirmwarePlugin/FirmwarePlugin.cc b/src/FirmwarePlugin/FirmwarePlugin.cc index 88e82a6e3..29857ce59 100644 --- a/src/FirmwarePlugin/FirmwarePlugin.cc +++ b/src/FirmwarePlugin/FirmwarePlugin.cc @@ -148,12 +148,6 @@ bool FirmwarePlugin::supportsJSButton(void) return false; } -bool FirmwarePlugin::supportsTerrainFrame(void) const -{ - // Generic firmware supports this since we don't know - return true; -} - bool FirmwarePlugin::adjustIncomingMavlinkMessage(Vehicle* vehicle, mavlink_message_t* message) { Q_UNUSED(vehicle); diff --git a/src/FirmwarePlugin/FirmwarePlugin.h b/src/FirmwarePlugin/FirmwarePlugin.h index 3bddb14b9..8f615d19b 100644 --- a/src/FirmwarePlugin/FirmwarePlugin.h +++ b/src/FirmwarePlugin/FirmwarePlugin.h @@ -182,9 +182,6 @@ public: /// (CompassMot). Default is true. virtual bool supportsMotorInterference(void); - /// Returns true if the firmware supports MAV_FRAME_GLOBAL_TERRAIN_ALT - virtual bool supportsTerrainFrame(void) const; - /// Called before any mavlink message is processed by Vehicle such that the firmwre plugin /// can adjust any message characteristics. This is handy to adjust or differences in mavlink /// spec implementations such that the base code can remain mavlink generic. diff --git a/src/FirmwarePlugin/PX4/PX4FirmwarePlugin.h b/src/FirmwarePlugin/PX4/PX4FirmwarePlugin.h index ed69b3f92..cf522c4f4 100644 --- a/src/FirmwarePlugin/PX4/PX4FirmwarePlugin.h +++ b/src/FirmwarePlugin/PX4/PX4FirmwarePlugin.h @@ -68,7 +68,6 @@ public: QString brandImageOutdoor (const Vehicle* vehicle) const override { Q_UNUSED(vehicle); return QStringLiteral("/qmlimages/PX4/BrandImage"); } QString autoDisarmParameter (Vehicle* vehicle) override { Q_UNUSED(vehicle); return QStringLiteral("COM_DISARM_LAND"); } uint32_t highLatencyCustomModeTo32Bits (uint16_t hlCustomMode) override; - bool supportsTerrainFrame (void) const override { return false; } bool supportsNegativeThrust (Vehicle *vehicle) override; protected: diff --git a/src/FlightDisplay/TerrainProgress.qml b/src/FlightDisplay/TerrainProgress.qml new file mode 100644 index 000000000..23d840e8e --- /dev/null +++ b/src/FlightDisplay/TerrainProgress.qml @@ -0,0 +1,93 @@ +/**************************************************************************** + * + * (c) 2009-2016 QGROUNDCONTROL PROJECT + * + * QGroundControl is licensed according to the terms in the file + * COPYING.md in the root of the source code directory. + * + ****************************************************************************/ + +import QtQuick 2.12 +import QtQuick.Layouts 1.12 + +import QGroundControl 1.0 +import QGroundControl.ScreenTools 1.0 +import QGroundControl.Vehicle 1.0 +import QGroundControl.Controls 1.0 +import QGroundControl.Palette 1.0 + +Rectangle { + height: mainLayout.height + (_margins * 2) + visible: false + color: qgcPal.window + + property var _activeVehicle: QGroundControl.multiVehicleManager.activeVehicle + property real _margins: ScreenTools.defaultFontPixelWidth / 2 + property real _totalBlocks: _activeVehicle ? _activeVehicle.terrain.blocksPending.rawValue + _activeVehicle.terrain.blocksLoaded.rawValue : 0 + property real _blocksLoaded: _activeVehicle ? _activeVehicle.terrain.blocksLoaded.rawValue : 0 + property real _blocksPending: _activeVehicle ? _activeVehicle.terrain.blocksPending.rawValue : 0 + property real _pctComplete: _activeVehicle && _totalBlocks ? _blocksLoaded / _totalBlocks : 0 + + on_BlocksPendingChanged: { + if (_blocksPending == 0) { + // UI doesn't go away immediately + visibilityTimer.restart() + } else { + visible = true + visibilityTimer.stop() + } + } + + on_BlocksLoadedChanged: { + if (_blocksLoaded != 0) { + // This causes the progress indicator to display even if it starts out as complete + visible = true + if (_blocksPending == 0) { + visibilityTimer.restart() + } + } + } + + Timer { + id: visibilityTimer + interval: 30 * 1000 + onTriggered: parent.visible = false + } + + QGCPalette { id: qgcPal } + + ColumnLayout { + id: mainLayout + anchors.margins: _margins + anchors.top: parent.top + anchors.left: parent.left + anchors.right: parent.right + spacing: _margins + + QGCLabel { + Layout.alignment: Qt.AlignHCenter + text: qsTr("Terrain Load Progress") + font.pointSize: ScreenTools.smallFontPointSize + } + + Rectangle { + Layout.fillWidth: true + height: ScreenTools.defaultFontPixelHeight + color: "transparent" + border.color: "green" + + Rectangle { + anchors.top: parent.top + anchors.bottom: parent.bottom + color: "green" + width: parent.width * _pctComplete + + QGCLabel { + anchors.centerIn: parent + text: qsTr("Done") + visible: _blocksPending == 0 + } + } + } + } +} diff --git a/src/FlightMap/Widgets/QGCInstrumentWidget.qml b/src/FlightMap/Widgets/QGCInstrumentWidget.qml index 4ad408e65..2e8b5eb69 100644 --- a/src/FlightMap/Widgets/QGCInstrumentWidget.qml +++ b/src/FlightMap/Widgets/QGCInstrumentWidget.qml @@ -7,73 +7,76 @@ * ****************************************************************************/ - -import QtQuick 2.3 +import QtQuick 2.12 +import QtQuick.Layouts 1.12 import QGroundControl 1.0 import QGroundControl.Controls 1.0 import QGroundControl.ScreenTools 1.0 import QGroundControl.FactSystem 1.0 import QGroundControl.FlightMap 1.0 +import QGroundControl.FlightDisplay 1.0 import QGroundControl.Palette 1.0 -Rectangle { - id: root - width: getPreferredInstrumentWidth() - height: _outerRadius * 2 - radius: _outerRadius - color: qgcPal.window - border.width: 1 - border.color: _isSatellite ? qgcPal.mapWidgetBorderLight : qgcPal.mapWidgetBorderDark - - property real _innerRadius: (width - (_topBottomMargin * 3)) / 4 - property real _outerRadius: _innerRadius + _topBottomMargin - property real _defaultSize: ScreenTools.defaultFontPixelHeight * (9) - property real _sizeRatio: ScreenTools.isTinyScreen ? (width / _defaultSize) * 0.5 : width / _defaultSize - property real _bigFontSize: ScreenTools.defaultFontPointSize * 2.5 * _sizeRatio - property real _normalFontSize: ScreenTools.defaultFontPointSize * 1.5 * _sizeRatio - property real _labelFontSize: ScreenTools.defaultFontPointSize * 0.75 * _sizeRatio - property real _spacing: ScreenTools.defaultFontPixelHeight * 0.33 - property real _topBottomMargin: (width * 0.05) / 2 - property real _availableValueHeight: maxHeight - (root.height + _valuesItem.anchors.topMargin) +ColumnLayout { + id: root + width: getPreferredInstrumentWidth() + spacing: ScreenTools.defaultFontPixelHeight / 4 - // Prevent all clicks from going through to lower layers - DeadMouseArea { - anchors.fill: parent - } + property real _innerRadius: (width - (_topBottomMargin * 3)) / 4 + property real _outerRadius: _innerRadius + _topBottomMargin + property real _defaultSize: ScreenTools.defaultFontPixelHeight * (9) + property real _sizeRatio: ScreenTools.isTinyScreen ? (width / _defaultSize) * 0.5 : width / _defaultSize + property real _bigFontSize: ScreenTools.defaultFontPointSize * 2.5 * _sizeRatio + property real _normalFontSize: ScreenTools.defaultFontPointSize * 1.5 * _sizeRatio + property real _labelFontSize: ScreenTools.defaultFontPointSize * 0.75 * _sizeRatio + property real _spacing: ScreenTools.defaultFontPixelHeight * 0.33 + property real _topBottomMargin: (width * 0.05) / 2 + property real _availableValueHeight: maxHeight - _valuesItem.y QGCPalette { id: qgcPal } - QGCAttitudeWidget { - id: attitude - anchors.leftMargin: _topBottomMargin - anchors.left: parent.left - size: _innerRadius * 2 - vehicle: activeVehicle - anchors.verticalCenter: parent.verticalCenter + Rectangle { + id: visualInstrument + height: _outerRadius * 2 + Layout.fillWidth: true + radius: _outerRadius + color: qgcPal.window + border.width: 1 + border.color: _isSatellite ? qgcPal.mapWidgetBorderLight : qgcPal.mapWidgetBorderDark + + DeadMouseArea { anchors.fill: parent } + + QGCAttitudeWidget { + id: attitude + anchors.leftMargin: _topBottomMargin + anchors.left: parent.left + size: _innerRadius * 2 + vehicle: activeVehicle + anchors.verticalCenter: parent.verticalCenter + } + + QGCCompassWidget { + id: compass + anchors.leftMargin: _spacing + anchors.left: attitude.right + size: _innerRadius * 2 + vehicle: activeVehicle + anchors.verticalCenter: parent.verticalCenter + } } - QGCCompassWidget { - id: compass - anchors.leftMargin: _spacing - anchors.left: attitude.right - size: _innerRadius * 2 - vehicle: activeVehicle - anchors.verticalCenter: parent.verticalCenter + TerrainProgress { + Layout.fillWidth: true } Item { id: _valuesItem - anchors.topMargin: ScreenTools.defaultFontPixelHeight / 4 - anchors.top: parent.bottom - width: parent.width + Layout.fillWidth: true height: _valuesWidget.height visible: widgetRoot.showValues - // Prevent all clicks from going through to lower layers - DeadMouseArea { - anchors.fill: parent - } + DeadMouseArea { anchors.fill: parent } Rectangle { anchors.fill: _valuesWidget diff --git a/src/MissionManager/CameraCalc.cc b/src/MissionManager/CameraCalc.cc index 36a1df311..06c8edca3 100644 --- a/src/MissionManager/CameraCalc.cc +++ b/src/MissionManager/CameraCalc.cc @@ -11,6 +11,7 @@ #include "JsonHelper.h" #include "Vehicle.h" #include "CameraMetaData.h" +#include "PlanMasterController.h" #include @@ -26,10 +27,9 @@ const char* CameraCalc::adjustedFootprintSideName = "AdjustedFootprintSi const char* CameraCalc::_jsonCameraSpecTypeKey = "CameraSpecType"; -CameraCalc::CameraCalc(Vehicle* vehicle, const QString& settingsGroup, QObject* parent) +CameraCalc::CameraCalc(PlanMasterController* masterController, const QString& settingsGroup, QObject* parent) : CameraSpec (settingsGroup, parent) - , _vehicle (vehicle) - , _dirty (false) + , _dirty (masterController) , _disableRecalc (false) , _distanceToSurfaceRelative (true) , _metaDataMap (FactMetaData::createMapFromJsonFile(QStringLiteral(":/json/CameraCalc.FactMetaData.json"), this)) @@ -43,7 +43,7 @@ CameraCalc::CameraCalc(Vehicle* vehicle, const QString& settingsGroup, QObject* , _adjustedFootprintFrontalFact (settingsGroup, _metaDataMap[adjustedFootprintFrontalName]) , _imageFootprintSide (0) , _imageFootprintFrontal (0) - , _knownCameraList (_vehicle->staticCameraList()) + , _knownCameraList (masterController->controllerVehicle()->staticCameraList()) { QQmlEngine::setObjectOwnership(this, QQmlEngine::CppOwnership); diff --git a/src/MissionManager/CameraCalc.h b/src/MissionManager/CameraCalc.h index 7713b7b4f..22cf4d797 100644 --- a/src/MissionManager/CameraCalc.h +++ b/src/MissionManager/CameraCalc.h @@ -12,14 +12,14 @@ #include "CameraSpec.h" #include "SettingsFact.h" -class Vehicle; +class PlanMasterController; class CameraCalc : public CameraSpec { Q_OBJECT public: - CameraCalc(Vehicle* vehicle, const QString& settingsGroup, QObject* parent = nullptr); + CameraCalc(PlanMasterController* masterController, const QString& settingsGroup, QObject* parent = nullptr); Q_PROPERTY(QString customCameraName READ customCameraName CONSTANT) ///< Camera name for custom camera setting Q_PROPERTY(QString manualCameraName READ manualCameraName CONSTANT) ///< Camera name for manual camera setting @@ -97,7 +97,6 @@ private slots: void _cameraNameChanged (void); private: - Vehicle* _vehicle; bool _dirty; bool _disableRecalc; bool _distanceToSurfaceRelative; diff --git a/src/MissionManager/CameraCalcTest.cc b/src/MissionManager/CameraCalcTest.cc index 35e3c9073..6ceccbbe3 100644 --- a/src/MissionManager/CameraCalcTest.cc +++ b/src/MissionManager/CameraCalcTest.cc @@ -9,9 +9,9 @@ #include "CameraCalcTest.h" #include "QGCApplication.h" +#include "PlanMasterController.h" CameraCalcTest::CameraCalcTest(void) - : _offlineVehicle(nullptr) { } @@ -20,8 +20,9 @@ void CameraCalcTest::init(void) { UnitTest::init(); - _offlineVehicle = new Vehicle(MAV_AUTOPILOT_PX4, MAV_TYPE_QUADROTOR, qgcApp()->toolbox()->firmwarePluginManager(), this); - _cameraCalc = new CameraCalc(_offlineVehicle, "CameraCalcUnitTest" /* settingsGroup */, this); + _masterController = new PlanMasterController(this); + _controllerVehicle = _masterController->controllerVehicle(); + _cameraCalc = new CameraCalc(_masterController, "CameraCalcUnitTest" /* settingsGroup */, this); _cameraCalc->cameraName()->setRawValue(_cameraCalc->customCameraName()); _cameraCalc->setDirty(false); @@ -37,7 +38,6 @@ void CameraCalcTest::init(void) void CameraCalcTest::cleanup(void) { delete _cameraCalc; - delete _offlineVehicle; delete _multiSpy; } diff --git a/src/MissionManager/CameraCalcTest.h b/src/MissionManager/CameraCalcTest.h index 82af0b399..5166f3957 100644 --- a/src/MissionManager/CameraCalcTest.h +++ b/src/MissionManager/CameraCalcTest.h @@ -12,6 +12,7 @@ #include "UnitTest.h" #include "MultiSignalSpy.h" #include "CameraCalc.h" +#include "PlanMasterController.h" #include @@ -50,7 +51,8 @@ private: static const size_t _cSignals = maxSignalIndex; const char* _rgSignals[_cSignals]; - Vehicle* _offlineVehicle; - MultiSignalSpy* _multiSpy; - CameraCalc* _cameraCalc; + PlanMasterController* _masterController = nullptr; + Vehicle* _controllerVehicle = nullptr; + MultiSignalSpy* _multiSpy = nullptr; + CameraCalc* _cameraCalc = nullptr; }; diff --git a/src/MissionManager/CameraSection.cc b/src/MissionManager/CameraSection.cc index a5b2c1434..a8046eefe 100644 --- a/src/MissionManager/CameraSection.cc +++ b/src/MissionManager/CameraSection.cc @@ -10,6 +10,7 @@ #include "CameraSection.h" #include "SimpleMissionItem.h" #include "FirmwarePlugin.h" +#include "PlanMasterController.h" QGC_LOGGING_CATEGORY(CameraSectionLog, "CameraSectionLog") @@ -22,19 +23,19 @@ const char* CameraSection::_cameraModeName = "CameraMode"; QMap CameraSection::_metaDataMap; -CameraSection::CameraSection(Vehicle* vehicle, QObject* parent) - : Section(vehicle, parent) - , _available(false) - , _settingsSpecified(false) - , _specifyGimbal(false) - , _specifyCameraMode(false) +CameraSection::CameraSection(PlanMasterController* masterController, QObject* parent) + : Section (masterController, parent) + , _available (false) + , _settingsSpecified (false) + , _specifyGimbal (false) + , _specifyCameraMode (false) , _gimbalYawFact (0, _gimbalYawName, FactMetaData::valueTypeDouble) , _gimbalPitchFact (0, _gimbalPitchName, FactMetaData::valueTypeDouble) , _cameraActionFact (0, _cameraActionName, FactMetaData::valueTypeDouble) , _cameraPhotoIntervalDistanceFact (0, _cameraPhotoIntervalDistanceName, FactMetaData::valueTypeDouble) , _cameraPhotoIntervalTimeFact (0, _cameraPhotoIntervalTimeName, FactMetaData::valueTypeUint32) , _cameraModeFact (0, _cameraModeName, FactMetaData::valueTypeUint32) - , _dirty(false) + , _dirty (false) { if (_metaDataMap.isEmpty()) { _metaDataMap = FactMetaData::createMapFromJsonFile(QStringLiteral(":/json/CameraSection.FactMetaData.json"), Q_NULLPTR /* metaDataParent */); @@ -579,7 +580,7 @@ void CameraSection::_cameraActionChanged(void) bool CameraSection::cameraModeSupported(void) const { - return _specifyCameraMode || _vehicle->firmwarePlugin()->supportedMissionCommands().contains(MAV_CMD_SET_CAMERA_MODE); + return _specifyCameraMode || _masterController->controllerVehicle()->firmwarePlugin()->supportedMissionCommands().contains(MAV_CMD_SET_CAMERA_MODE); } void CameraSection::_dirtyIfSpecified(void) diff --git a/src/MissionManager/CameraSection.h b/src/MissionManager/CameraSection.h index 5a94f171f..0d9375c79 100644 --- a/src/MissionManager/CameraSection.h +++ b/src/MissionManager/CameraSection.h @@ -16,12 +16,14 @@ #define VIDEO_CAPTURE_STATUS_INTERVAL 0.2 //-- Send capture status every 5 seconds +class PlanMasterController; + class CameraSection : public Section { Q_OBJECT public: - CameraSection(Vehicle* vehicle, QObject* parent = nullptr); + CameraSection(PlanMasterController* masterController, QObject* parent = nullptr); // These enum values must match the json meta data diff --git a/src/MissionManager/CameraSectionTest.cc b/src/MissionManager/CameraSectionTest.cc index a31bfed28..6f9497a90 100644 --- a/src/MissionManager/CameraSectionTest.cc +++ b/src/MissionManager/CameraSectionTest.cc @@ -42,15 +42,15 @@ void CameraSectionTest::init(void) SectionTest::_createSpy(_cameraSection, &_spySection); QVERIFY(_spySection); - _validGimbalItem = new SimpleMissionItem(_offlineVehicle, + _validGimbalItem = new SimpleMissionItem(_masterController, false, // flyView MissionItem(0, MAV_CMD_DO_MOUNT_CONTROL, MAV_FRAME_MISSION, 10.1234, 0, 20.1234, 0, 0, 0, MAV_MOUNT_MODE_MAVLINK_TARGETING, true, false), this); - _validTimeItem = new SimpleMissionItem(_offlineVehicle, + _validTimeItem = new SimpleMissionItem(_masterController, false, // flyView MissionItem(0, MAV_CMD_IMAGE_START_CAPTURE, MAV_FRAME_MISSION, 0, 48, 0, NAN, NAN, NAN, NAN, true, false), this); - _validDistanceItem = new SimpleMissionItem(_offlineVehicle, + _validDistanceItem = new SimpleMissionItem(_masterController, false, // flyView MissionItem(0, MAV_CMD_DO_SET_CAM_TRIGG_DIST, @@ -61,7 +61,7 @@ void CameraSectionTest::init(void) 0, 0, 0, 0, true, false), this); - _validStartVideoItem = new SimpleMissionItem(_offlineVehicle, + _validStartVideoItem = new SimpleMissionItem(_masterController, false, // flyView MissionItem(0, // sequence number MAV_CMD_VIDEO_START_CAPTURE, @@ -72,7 +72,7 @@ void CameraSectionTest::init(void) true, // autocontinue false), // isCurrentItem this); - _validCameraPhotoModeItem = new SimpleMissionItem(_offlineVehicle, + _validCameraPhotoModeItem = new SimpleMissionItem(_masterController, false, // flyView MissionItem(0, // sequence number MAV_CMD_SET_CAMERA_MODE, @@ -83,7 +83,7 @@ void CameraSectionTest::init(void) true, // autocontinue false), // isCurrentItem this); - _validCameraVideoModeItem = new SimpleMissionItem(_offlineVehicle, + _validCameraVideoModeItem = new SimpleMissionItem(_masterController, false, // flyView MissionItem(0, // sequence number MAV_CMD_SET_CAMERA_MODE, @@ -94,7 +94,7 @@ void CameraSectionTest::init(void) true, // autocontinue false), // isCurrentItem this); - _validCameraSurveyPhotoModeItem = new SimpleMissionItem(_offlineVehicle, + _validCameraSurveyPhotoModeItem = new SimpleMissionItem(_masterController, false, // flyView MissionItem(0, // sequence number MAV_CMD_SET_CAMERA_MODE, @@ -105,7 +105,7 @@ void CameraSectionTest::init(void) true, // autocontinue false), // isCurrentItem this); - _validTakePhotoItem = new SimpleMissionItem(_offlineVehicle, + _validTakePhotoItem = new SimpleMissionItem(_masterController, false, // flyView MissionItem(0, MAV_CMD_IMAGE_START_CAPTURE, @@ -119,9 +119,9 @@ void CameraSectionTest::init(void) false), // isCurrentItem this); - _validStopVideoItem = createValidStopVideoItem(_offlineVehicle, this); - _validStopDistanceItem = createValidStopDistanceItem(_offlineVehicle, this); - _validStopTimeItem = createValidStopTimeItem(_offlineVehicle, this); + _validStopVideoItem = createValidStopVideoItem(_masterController, this); + _validStopDistanceItem = createValidStopDistanceItem(_masterController, this); + _validStopTimeItem = createValidStopTimeItem(_masterController, this); } void CameraSectionTest::cleanup(void) @@ -352,7 +352,7 @@ void CameraSectionTest::_checkAvailable(void) 70.1234567, true, // autoContinue false); // isCurrentItem - SimpleMissionItem* item = new SimpleMissionItem(_offlineVehicle, false /* flyView */, missionItem, this); + SimpleMissionItem* item = new SimpleMissionItem(_masterController, false /* flyView */, missionItem, this); QVERIFY(item->cameraSection()); QCOMPARE(item->cameraSection()->available(), false); } @@ -592,7 +592,7 @@ void CameraSectionTest::_testScanForGimbalSection(void) // Check for a scan success - SimpleMissionItem* newValidGimbalItem = new SimpleMissionItem(_offlineVehicle, false /* flyView */, this); + SimpleMissionItem* newValidGimbalItem = new SimpleMissionItem(_masterController, false /* flyView */, this); newValidGimbalItem->missionItem() = _validGimbalItem->missionItem(); visualItems.append(newValidGimbalItem); scanIndex = 0; @@ -619,7 +619,7 @@ void CameraSectionTest::_testScanForGimbalSection(void) // Gimbal command but incorrect settings - SimpleMissionItem invalidSimpleItem(_offlineVehicle, false /* flyView */, _validGimbalItem->missionItem(), nullptr); + SimpleMissionItem invalidSimpleItem(_masterController, false /* flyView */, _validGimbalItem->missionItem(), nullptr); invalidSimpleItem.missionItem().setParam2(10); // roll is not supported visualItems.append(&invalidSimpleItem); QCOMPARE(_cameraSection->scanForSection(&visualItems, scanIndex), false); @@ -676,7 +676,7 @@ void CameraSectionTest::_testScanForCameraModeSection(void) // Check for a scan success - SimpleMissionItem* newValidCameraModeItem = new SimpleMissionItem(_offlineVehicle, false /* flyView */, this); + SimpleMissionItem* newValidCameraModeItem = new SimpleMissionItem(_masterController, false /* flyView */, this); newValidCameraModeItem->missionItem() = _validCameraPhotoModeItem->missionItem(); visualItems.append(newValidCameraModeItem); scanIndex = 0; @@ -709,7 +709,7 @@ void CameraSectionTest::_testScanForCameraModeSection(void) */ // Mode command but incorrect settings - SimpleMissionItem invalidSimpleItem(_offlineVehicle, false /* flyView */, _validCameraPhotoModeItem->missionItem(), nullptr); + SimpleMissionItem invalidSimpleItem(_masterController, false /* flyView */, _validCameraPhotoModeItem->missionItem(), nullptr); invalidSimpleItem.missionItem().setParam3(1); // Param3 should be NaN visualItems.append(&invalidSimpleItem); QCOMPARE(_cameraSection->scanForSection(&visualItems, scanIndex), false); @@ -735,7 +735,7 @@ void CameraSectionTest::_testScanForPhotoIntervalTimeSection(void) Mission Param #3 Number of images to capture total - 0 for unlimited capture */ - SimpleMissionItem* newValidTimeItem = new SimpleMissionItem(_offlineVehicle, false /* flyView */, this); + SimpleMissionItem* newValidTimeItem = new SimpleMissionItem(_masterController, false /* flyView */, this); newValidTimeItem->missionItem() = _validTimeItem->missionItem(); visualItems.append(newValidTimeItem); QCOMPARE(_cameraSection->scanForSection(&visualItems, scanIndex), true); @@ -748,7 +748,7 @@ void CameraSectionTest::_testScanForPhotoIntervalTimeSection(void) // Image start command but incorrect settings - SimpleMissionItem invalidSimpleItem(_offlineVehicle, false /* flyView */, _validTimeItem->missionItem(), nullptr); + SimpleMissionItem invalidSimpleItem(_masterController, false /* flyView */, _validTimeItem->missionItem(), nullptr); invalidSimpleItem.missionItem().setParam3(10); // must be 0 for unlimited visualItems.append(&invalidSimpleItem); QCOMPARE(_cameraSection->scanForSection(&visualItems, scanIndex), false); @@ -776,7 +776,7 @@ void CameraSectionTest::_testScanForPhotoIntervalDistanceSection(void) Mission Param #7 Empty */ - SimpleMissionItem* newValidDistanceItem = new SimpleMissionItem(_offlineVehicle, false /* flyView */, this); + SimpleMissionItem* newValidDistanceItem = new SimpleMissionItem(_masterController, false /* flyView */, this); newValidDistanceItem->missionItem() = _validDistanceItem->missionItem(); visualItems.append(newValidDistanceItem); QCOMPARE(_cameraSection->scanForSection(&visualItems, scanIndex), true); @@ -789,7 +789,7 @@ void CameraSectionTest::_testScanForPhotoIntervalDistanceSection(void) // Trigger distance command but incorrect settings - SimpleMissionItem invalidSimpleItem(_offlineVehicle, false /* flyView */, _validDistanceItem->missionItem(), nullptr); + SimpleMissionItem invalidSimpleItem(_masterController, false /* flyView */, _validDistanceItem->missionItem(), nullptr); invalidSimpleItem.missionItem().setParam1(-1); // must be >= 0 visualItems.append(&invalidSimpleItem); QCOMPARE(_cameraSection->scanForSection(&visualItems, scanIndex), false); @@ -862,7 +862,7 @@ void CameraSectionTest::_testScanForStartVideoSection(void) Mission Param #3 Reserved */ - SimpleMissionItem* newValidStartVideoItem = new SimpleMissionItem(_offlineVehicle, false /* flyView */, this); + SimpleMissionItem* newValidStartVideoItem = new SimpleMissionItem(_masterController, false /* flyView */, this); newValidStartVideoItem->missionItem() = _validStartVideoItem->missionItem(); visualItems.append(newValidStartVideoItem); QCOMPARE(_cameraSection->scanForSection(&visualItems, scanIndex), true); @@ -874,7 +874,7 @@ void CameraSectionTest::_testScanForStartVideoSection(void) // Start Video command but incorrect settings - SimpleMissionItem invalidSimpleItem(_offlineVehicle, false /* flyView */, _validStartVideoItem->missionItem(), nullptr); + SimpleMissionItem invalidSimpleItem(_masterController, false /* flyView */, _validStartVideoItem->missionItem(), nullptr); invalidSimpleItem.missionItem().setParam1(10); // Reserved (must be 0) visualItems.append(&invalidSimpleItem); QCOMPARE(_cameraSection->scanForSection(&visualItems, scanIndex), false); @@ -898,7 +898,7 @@ void CameraSectionTest::_testScanForStopVideoSection(void) Mission Param #1 Reserved (Set to 0) */ - SimpleMissionItem* newValidStopVideoItem = new SimpleMissionItem(_offlineVehicle, false /* flyView */, this); + SimpleMissionItem* newValidStopVideoItem = new SimpleMissionItem(_masterController, false /* flyView */, this); newValidStopVideoItem->missionItem() = _validStopVideoItem->missionItem(); visualItems.append(newValidStopVideoItem); QCOMPARE(_cameraSection->scanForSection(&visualItems, scanIndex), true); @@ -910,7 +910,7 @@ void CameraSectionTest::_testScanForStopVideoSection(void) // Trigger distance command but incorrect settings - SimpleMissionItem invalidSimpleItem(_offlineVehicle, false /* flyView */, _validStopVideoItem->missionItem(), nullptr); + SimpleMissionItem invalidSimpleItem(_masterController, false /* flyView */, _validStopVideoItem->missionItem(), nullptr); invalidSimpleItem.missionItem().setParam1(10); // must be 0 visualItems.append(&invalidSimpleItem); QCOMPARE(_cameraSection->scanForSection(&visualItems, scanIndex), false); @@ -936,8 +936,8 @@ void CameraSectionTest::_testScanForStopPhotoSection(void) _commonScanTest(_cameraSection); - SimpleMissionItem* newValidStopDistanceItem = new SimpleMissionItem(_offlineVehicle, false /* flyView */, this); - SimpleMissionItem* newValidStopTimeItem = new SimpleMissionItem(_offlineVehicle, false /* flyView */, this); + SimpleMissionItem* newValidStopDistanceItem = new SimpleMissionItem(_masterController, false /* flyView */, this); + SimpleMissionItem* newValidStopTimeItem = new SimpleMissionItem(_masterController, false /* flyView */, this); newValidStopDistanceItem->missionItem() = _validStopDistanceItem->missionItem(); newValidStopTimeItem->missionItem() = _validStopTimeItem->missionItem(); visualItems.append(newValidStopDistanceItem); @@ -950,8 +950,8 @@ void CameraSectionTest::_testScanForStopPhotoSection(void) // Out of order commands - SimpleMissionItem validStopDistanceItem(_offlineVehicle, false /* flyView */, nullptr); - SimpleMissionItem validStopTimeItem(_offlineVehicle, false /* flyView */, nullptr); + SimpleMissionItem validStopDistanceItem(_masterController, false /* flyView */, nullptr); + SimpleMissionItem validStopTimeItem(_masterController, false /* flyView */, nullptr); validStopDistanceItem.missionItem() = _validStopDistanceItem->missionItem(); validStopTimeItem.missionItem() = _validStopTimeItem->missionItem(); visualItems.append(&validStopTimeItem); @@ -979,7 +979,7 @@ void CameraSectionTest::_testScanForTakePhotoSection(void) Mission Param #4 0 Unused sequence id */ - SimpleMissionItem* newValidTakePhotoItem = new SimpleMissionItem(_offlineVehicle, false /* flyView */, this); + SimpleMissionItem* newValidTakePhotoItem = new SimpleMissionItem(_masterController, false /* flyView */, this); newValidTakePhotoItem->missionItem() = _validTakePhotoItem->missionItem(); visualItems.append(newValidTakePhotoItem); QCOMPARE(_cameraSection->scanForSection(&visualItems, scanIndex), true); @@ -991,7 +991,7 @@ void CameraSectionTest::_testScanForTakePhotoSection(void) // Take Photo command but incorrect settings - SimpleMissionItem invalidSimpleItem(_offlineVehicle, false /* flyView */, _validTimeItem->missionItem(), nullptr); + SimpleMissionItem invalidSimpleItem(_masterController, false /* flyView */, _validTimeItem->missionItem(), nullptr); invalidSimpleItem.missionItem().setParam3(10); // must be 1 for single photo visualItems.append(&invalidSimpleItem); QCOMPARE(_cameraSection->scanForSection(&visualItems, scanIndex), false); @@ -1051,13 +1051,13 @@ void CameraSectionTest::_testScanForMultipleItems(void) // Camera action followed by gimbal/mode for (SimpleMissionItem* actionItem: rgActionItems) { for (SimpleMissionItem* cameraItem: rgCameraItems) { - SimpleMissionItem* item1 = new SimpleMissionItem(_offlineVehicle, false /* flyView */, this); + SimpleMissionItem* item1 = new SimpleMissionItem(_masterController, false /* flyView */, this); item1->missionItem() = actionItem->missionItem(); - SimpleMissionItem* item2 = new SimpleMissionItem(_offlineVehicle, false /* flyView */, this); + SimpleMissionItem* item2 = new SimpleMissionItem(_masterController, false /* flyView */, this); item2->missionItem() = cameraItem->missionItem(); visualItems.append(item1); visualItems.append(item2); - qDebug() << commandTree->getUIInfo(_offlineVehicle, (MAV_CMD)item1->command())->rawName() << commandTree->getUIInfo(_offlineVehicle, (MAV_CMD)item2->command())->rawName();; + qDebug() << commandTree->getUIInfo(_controllerVehicle, (MAV_CMD)item1->command())->rawName() << commandTree->getUIInfo(_controllerVehicle, (MAV_CMD)item2->command())->rawName();; scanIndex = 0; QCOMPARE(_cameraSection->scanForSection(&visualItems, scanIndex), true); @@ -1072,13 +1072,13 @@ void CameraSectionTest::_testScanForMultipleItems(void) // Gimbal/Mode followed by camera action for (SimpleMissionItem* actionItem: rgCameraItems) { for (SimpleMissionItem* cameraItem: rgActionItems) { - SimpleMissionItem* item1 = new SimpleMissionItem(_offlineVehicle, false /* flyView */, this); + SimpleMissionItem* item1 = new SimpleMissionItem(_masterController, false /* flyView */, this); item1->missionItem() = actionItem->missionItem(); - SimpleMissionItem* item2 = new SimpleMissionItem(_offlineVehicle, false /* flyView */, this); + SimpleMissionItem* item2 = new SimpleMissionItem(_masterController, false /* flyView */, this); item2->missionItem() = cameraItem->missionItem(); visualItems.append(item1); visualItems.append(item2); - qDebug() << commandTree->getUIInfo(_offlineVehicle, (MAV_CMD)item1->command())->rawName() << commandTree->getUIInfo(_offlineVehicle, (MAV_CMD)item2->command())->rawName();; + qDebug() << commandTree->getUIInfo(_controllerVehicle, (MAV_CMD)item1->command())->rawName() << commandTree->getUIInfo(_controllerVehicle, (MAV_CMD)item2->command())->rawName();; scanIndex = 0; QCOMPARE(_cameraSection->scanForSection(&visualItems, scanIndex), true); @@ -1111,26 +1111,26 @@ void CameraSectionTest::_testSpecifiedGimbalValuesChanged(void) QVERIFY(_spyCamera->checkSignalByMask(specifiedGimbalPitchChangedMask)); } -SimpleMissionItem* CameraSectionTest::createValidStopVideoItem(Vehicle* vehicle, QObject* parent) +SimpleMissionItem* CameraSectionTest::createValidStopVideoItem(PlanMasterController* masterController, QObject* parent) { - return new SimpleMissionItem(vehicle, + return new SimpleMissionItem(masterController, false, // flyView MissionItem(0, MAV_CMD_VIDEO_STOP_CAPTURE, MAV_FRAME_MISSION, 0, qQNaN(), qQNaN(), qQNaN(), qQNaN(), qQNaN(), qQNaN(), true, false), parent); } -SimpleMissionItem* CameraSectionTest::createValidStopDistanceItem(Vehicle* vehicle, QObject* parent) +SimpleMissionItem* CameraSectionTest::createValidStopDistanceItem(PlanMasterController* masterController, QObject* parent) { - return new SimpleMissionItem(vehicle, + return new SimpleMissionItem(masterController, false, // flyView MissionItem(0, MAV_CMD_DO_SET_CAM_TRIGG_DIST, MAV_FRAME_MISSION, 0, 0, 0, 0, 0, 0, 0, true, false), parent); } -SimpleMissionItem* CameraSectionTest::createValidStopTimeItem(Vehicle* vehicle, QObject* parent) +SimpleMissionItem* CameraSectionTest::createValidStopTimeItem(PlanMasterController* masterController, QObject* parent) { - return new SimpleMissionItem(vehicle, + return new SimpleMissionItem(masterController, false, // flyView MissionItem(1, MAV_CMD_IMAGE_STOP_CAPTURE, MAV_FRAME_MISSION, 0, qQNaN(), qQNaN(), qQNaN(), qQNaN(), qQNaN(), qQNaN(), true, false), parent); diff --git a/src/MissionManager/CameraSectionTest.h b/src/MissionManager/CameraSectionTest.h index 473998df5..5b57d907d 100644 --- a/src/MissionManager/CameraSectionTest.h +++ b/src/MissionManager/CameraSectionTest.h @@ -11,6 +11,7 @@ #include "SectionTest.h" #include "CameraSection.h" +#include "PlanMasterController.h" /// Unit test for CameraSection class CameraSectionTest : public SectionTest @@ -23,9 +24,9 @@ public: void init(void) override; void cleanup(void) override; - static SimpleMissionItem* createValidStopVideoItem (Vehicle* vehicle, QObject* parent); - static SimpleMissionItem* createValidStopDistanceItem(Vehicle* vehicle, QObject* parent); - static SimpleMissionItem* createValidStopTimeItem (Vehicle* vehicle, QObject* parent); + static SimpleMissionItem* createValidStopVideoItem (PlanMasterController* masterController, QObject* parent); + static SimpleMissionItem* createValidStopDistanceItem(PlanMasterController* masterController, QObject* parent); + static SimpleMissionItem* createValidStopTimeItem (PlanMasterController* masterController, QObject* parent); private slots: void _testDirty (void); diff --git a/src/MissionManager/ComplexMissionItem.cc b/src/MissionManager/ComplexMissionItem.cc index c525a28b1..c7ba840f8 100644 --- a/src/MissionManager/ComplexMissionItem.cc +++ b/src/MissionManager/ComplexMissionItem.cc @@ -11,6 +11,7 @@ #include "QGCApplication.h" #include "QGCCorePlugin.h" #include "QGCOptions.h" +#include "PlanMasterController.h" #include @@ -18,8 +19,8 @@ const char* ComplexMissionItem::jsonComplexItemTypeKey = "complexItemType"; const char* ComplexMissionItem::_presetSettingsKey = "_presets"; -ComplexMissionItem::ComplexMissionItem(Vehicle* vehicle, bool flyView, QObject* parent) - : VisualMissionItem (vehicle, flyView, parent) +ComplexMissionItem::ComplexMissionItem(PlanMasterController* masterController, bool flyView, QObject* parent) + : VisualMissionItem (masterController, flyView, parent) { } diff --git a/src/MissionManager/ComplexMissionItem.h b/src/MissionManager/ComplexMissionItem.h index 53b0ba67c..81def4071 100644 --- a/src/MissionManager/ComplexMissionItem.h +++ b/src/MissionManager/ComplexMissionItem.h @@ -7,20 +7,21 @@ * ****************************************************************************/ -#ifndef ComplexMissionItem_H -#define ComplexMissionItem_H +#pragma once #include "VisualMissionItem.h" #include "QGCGeo.h" #include +class PlanMasterController; + class ComplexMissionItem : public VisualMissionItem { Q_OBJECT public: - ComplexMissionItem(Vehicle* vehicle, bool flyView, QObject* parent); + ComplexMissionItem(PlanMasterController* masterController, bool flyView, QObject* parent); const ComplexMissionItem& operator=(const ComplexMissionItem& other); @@ -88,5 +89,3 @@ protected: static const char* _presetSettingsKey; }; - -#endif diff --git a/src/MissionManager/CorridorScanComplexItem.cc b/src/MissionManager/CorridorScanComplexItem.cc index c61abc178..4ad064585 100644 --- a/src/MissionManager/CorridorScanComplexItem.cc +++ b/src/MissionManager/CorridorScanComplexItem.cc @@ -16,6 +16,7 @@ #include "SettingsManager.h" #include "AppSettings.h" #include "QGCQGeoCoordinate.h" +#include "PlanMasterController.h" #include @@ -27,8 +28,8 @@ const char* CorridorScanComplexItem::_jsonEntryPointKey = "EntryPoint"; const char* CorridorScanComplexItem::jsonComplexItemTypeValue = "CorridorScan"; -CorridorScanComplexItem::CorridorScanComplexItem(Vehicle* vehicle, bool flyView, const QString& kmlFile, QObject* parent) - : TransectStyleComplexItem (vehicle, flyView, settingsGroup, parent) +CorridorScanComplexItem::CorridorScanComplexItem(PlanMasterController* masterController, bool flyView, const QString& kmlFile, QObject* parent) + : TransectStyleComplexItem (masterController, flyView, settingsGroup, parent) , _entryPoint (0) , _metaDataMap (FactMetaData::createMapFromJsonFile(QStringLiteral(":/json/CorridorScan.SettingsGroup.json"), this)) , _corridorWidthFact (settingsGroup, _metaDataMap[corridorWidthName]) diff --git a/src/MissionManager/CorridorScanComplexItem.h b/src/MissionManager/CorridorScanComplexItem.h index 6dc18c924..ab3704cd2 100644 --- a/src/MissionManager/CorridorScanComplexItem.h +++ b/src/MissionManager/CorridorScanComplexItem.h @@ -23,10 +23,9 @@ class CorridorScanComplexItem : public TransectStyleComplexItem Q_OBJECT public: - /// @param vehicle Vehicle which this is being contructed for /// @param flyView true: Created for use in the Fly View, false: Created for use in the Plan View /// @param kmlFile Polyline comes from this file, empty for default polyline - CorridorScanComplexItem(Vehicle* vehicle, bool flyView, const QString& kmlFile, QObject* parent); + CorridorScanComplexItem(PlanMasterController* masterController, bool flyView, const QString& kmlFile, QObject* parent); Q_PROPERTY(QGCMapPolyline* corridorPolyline READ corridorPolyline CONSTANT) Q_PROPERTY(Fact* corridorWidth READ corridorWidth CONSTANT) diff --git a/src/MissionManager/CorridorScanComplexItemTest.cc b/src/MissionManager/CorridorScanComplexItemTest.cc index e7c0797f9..691bf5c7b 100644 --- a/src/MissionManager/CorridorScanComplexItemTest.cc +++ b/src/MissionManager/CorridorScanComplexItemTest.cc @@ -11,7 +11,6 @@ #include "QGCApplication.h" CorridorScanComplexItemTest::CorridorScanComplexItemTest(void) - : _offlineVehicle(nullptr) { _linePoints << QGeoCoordinate(47.633550640000003, -122.08982199) << QGeoCoordinate(47.634129020000003, -122.08887249) @@ -22,8 +21,9 @@ void CorridorScanComplexItemTest::init(void) { UnitTest::init(); - _offlineVehicle = new Vehicle(MAV_AUTOPILOT_PX4, MAV_TYPE_QUADROTOR, qgcApp()->toolbox()->firmwarePluginManager(), this); - _corridorItem = new CorridorScanComplexItem(_offlineVehicle, false /* flyView */, QString() /* kmlFile */, this /* parent */); + _masterController = new PlanMasterController(this); + _controllerVehicle = _masterController->controllerVehicle(); + _corridorItem = new CorridorScanComplexItem(_masterController, false /* flyView */, QString() /* kmlFile */, this /* parent */); // vehicleSpeed need for terrain calcs MissionController::MissionFlightStatus_t missionFlightStatus; @@ -42,7 +42,6 @@ void CorridorScanComplexItemTest::init(void) void CorridorScanComplexItemTest::cleanup(void) { delete _corridorItem; - delete _offlineVehicle; } void CorridorScanComplexItemTest::_testDirty(void) diff --git a/src/MissionManager/CorridorScanComplexItemTest.h b/src/MissionManager/CorridorScanComplexItemTest.h index 6fdad7099..4e1faac33 100644 --- a/src/MissionManager/CorridorScanComplexItemTest.h +++ b/src/MissionManager/CorridorScanComplexItemTest.h @@ -13,6 +13,7 @@ #include "TCPLink.h" #include "MultiSignalSpy.h" #include "CorridorScanComplexItem.h" +#include "PlanMasterController.h" #include @@ -50,8 +51,9 @@ private: static const size_t _cCorridorPolygonSignals = maxCorridorPolygonSignalIndex; const char* _rgCorridorPolygonSignals[_cCorridorPolygonSignals]; - Vehicle* _offlineVehicle; - MultiSignalSpy* _multiSpyCorridorPolygon; - CorridorScanComplexItem* _corridorItem; + PlanMasterController* _masterController = nullptr; + Vehicle* _controllerVehicle = nullptr; + MultiSignalSpy* _multiSpyCorridorPolygon = nullptr; + CorridorScanComplexItem* _corridorItem = nullptr; QList _linePoints; }; diff --git a/src/MissionManager/FWLandingPatternTest.cc b/src/MissionManager/FWLandingPatternTest.cc index 035329b05..4ff6f3aa6 100644 --- a/src/MissionManager/FWLandingPatternTest.cc +++ b/src/MissionManager/FWLandingPatternTest.cc @@ -14,24 +14,17 @@ #include "CameraSectionTest.h" FWLandingPatternTest::FWLandingPatternTest(void) - : _offlineVehicle (Q_NULLPTR) - , _fwItem (Q_NULLPTR) - , _multiSpy (Q_NULLPTR) - , _validStopVideoItem (Q_NULLPTR) - , _validStopDistanceItem (Q_NULLPTR) - , _validStopTimeItem (Q_NULLPTR) { } void FWLandingPatternTest::init(void) { - UnitTest::init(); + VisualMissionItemTest::init(); rgSignals[dirtyChangedIndex] = SIGNAL(dirtyChanged(bool)); - _offlineVehicle = new Vehicle(MAV_AUTOPILOT_PX4, MAV_TYPE_QUADROTOR, qgcApp()->toolbox()->firmwarePluginManager(), this); - _fwItem = new FixedWingLandingComplexItem(_offlineVehicle, false /* flyView */, this); + _fwItem = new FixedWingLandingComplexItem(_masterController, false /* flyView */, this); _multiSpy = new MultiSignalSpy(); QCOMPARE(_multiSpy->init(_fwItem, rgSignals, cSignals), true); @@ -42,20 +35,19 @@ void FWLandingPatternTest::init(void) QVERIFY(!_fwItem->dirty()); _multiSpy->clearAllSignals(); - _validStopVideoItem = CameraSectionTest::createValidStopTimeItem(_offlineVehicle, this); - _validStopDistanceItem = CameraSectionTest::createValidStopTimeItem(_offlineVehicle, this); - _validStopTimeItem = CameraSectionTest::createValidStopTimeItem(_offlineVehicle, this); + _validStopVideoItem = CameraSectionTest::createValidStopTimeItem(_masterController, this); + _validStopDistanceItem = CameraSectionTest::createValidStopTimeItem(_masterController, this); + _validStopTimeItem = CameraSectionTest::createValidStopTimeItem(_masterController, this); } void FWLandingPatternTest::cleanup(void) { delete _fwItem; - delete _offlineVehicle; delete _multiSpy; delete _validStopVideoItem; delete _validStopDistanceItem; delete _validStopTimeItem; - UnitTest::cleanup(); + VisualMissionItemTest::cleanup(); } @@ -97,14 +89,14 @@ void FWLandingPatternTest::_testAppendSectionItems(void) QmlObjectListModel* simpleItems = new QmlObjectListModel(this); for (MissionItem* item: rgMissionItems) { - SimpleMissionItem* simpleItem = new SimpleMissionItem(_offlineVehicle, false /* flyView */, simpleItems); + SimpleMissionItem* simpleItem = new SimpleMissionItem(_masterController, false /* flyView */, simpleItems); simpleItem->missionItem() = *item; simpleItems->append(simpleItem); } // Scan the items back in to verify the same values come back // Note that the compares does the best it can with doubles going to floats and back causing inaccuracies beyond a fuzzy compare. - QVERIFY(FixedWingLandingComplexItem::scanForItem(simpleItems, false /* flyView */, _offlineVehicle)); + QVERIFY(FixedWingLandingComplexItem::scanForItem(simpleItems, false /* flyView */, _masterController)); QCOMPARE(simpleItems->count(), 1); _validateItem(simpleItems->value(0)); @@ -118,11 +110,11 @@ void FWLandingPatternTest::_testAppendSectionItems(void) _fwItem->appendMissionItems(rgMissionItems, this); simpleItems = new QmlObjectListModel(this); for (MissionItem* item: rgMissionItems) { - SimpleMissionItem* simpleItem = new SimpleMissionItem(_offlineVehicle, false /* flyView */, simpleItems); + SimpleMissionItem* simpleItem = new SimpleMissionItem(_masterController, false /* flyView */, simpleItems); simpleItem->missionItem() = *item; simpleItems->append(simpleItem); } - QVERIFY(FixedWingLandingComplexItem::scanForItem(simpleItems, false /* flyView */, _offlineVehicle)); + QVERIFY(FixedWingLandingComplexItem::scanForItem(simpleItems, false /* flyView */, _masterController)); QCOMPARE(simpleItems->count(), 1); _validateItem(simpleItems->value(0)); @@ -204,7 +196,7 @@ void FWLandingPatternTest::_testSaveLoad(void) _fwItem->save(items); QString errorString; - FixedWingLandingComplexItem* newItem = new FixedWingLandingComplexItem(_offlineVehicle, false /* flyView */, this /* parent */); + FixedWingLandingComplexItem* newItem = new FixedWingLandingComplexItem(_masterController, false /* flyView */, this /* parent */); bool success =newItem->load(items[0].toObject(), 10, errorString); if (!success) { qDebug() << errorString; diff --git a/src/MissionManager/FWLandingPatternTest.h b/src/MissionManager/FWLandingPatternTest.h index 8aaaff6df..9a5dda9d1 100644 --- a/src/MissionManager/FWLandingPatternTest.h +++ b/src/MissionManager/FWLandingPatternTest.h @@ -9,11 +9,12 @@ #pragma once -#include "UnitTest.h" +#include "VisualMissionItemTest.h" #include "FixedWingLandingComplexItem.h" #include "MultiSignalSpy.h" +#include "PlanMasterController.h" -class FWLandingPatternTest : public UnitTest +class FWLandingPatternTest : public VisualMissionItemTest { Q_OBJECT @@ -45,10 +46,9 @@ private: static const size_t cSignals = maxSignalIndex; const char* rgSignals[cSignals]; - Vehicle* _offlineVehicle; - FixedWingLandingComplexItem* _fwItem; - MultiSignalSpy* _multiSpy; - SimpleMissionItem* _validStopVideoItem; - SimpleMissionItem* _validStopDistanceItem; - SimpleMissionItem* _validStopTimeItem; + FixedWingLandingComplexItem* _fwItem = nullptr; + MultiSignalSpy* _multiSpy = nullptr; + SimpleMissionItem* _validStopVideoItem = nullptr; + SimpleMissionItem* _validStopDistanceItem = nullptr; + SimpleMissionItem* _validStopTimeItem = nullptr; }; diff --git a/src/MissionManager/FixedWingLandingComplexItem.cc b/src/MissionManager/FixedWingLandingComplexItem.cc index c4fae6d6f..d2f8256da 100644 --- a/src/MissionManager/FixedWingLandingComplexItem.cc +++ b/src/MissionManager/FixedWingLandingComplexItem.cc @@ -13,6 +13,7 @@ #include "QGCGeo.h" #include "QGroundControlQmlGlobal.h" #include "SimpleMissionItem.h" +#include "PlanMasterController.h" #include @@ -45,8 +46,8 @@ const char* FixedWingLandingComplexItem::_jsonFallRateKey = "fal const char* FixedWingLandingComplexItem::_jsonLandingAltitudeRelativeKey = "landAltitudeRelative"; const char* FixedWingLandingComplexItem::_jsonLoiterAltitudeRelativeKey = "loiterAltitudeRelative"; -FixedWingLandingComplexItem::FixedWingLandingComplexItem(Vehicle* vehicle, bool flyView, QObject* parent) - : ComplexMissionItem (vehicle, flyView, parent) +FixedWingLandingComplexItem::FixedWingLandingComplexItem(PlanMasterController* masterController, bool flyView, QObject* parent) + : ComplexMissionItem (masterController, flyView, parent) , _sequenceNumber (0) , _dirty (false) , _landingCoordSet (false) @@ -105,7 +106,7 @@ FixedWingLandingComplexItem::FixedWingLandingComplexItem(Vehicle* vehicle, bool connect(this, &FixedWingLandingComplexItem::landingCoordSetChanged, this, &FixedWingLandingComplexItem::readyForSaveStateChanged); connect(this, &FixedWingLandingComplexItem::wizardModeChanged, this, &FixedWingLandingComplexItem::readyForSaveStateChanged); - if (vehicle->apmFirmware()) { + if (_masterController->controllerVehicle()->apmFirmware()) { // ArduPilot does not support camera commands _stopTakingVideoFact.setRawValue(false); _stopTakingPhotosFact.setRawValue(false); @@ -364,7 +365,7 @@ void FixedWingLandingComplexItem::appendMissionItems(QList& items, items.append(item); } -bool FixedWingLandingComplexItem::scanForItem(QmlObjectListModel* visualItems, bool flyView, Vehicle* vehicle) +bool FixedWingLandingComplexItem::scanForItem(QmlObjectListModel* visualItems, bool flyView, PlanMasterController* masterController) { qCDebug(FixedWingLandingComplexItemLog) << "FixedWingLandingComplexItem::scanForItem count" << visualItems->count(); @@ -455,7 +456,7 @@ bool FixedWingLandingComplexItem::scanForItem(QmlObjectListModel* visualItems, b // Now stuff all the scanned information into the item - FixedWingLandingComplexItem* complexItem = new FixedWingLandingComplexItem(vehicle, flyView, visualItems); + FixedWingLandingComplexItem* complexItem = new FixedWingLandingComplexItem(masterController, flyView, visualItems); complexItem->_ignoreRecalcSignals = true; diff --git a/src/MissionManager/FixedWingLandingComplexItem.h b/src/MissionManager/FixedWingLandingComplexItem.h index b64c60fe4..6655d5303 100644 --- a/src/MissionManager/FixedWingLandingComplexItem.h +++ b/src/MissionManager/FixedWingLandingComplexItem.h @@ -18,13 +18,14 @@ Q_DECLARE_LOGGING_CATEGORY(FixedWingLandingComplexItemLog) class FWLandingPatternTest; +class PlanMasterController; class FixedWingLandingComplexItem : public ComplexMissionItem { Q_OBJECT public: - FixedWingLandingComplexItem(Vehicle* vehicle, bool flyView, QObject* parent); + FixedWingLandingComplexItem(PlanMasterController* masterController, bool flyView, QObject* parent); Q_PROPERTY(Fact* loiterAltitude READ loiterAltitude CONSTANT) Q_PROPERTY(Fact* loiterRadius READ loiterRadius CONSTANT) @@ -62,7 +63,7 @@ public: void setLoiterDragAngleOnly (bool loiterDragAngleOnly); /// Scans the loaded items for a landing pattern complex item - static bool scanForItem(QmlObjectListModel* visualItems, bool flyView, Vehicle* vehicle); + static bool scanForItem(QmlObjectListModel* visualItems, bool flyView, PlanMasterController* masterController); static MissionItem* createDoLandStartItem (int seqNum, QObject* parent); static MissionItem* createLoiterToAltItem (int seqNum, bool altRel, double loiterRaidus, double lat, double lon, double alt, QObject* parent); diff --git a/src/MissionManager/GeoFenceController.cc b/src/MissionManager/GeoFenceController.cc index b4b0e12d4..4e3042d19 100644 --- a/src/MissionManager/GeoFenceController.cc +++ b/src/MissionManager/GeoFenceController.cc @@ -42,13 +42,10 @@ const char* GeoFenceController::_px4ParamCircularFence = "GF_MAX_HOR_DIST"; GeoFenceController::GeoFenceController(PlanMasterController* masterController, QObject* parent) : PlanElementController (masterController, parent) - , _geoFenceManager (_managerVehicle->geoFenceManager()) - , _dirty (false) + , _managerVehicle (masterController->managerVehicle()) + , _geoFenceManager (masterController->managerVehicle()->geoFenceManager()) , _breachReturnAltitudeFact (0, _breachReturnAltitudeFactName, FactMetaData::valueTypeDouble) , _breachReturnDefaultAltitude (qgcApp()->toolbox()->settingsManager()->appSettings()->defaultMissionItemAltitude()->rawValue().toDouble()) - - , _itemsRequested (false) - , _px4ParamCircularFenceFact(nullptr) { if (_metaDataMap.isEmpty()) { _metaDataMap = FactMetaData::createMapFromJsonFile(QStringLiteral(":/json/BreachReturn.FactMetaData.json"), nullptr /* metaDataParent */); @@ -60,8 +57,6 @@ GeoFenceController::GeoFenceController(PlanMasterController* masterController, Q connect(&_polygons, &QmlObjectListModel::countChanged, this, &GeoFenceController::_updateContainsItems); connect(&_circles, &QmlObjectListModel::countChanged, this, &GeoFenceController::_updateContainsItems); - managerVehicleChanged(_managerVehicle); - connect(this, &GeoFenceController::breachReturnPointChanged, this, &GeoFenceController::_setDirty); connect(&_breachReturnAltitudeFact, &Fact::rawValueChanged, this, &GeoFenceController::_setDirty); connect(&_polygons, &QmlObjectListModel::dirtyChanged, this, &GeoFenceController::_setDirty); @@ -77,6 +72,9 @@ void GeoFenceController::start(bool flyView) { qCDebug(GeoFenceControllerLog) << "start flyView" << flyView; + _managerVehicleChanged(_masterController->managerVehicle()); + connect(_masterController, &PlanMasterController::managerVehicleChanged, this, &GeoFenceController::_managerVehicleChanged); + PlanElementController::start(flyView); _init(); } @@ -95,7 +93,7 @@ void GeoFenceController::setBreachReturnPoint(const QGeoCoordinate& breachReturn } } -void GeoFenceController::managerVehicleChanged(Vehicle* managerVehicle) +void GeoFenceController::_managerVehicleChanged(Vehicle* managerVehicle) { if (_managerVehicle) { _geoFenceManager->disconnect(this); diff --git a/src/MissionManager/GeoFenceController.h b/src/MissionManager/GeoFenceController.h index c0ab56eaf..83bb2e253 100644 --- a/src/MissionManager/GeoFenceController.h +++ b/src/MissionManager/GeoFenceController.h @@ -75,7 +75,6 @@ public: bool dirty (void) const final; void setDirty (bool dirty) final; bool containsItems (void) const final; - void managerVehicleChanged (Vehicle* managerVehicle) final; bool showPlanFromManagerVehicle (void) final; QmlObjectListModel* polygons (void) { return &_polygons; } @@ -101,19 +100,21 @@ private slots: void _managerSendComplete (bool error); void _managerRemoveAllComplete (bool error); void _parametersReady (void); + void _managerVehicleChanged (Vehicle* managerVehicle); private: void _init(void); - GeoFenceManager* _geoFenceManager; - bool _dirty; + Vehicle* _managerVehicle = nullptr; + GeoFenceManager* _geoFenceManager = nullptr; + bool _dirty = false; QmlObjectListModel _polygons; QmlObjectListModel _circles; QGeoCoordinate _breachReturnPoint; Fact _breachReturnAltitudeFact; - double _breachReturnDefaultAltitude; - bool _itemsRequested; - Fact* _px4ParamCircularFenceFact; + double _breachReturnDefaultAltitude = qQNaN(); + bool _itemsRequested = false; + Fact* _px4ParamCircularFenceFact = nullptr; static QMap _metaDataMap; diff --git a/src/MissionManager/MissionController.cc b/src/MissionManager/MissionController.cc index bd8d14091..f362e9e8e 100644 --- a/src/MissionManager/MissionController.cc +++ b/src/MissionManager/MissionController.cc @@ -59,22 +59,13 @@ const QString MissionController::patternCorridorScanName (QT_TRANSLATE_NOOP(" MissionController::MissionController(PlanMasterController* masterController, QObject *parent) : PlanElementController (masterController, parent) - , _missionManager (_managerVehicle->missionManager()) - , _missionItemCount (0) - , _visualItems (nullptr) - , _settingsItem (nullptr) - , _firstItemsFromVehicle (false) - , _itemsRequested (false) - , _inRecalcSequence (false) + , _controllerVehicle (masterController->controllerVehicle()) + , _managerVehicle (masterController->managerVehicle()) + , _missionManager (masterController->managerVehicle()->missionManager()) , _appSettings (qgcApp()->toolbox()->settingsManager()->appSettings()) - , _progressPct (0) - , _currentPlanViewSeqNum (-1) - , _currentPlanViewVIIndex (-1) - , _currentPlanViewItem (nullptr) - , _splitSegment (nullptr) { _resetMissionFlightStatus(); - managerVehicleChanged(_managerVehicle); + _updateTimer.setSingleShot(true); connect(&_updateTimer, &QTimer::timeout, this, &MissionController::_updateTimeout); } @@ -133,6 +124,9 @@ void MissionController::start(bool flyView) { qCDebug(MissionControllerLog) << "start flyView" << flyView; + _managerVehicleChanged(_managerVehicle); + connect(_masterController, &PlanMasterController::managerVehicleChanged, this, &MissionController::_managerVehicleChanged); + PlanElementController::start(flyView); _init(); } @@ -189,10 +183,10 @@ void MissionController::_newMissionItemsAvailableFromVehicle(bool removeAllReque for (; i < newMissionItems.count(); i++) { const MissionItem* missionItem = newMissionItems[i]; - SimpleMissionItem* simpleItem = new SimpleMissionItem(_controllerVehicle, _flyView, *missionItem, this); + SimpleMissionItem* simpleItem = new SimpleMissionItem(_masterController, _flyView, *missionItem, this); if (TakeoffMissionItem::isTakeoffCommand(static_cast(simpleItem->command()))) { // This needs to be a TakeoffMissionItem - TakeoffMissionItem* takeoffItem = new TakeoffMissionItem(*missionItem, _controllerVehicle, _flyView, settingsItem, this); + TakeoffMissionItem* takeoffItem = new TakeoffMissionItem(*missionItem, _masterController, _flyView, settingsItem, this); simpleItem->deleteLater(); simpleItem = takeoffItem; } @@ -202,7 +196,7 @@ void MissionController::_newMissionItemsAvailableFromVehicle(bool removeAllReque _visualItems = newControllerMissionItems; _settingsItem = settingsItem; - MissionController::_scanForAdditionalSettings(_visualItems, _controllerVehicle); + MissionController::_scanForAdditionalSettings(_visualItems, _masterController); _initAllVisualItems(); _updateContainsItems(); @@ -223,17 +217,6 @@ void MissionController::loadFromVehicle(void) } } -void MissionController::_warnIfTerrainFrameUsed(void) -{ - for (int i=1; i<_visualItems->count(); i++) { - SimpleMissionItem* simpleItem = qobject_cast(_visualItems->get(i)); - if (simpleItem && simpleItem->altitudeMode() == QGroundControlQmlGlobal::AltitudeModeTerrainFrame) { - qgcApp()->showMessage(tr("Warning: You are using MAV_FRAME_GLOBAL_TERRAIN_ALT in a mission. %1 does not support sending terrain tiles to vehicle.").arg(qgcApp()->applicationName())); - break; - } - } -} - void MissionController::sendToVehicle(void) { if (_masterController->offline()) { @@ -242,7 +225,6 @@ void MissionController::sendToVehicle(void) qCWarning(MissionControllerLog) << "MissionControllerLog::sendToVehicle called while syncInProgress"; } else { qCDebug(MissionControllerLog) << "MissionControllerLog::sendToVehicle"; - _warnIfTerrainFrameUsed(); if (_visualItems->count() == 1) { // This prevents us from sending a possibly bogus home position to the vehicle QmlObjectListModel emptyModel; @@ -355,7 +337,7 @@ int MissionController::_nextSequenceNumber(void) VisualMissionItem* MissionController::_insertSimpleMissionItemWorker(QGeoCoordinate coordinate, MAV_CMD command, int visualItemIndex, bool makeCurrentItem) { int sequenceNumber = _nextSequenceNumber(); - SimpleMissionItem * newItem = new SimpleMissionItem(_controllerVehicle, _flyView, this); + SimpleMissionItem * newItem = new SimpleMissionItem(_masterController, _flyView, this); newItem->setSequenceNumber(sequenceNumber); newItem->setCoordinate(coordinate); newItem->setCommand(command); @@ -396,7 +378,7 @@ VisualMissionItem* MissionController::insertSimpleMissionItem(QGeoCoordinate coo VisualMissionItem* MissionController::insertTakeoffItem(QGeoCoordinate /*coordinate*/, int visualItemIndex, bool makeCurrentItem) { int sequenceNumber = _nextSequenceNumber(); - TakeoffMissionItem * newItem = new TakeoffMissionItem(_controllerVehicle->vtol() ? MAV_CMD_NAV_VTOL_TAKEOFF : MAV_CMD_NAV_TAKEOFF, _controllerVehicle, _flyView, _settingsItem, this); + TakeoffMissionItem * newItem = new TakeoffMissionItem(_controllerVehicle->vtol() ? MAV_CMD_NAV_VTOL_TAKEOFF : MAV_CMD_NAV_TAKEOFF, _masterController, _flyView, _settingsItem, this); newItem->setSequenceNumber(sequenceNumber); _initVisualItem(newItem); @@ -465,14 +447,14 @@ VisualMissionItem* MissionController::insertComplexMissionItem(QString itemName, ComplexMissionItem* newItem = nullptr; if (itemName == patternSurveyName) { - newItem = new SurveyComplexItem(_controllerVehicle, _flyView, QString() /* kmlFile */, _visualItems /* parent */); + newItem = new SurveyComplexItem(_masterController, _flyView, QString() /* kmlFile */, _visualItems /* parent */); newItem->setCoordinate(mapCenterCoordinate); } else if (itemName == patternFWLandingName) { - newItem = new FixedWingLandingComplexItem(_controllerVehicle, _flyView, _visualItems /* parent */); + newItem = new FixedWingLandingComplexItem(_masterController, _flyView, _visualItems /* parent */); } else if (itemName == patternStructureScanName) { - newItem = new StructureScanComplexItem(_controllerVehicle, _flyView, QString() /* kmlFile */, _visualItems /* parent */); + newItem = new StructureScanComplexItem(_masterController, _flyView, QString() /* kmlFile */, _visualItems /* parent */); } else if (itemName == patternCorridorScanName) { - newItem = new CorridorScanComplexItem(_controllerVehicle, _flyView, QString() /* kmlFile */, _visualItems /* parent */); + newItem = new CorridorScanComplexItem(_masterController, _flyView, QString() /* kmlFile */, _visualItems /* parent */); } else { qWarning() << "Internal error: Unknown complex item:" << itemName; return nullptr; @@ -488,11 +470,11 @@ VisualMissionItem* MissionController::insertComplexMissionItemFromKMLOrSHP(QStri ComplexMissionItem* newItem = nullptr; if (itemName == patternSurveyName) { - newItem = new SurveyComplexItem(_controllerVehicle, _flyView, file, _visualItems); + newItem = new SurveyComplexItem(_masterController, _flyView, file, _visualItems); } else if (itemName == patternStructureScanName) { - newItem = new StructureScanComplexItem(_controllerVehicle, _flyView, file, _visualItems); + newItem = new StructureScanComplexItem(_masterController, _flyView, file, _visualItems); } else if (itemName == patternCorridorScanName) { - newItem = new CorridorScanComplexItem(_controllerVehicle, _flyView, file, _visualItems); + newItem = new CorridorScanComplexItem(_masterController, _flyView, file, _visualItems); } else { qWarning() << "Internal error: Unknown complex item:" << itemName; return nullptr; @@ -651,7 +633,7 @@ bool MissionController::_loadJsonMissionFileV1(const QJsonObject& json, QmlObjec return false; } - SurveyComplexItem* item = new SurveyComplexItem(_controllerVehicle, _flyView, QString() /* kmlFile */, visualItems /* parent */); + SurveyComplexItem* item = new SurveyComplexItem(_masterController, _flyView, QString() /* kmlFile */, visualItems /* parent */); const QJsonObject itemObject = itemValue.toObject(); if (item->load(itemObject, itemObject["id"].toInt(), errorString)) { surveyItems.append(item); @@ -669,7 +651,7 @@ bool MissionController::_loadJsonMissionFileV1(const QJsonObject& json, QmlObjec MissionSettingsItem* settingsItem = _addMissionSettings(visualItems); if (json.contains(_jsonPlannedHomePositionKey)) { - SimpleMissionItem* item = new SimpleMissionItem(_controllerVehicle, _flyView, visualItems); + SimpleMissionItem* item = new SimpleMissionItem(_masterController, _flyView, visualItems); if (item->load(json[_jsonPlannedHomePositionKey].toObject(), 0, errorString)) { settingsItem->setInitialHomePositionFromUser(item->coordinate()); item->deleteLater(); @@ -705,11 +687,11 @@ bool MissionController::_loadJsonMissionFileV1(const QJsonObject& json, QmlObjec } const QJsonObject itemObject = itemValue.toObject(); - SimpleMissionItem* item = new SimpleMissionItem(_controllerVehicle, _flyView, visualItems); + SimpleMissionItem* item = new SimpleMissionItem(_masterController, _flyView, visualItems); if (item->load(itemObject, itemObject["id"].toInt(), errorString)) { if (TakeoffMissionItem::isTakeoffCommand(item->mavCommand())) { // This needs to be a TakeoffMissionItem - TakeoffMissionItem* takeoffItem = new TakeoffMissionItem(_controllerVehicle, _flyView, settingsItem, visualItems); + TakeoffMissionItem* takeoffItem = new TakeoffMissionItem(_masterController, _flyView, settingsItem, visualItems); takeoffItem->load(itemObject, itemObject["id"].toInt(), errorString); item->deleteLater(); item = takeoffItem; @@ -764,7 +746,7 @@ bool MissionController::_loadJsonMissionFileV2(const QJsonObject& json, QmlObjec if (!JsonHelper::loadGeoCoordinate(json[_jsonPlannedHomePositionKey], true /* altitudeRequired */, homeCoordinate, errorString)) { return false; } - MissionSettingsItem* settingsItem = new MissionSettingsItem(_controllerVehicle, _flyView, visualItems); + MissionSettingsItem* settingsItem = new MissionSettingsItem(_masterController, _flyView, visualItems); settingsItem->setCoordinate(homeCoordinate); visualItems->insert(0, settingsItem); qCDebug(MissionControllerLog) << "plannedHomePosition" << homeCoordinate; @@ -793,11 +775,11 @@ bool MissionController::_loadJsonMissionFileV2(const QJsonObject& json, QmlObjec QString itemType = itemObject[VisualMissionItem::jsonTypeKey].toString(); if (itemType == VisualMissionItem::jsonTypeSimpleItemValue) { - SimpleMissionItem* simpleItem = new SimpleMissionItem(_controllerVehicle, _flyView, visualItems); + SimpleMissionItem* simpleItem = new SimpleMissionItem(_masterController, _flyView, visualItems); if (simpleItem->load(itemObject, nextSequenceNumber, errorString)) { if (TakeoffMissionItem::isTakeoffCommand(static_cast(simpleItem->command()))) { // This needs to be a TakeoffMissionItem - TakeoffMissionItem* takeoffItem = new TakeoffMissionItem(_controllerVehicle, _flyView, settingsItem, this); + TakeoffMissionItem* takeoffItem = new TakeoffMissionItem(_masterController, _flyView, settingsItem, this); takeoffItem->load(itemObject, nextSequenceNumber, errorString); simpleItem->deleteLater(); simpleItem = takeoffItem; @@ -819,7 +801,7 @@ bool MissionController::_loadJsonMissionFileV2(const QJsonObject& json, QmlObjec if (complexItemType == SurveyComplexItem::jsonComplexItemTypeValue) { qCDebug(MissionControllerLog) << "Loading Survey: nextSequenceNumber" << nextSequenceNumber; - SurveyComplexItem* surveyItem = new SurveyComplexItem(_controllerVehicle, _flyView, QString() /* kmlFile */, visualItems); + SurveyComplexItem* surveyItem = new SurveyComplexItem(_masterController, _flyView, QString() /* kmlFile */, visualItems); if (!surveyItem->load(itemObject, nextSequenceNumber++, errorString)) { return false; } @@ -828,7 +810,7 @@ bool MissionController::_loadJsonMissionFileV2(const QJsonObject& json, QmlObjec visualItems->append(surveyItem); } else if (complexItemType == FixedWingLandingComplexItem::jsonComplexItemTypeValue) { qCDebug(MissionControllerLog) << "Loading Fixed Wing Landing Pattern: nextSequenceNumber" << nextSequenceNumber; - FixedWingLandingComplexItem* landingItem = new FixedWingLandingComplexItem(_controllerVehicle, _flyView, visualItems); + FixedWingLandingComplexItem* landingItem = new FixedWingLandingComplexItem(_masterController, _flyView, visualItems); if (!landingItem->load(itemObject, nextSequenceNumber++, errorString)) { return false; } @@ -837,7 +819,7 @@ bool MissionController::_loadJsonMissionFileV2(const QJsonObject& json, QmlObjec visualItems->append(landingItem); } else if (complexItemType == StructureScanComplexItem::jsonComplexItemTypeValue) { qCDebug(MissionControllerLog) << "Loading Structure Scan: nextSequenceNumber" << nextSequenceNumber; - StructureScanComplexItem* structureItem = new StructureScanComplexItem(_controllerVehicle, _flyView, QString() /* kmlFile */, visualItems); + StructureScanComplexItem* structureItem = new StructureScanComplexItem(_masterController, _flyView, QString() /* kmlFile */, visualItems); if (!structureItem->load(itemObject, nextSequenceNumber++, errorString)) { return false; } @@ -846,7 +828,7 @@ bool MissionController::_loadJsonMissionFileV2(const QJsonObject& json, QmlObjec visualItems->append(structureItem); } else if (complexItemType == CorridorScanComplexItem::jsonComplexItemTypeValue) { qCDebug(MissionControllerLog) << "Loading Corridor Scan: nextSequenceNumber" << nextSequenceNumber; - CorridorScanComplexItem* corridorItem = new CorridorScanComplexItem(_controllerVehicle, _flyView, QString() /* kmlFile */, visualItems); + CorridorScanComplexItem* corridorItem = new CorridorScanComplexItem(_masterController, _flyView, QString() /* kmlFile */, visualItems); if (!corridorItem->load(itemObject, nextSequenceNumber++, errorString)) { return false; } @@ -937,14 +919,14 @@ bool MissionController::_loadTextMissionFile(QTextStream& stream, QmlObjectListM MissionSettingsItem* settingsItem = _addMissionSettings(visualItems); while (!stream.atEnd()) { - SimpleMissionItem* item = new SimpleMissionItem(_controllerVehicle, _flyView, visualItems); + SimpleMissionItem* item = new SimpleMissionItem(_masterController, _flyView, visualItems); if (item->load(stream)) { if (firstItem && plannedHomePositionInFile) { settingsItem->setInitialHomePositionFromUser(item->coordinate()); } else { if (TakeoffMissionItem::isTakeoffCommand(static_cast(item->command()))) { // This needs to be a TakeoffMissionItem - TakeoffMissionItem* takeoffItem = new TakeoffMissionItem(_controllerVehicle, _flyView, settingsItem, visualItems); + TakeoffMissionItem* takeoffItem = new TakeoffMissionItem(_masterController, _flyView, settingsItem, visualItems); takeoffItem->load(stream); item->deleteLater(); item = takeoffItem; @@ -991,7 +973,7 @@ void MissionController::_initLoadedVisualItems(QmlObjectListModel* loadedVisualI _settingsItem = _visualItems->value(0); } - MissionController::_scanForAdditionalSettings(_visualItems, _controllerVehicle); + MissionController::_scanForAdditionalSettings(_visualItems, _masterController); _initAllVisualItems(); } @@ -1925,7 +1907,7 @@ void MissionController::_itemCommandChanged(void) _recalcWaypointLines(); } -void MissionController::managerVehicleChanged(Vehicle* managerVehicle) +void MissionController::_managerVehicleChanged(Vehicle* managerVehicle) { if (_managerVehicle) { _missionManager->disconnect(this); @@ -2015,7 +1997,7 @@ MissionSettingsItem* MissionController::_addMissionSettings(QmlObjectListModel* { qCDebug(MissionControllerLog) << "_addMissionSettings"; - MissionSettingsItem* settingsItem = new MissionSettingsItem(_managerVehicle, _flyView, visualItems); + MissionSettingsItem* settingsItem = new MissionSettingsItem(_masterController, _flyView, visualItems); visualItems->insert(0, settingsItem); if (visualItems == _visualItems) { @@ -2126,10 +2108,10 @@ void MissionController::setDirty(bool dirty) } } -void MissionController::_scanForAdditionalSettings(QmlObjectListModel* visualItems, Vehicle* vehicle) +void MissionController::_scanForAdditionalSettings(QmlObjectListModel* visualItems, PlanMasterController* masterController) { // First we look for a Fixed Wing Landing Pattern which is at the end - FixedWingLandingComplexItem::scanForItem(visualItems, _flyView, vehicle); + FixedWingLandingComplexItem::scanForItem(visualItems, _flyView, masterController); int scanIndex = 0; while (scanIndex < visualItems->count()) { @@ -2149,7 +2131,7 @@ void MissionController::_scanForAdditionalSettings(QmlObjectListModel* visualIte SimpleMissionItem* simpleItem = qobject_cast(visualItem); if (simpleItem) { scanIndex++; - simpleItem->scanForSections(visualItems, scanIndex, vehicle); + simpleItem->scanForSections(visualItems, scanIndex, masterController); } else { // Complex item, can't have sections scanIndex++; diff --git a/src/MissionManager/MissionController.h b/src/MissionManager/MissionController.h index d72f52895..dc128f188 100644 --- a/src/MissionManager/MissionController.h +++ b/src/MissionManager/MissionController.h @@ -190,7 +190,6 @@ public: bool dirty (void) const final; void setDirty (bool dirty) final; bool containsItems (void) const final; - void managerVehicleChanged (Vehicle* managerVehicle) final; bool showPlanFromManagerVehicle (void) final; // Create KML file @@ -286,6 +285,7 @@ private slots: void _updateTimeout (void); void _complexBoundingBoxChanged (void); void _recalcAll (void); + void _managerVehicleChanged (Vehicle* managerVehicle); private: void _init(void); @@ -310,7 +310,7 @@ private: bool _loadJsonMissionFileV2(const QJsonObject& json, QmlObjectListModel* visualItems, QString& errorString); bool _loadTextMissionFile(QTextStream& stream, QmlObjectListModel* visualItems, QString& errorString); int _nextSequenceNumber(void); - void _scanForAdditionalSettings(QmlObjectListModel* visualItems, Vehicle* vehicle); + void _scanForAdditionalSettings(QmlObjectListModel* visualItems, PlanMasterController* masterController); static bool _convertToMissionItems(QmlObjectListModel* visualMissionItems, QList& rgMissionItems, QObject* missionItemParent); void _setPlannedHomePositionFromFirstCoordinate(const QGeoCoordinate& clickCoordinate); void _resetMissionFlightStatus(void); @@ -323,35 +323,36 @@ private: void _addTimeDistance(bool vtolInHover, double hoverTime, double cruiseTime, double extraTime, double distance, int seqNum); VisualMissionItem* _insertSimpleMissionItemWorker(QGeoCoordinate coordinate, MAV_CMD command, int visualItemIndex, bool makeCurrentItem); void _insertComplexMissionItemWorker(const QGeoCoordinate& mapCenterCoordinate, ComplexMissionItem* complexItem, int visualItemIndex, bool makeCurrentItem); - void _warnIfTerrainFrameUsed(void); bool _isROIBeginItem(SimpleMissionItem* simpleItem); bool _isROICancelItem(SimpleMissionItem* simpleItem); CoordinateVector* _createCoordinateVectorWorker(VisualItemPair& pair); private: - MissionManager* _missionManager; - int _missionItemCount; - QmlObjectListModel* _visualItems; - MissionSettingsItem* _settingsItem; + Vehicle* _controllerVehicle = nullptr; + Vehicle* _managerVehicle = nullptr; + MissionManager* _missionManager = nullptr; + int _missionItemCount = 0; + QmlObjectListModel* _visualItems = nullptr; + MissionSettingsItem* _settingsItem = nullptr; QmlObjectListModel _waypointLines; QVariantList _waypointPath; QmlObjectListModel _directionArrows; QmlObjectListModel _incompleteComplexItemLines; CoordVectHashTable _linesTable; - bool _firstItemsFromVehicle; - bool _itemsRequested; - bool _inRecalcSequence; + bool _firstItemsFromVehicle = false; + bool _itemsRequested = false; + bool _inRecalcSequence = false; MissionFlightStatus_t _missionFlightStatus; - AppSettings* _appSettings; - double _progressPct; - int _currentPlanViewSeqNum; - int _currentPlanViewVIIndex; - VisualMissionItem* _currentPlanViewItem; + AppSettings* _appSettings = nullptr; + double _progressPct = 0; + int _currentPlanViewSeqNum = -1; + int _currentPlanViewVIIndex = -1; + VisualMissionItem* _currentPlanViewItem = nullptr; QTimer _updateTimer; QGCGeoBoundingCube _travelBoundingCube; QGeoCoordinate _takeoffCoordinate; QGeoCoordinate _previousCoordinate; - CoordinateVector* _splitSegment; + CoordinateVector* _splitSegment = nullptr; bool _isInsertTakeoffValid = true; bool _isInsertLandValid = true; bool _isROIActive = false; diff --git a/src/MissionManager/MissionItemTest.cc b/src/MissionManager/MissionItemTest.cc index ddee6ee61..33946aef8 100644 --- a/src/MissionManager/MissionItemTest.cc +++ b/src/MissionManager/MissionItemTest.cc @@ -28,22 +28,19 @@ const size_t MissionItemTest::_cTestCases = sizeof(_rgTestCases)/sizeof(_rgTestC #endif MissionItemTest::MissionItemTest(void) - : _offlineVehicle(nullptr) + : _masterController(nullptr) { } void MissionItemTest::init(void) { UnitTest::init(); - _offlineVehicle = new Vehicle(MAV_AUTOPILOT_PX4, - MAV_TYPE_QUADROTOR, - qgcApp()->toolbox()->firmwarePluginManager(), - this); + _masterController = new PlanMasterController(this); } void MissionItemTest::cleanup(void) { - _offlineVehicle->deleteLater(); + _masterController->deleteLater(); UnitTest::cleanup(); } @@ -281,7 +278,7 @@ void MissionItemTest::_testSimpleLoadFromStream(void) { // We specifically test SimpleMissionItem loading as well since it has additional // signalling which can affect values. - SimpleMissionItem simpleMissionItem(_offlineVehicle, false /* flyView */, nullptr); + SimpleMissionItem simpleMissionItem(_masterController, false /* flyView */, nullptr); QString testString("10\t0\t3\t80\t10\t20\t30\t40\t-10\t-20\t-30\t1\r\n"); QTextStream testStream(&testString, QIODevice::ReadOnly); @@ -451,7 +448,7 @@ void MissionItemTest::_testSimpleLoadFromJson(void) // We specifically test SimpleMissionItem loading as well since it has additional // signalling which can affect values. - SimpleMissionItem simpleMissionItem(_offlineVehicle, false /* flyView */, nullptr); + SimpleMissionItem simpleMissionItem(_masterController, false /* flyView */, nullptr); QString errorString; QJsonArray coordinateArray; QJsonObject jsonObject; diff --git a/src/MissionManager/MissionItemTest.h b/src/MissionManager/MissionItemTest.h index 338ca8b08..32d0a4a65 100644 --- a/src/MissionManager/MissionItemTest.h +++ b/src/MissionManager/MissionItemTest.h @@ -7,14 +7,13 @@ * ****************************************************************************/ - -#ifndef MissionItemTest_H -#define MissionItemTest_H +#pragma once #include "UnitTest.h" #include "MultiSignalSpy.h" #include "MissionItem.h" #include "Vehicle.h" +#include "PlanMasterController.h" /// Unit test for the MissionItem Object class MissionItemTest : public UnitTest @@ -46,8 +45,6 @@ private: QJsonObject _createV2Json(void); QJsonObject _createV3Json(bool allNaNs = false); - int _seq = 10; - Vehicle* _offlineVehicle; + int _seq = 10; + PlanMasterController* _masterController = nullptr; }; - -#endif diff --git a/src/MissionManager/MissionSettingsItem.cc b/src/MissionManager/MissionSettingsItem.cc index 8f9a02e5f..30c412489 100644 --- a/src/MissionManager/MissionSettingsItem.cc +++ b/src/MissionManager/MissionSettingsItem.cc @@ -16,6 +16,7 @@ #include "SettingsManager.h" #include "AppSettings.h" #include "MissionCommandUIInfo.h" +#include "PlanMasterController.h" #include @@ -25,11 +26,12 @@ const char* MissionSettingsItem::_plannedHomePositionAltitudeName = "PlannedHome QMap MissionSettingsItem::_metaDataMap; -MissionSettingsItem::MissionSettingsItem(Vehicle* vehicle, bool flyView, QObject* parent) - : ComplexMissionItem (vehicle, flyView, parent) +MissionSettingsItem::MissionSettingsItem(PlanMasterController* masterController, bool flyView, QObject* parent) + : ComplexMissionItem (masterController, flyView, parent) + , _managerVehicle (masterController->managerVehicle()) , _plannedHomePositionAltitudeFact (0, _plannedHomePositionAltitudeName, FactMetaData::valueTypeDouble) - , _cameraSection (vehicle) - , _speedSection (vehicle) + , _cameraSection (masterController) + , _speedSection (masterController) { _isIncomplete = false; _editorQml = "qrc:/qml/MissionSettingsEditor.qml"; @@ -54,10 +56,10 @@ MissionSettingsItem::MissionSettingsItem(Vehicle* vehicle, bool flyView, QObject connect(&_cameraSection, &CameraSection::specifiedGimbalYawChanged, this, &MissionSettingsItem::specifiedGimbalYawChanged); connect(&_cameraSection, &CameraSection::specifiedGimbalPitchChanged, this, &MissionSettingsItem::specifiedGimbalPitchChanged); connect(&_speedSection, &SpeedSection::specifiedFlightSpeedChanged, this, &MissionSettingsItem::specifiedFlightSpeedChanged); - connect(_vehicle, &Vehicle::homePositionChanged, this, &MissionSettingsItem::_updateHomePosition); connect(&_plannedHomePositionAltitudeFact, &Fact::rawValueChanged, this, &MissionSettingsItem::_updateAltitudeInCoordinate); - _updateHomePosition(_vehicle->homePosition()); + connect(_managerVehicle, &Vehicle::homePositionChanged, this, &MissionSettingsItem::_updateHomePosition); + _updateHomePosition(_managerVehicle->homePosition()); } int MissionSettingsItem::lastSequenceNumber(void) const diff --git a/src/MissionManager/MissionSettingsItem.h b/src/MissionManager/MissionSettingsItem.h index a0d671c79..ba3e7417e 100644 --- a/src/MissionManager/MissionSettingsItem.h +++ b/src/MissionManager/MissionSettingsItem.h @@ -19,12 +19,14 @@ Q_DECLARE_LOGGING_CATEGORY(MissionSettingsComplexItemLog) +class PlanMasterController; + class MissionSettingsItem : public ComplexMissionItem { Q_OBJECT public: - MissionSettingsItem(Vehicle* vehicle, bool flyView, QObject* parent); + MissionSettingsItem(PlanMasterController* masterController, bool flyView, QObject* parent); Q_PROPERTY(Fact* plannedHomePositionAltitude READ plannedHomePositionAltitude CONSTANT) Q_PROPERTY(QObject* cameraSection READ cameraSection CONSTANT) @@ -103,6 +105,7 @@ private slots: void _updateHomePosition (const QGeoCoordinate& homePosition); private: + Vehicle* _managerVehicle = nullptr; QGeoCoordinate _plannedHomePositionCoordinate; // Does not include altitude Fact _plannedHomePositionAltitudeFact; int _sequenceNumber = 0; diff --git a/src/MissionManager/MissionSettingsTest.cc b/src/MissionManager/MissionSettingsTest.cc index 964707cc9..40e45be9d 100644 --- a/src/MissionManager/MissionSettingsTest.cc +++ b/src/MissionManager/MissionSettingsTest.cc @@ -22,7 +22,7 @@ void MissionSettingsTest::init(void) { VisualMissionItemTest::init(); - _settingsItem = new MissionSettingsItem(_offlineVehicle, false /* flyView */, this); + _settingsItem = new MissionSettingsItem(_masterController, false /* flyView */, this); } void MissionSettingsTest::cleanup(void) diff --git a/src/MissionManager/PlanElementController.cc b/src/MissionManager/PlanElementController.cc index 816339e45..6939231bc 100644 --- a/src/MissionManager/PlanElementController.cc +++ b/src/MissionManager/PlanElementController.cc @@ -17,8 +17,6 @@ PlanElementController::PlanElementController(PlanMasterController* masterController, QObject* parent) : QObject (parent) , _masterController (masterController) - , _controllerVehicle(masterController->controllerVehicle()) - , _managerVehicle (masterController->managerVehicle()) , _flyView (false) { @@ -33,8 +31,3 @@ void PlanElementController::start(bool flyView) { _flyView = flyView; } - -void PlanElementController::managerVehicleChanged(Vehicle* managerVehicle) -{ - _managerVehicle = managerVehicle; -} diff --git a/src/MissionManager/PlanElementController.h b/src/MissionManager/PlanElementController.h index 7e37db275..c085887cd 100644 --- a/src/MissionManager/PlanElementController.h +++ b/src/MissionManager/PlanElementController.h @@ -27,10 +27,13 @@ public: PlanElementController(PlanMasterController* masterController, QObject* parent = nullptr); ~PlanElementController(); - Q_PROPERTY(bool supported READ supported NOTIFY supportedChanged) ///< true: Element is supported by Vehicle - Q_PROPERTY(bool containsItems READ containsItems NOTIFY containsItemsChanged) ///< true: Elemement is non-empty - Q_PROPERTY(bool syncInProgress READ syncInProgress NOTIFY syncInProgressChanged) ///< true: information is currently being saved/sent, false: no active save/send in progress - Q_PROPERTY(bool dirty READ dirty WRITE setDirty NOTIFY dirtyChanged) ///< true: unsaved/sent changes are present, false: no changes since last save/send + Q_PROPERTY(PlanMasterController* masterController READ masterController CONSTANT) + Q_PROPERTY(bool supported READ supported NOTIFY supportedChanged) ///< true: Element is supported by Vehicle + Q_PROPERTY(bool containsItems READ containsItems NOTIFY containsItemsChanged) ///< true: Elemement is non-empty + Q_PROPERTY(bool syncInProgress READ syncInProgress NOTIFY syncInProgressChanged) ///< true: information is currently being saved/sent, false: no active save/send in progress + Q_PROPERTY(bool dirty READ dirty WRITE setDirty NOTIFY dirtyChanged) ///< true: unsaved/sent changes are present, false: no changes since last save/send + + PlanMasterController* masterController(void) { return _masterController; } /// Should be called immediately upon Component.onCompleted. virtual void start(bool flyView); @@ -55,9 +58,6 @@ public: /// Signals removeAllComplete when done virtual void removeAllFromVehicle(void) = 0; - /// Called when a new manager vehicle has been set. - virtual void managerVehicleChanged(Vehicle* managerVehicle) = 0; - signals: void supportedChanged (bool supported); void containsItemsChanged (bool containsItems); @@ -68,8 +68,6 @@ signals: protected: PlanMasterController* _masterController; - Vehicle* _controllerVehicle; ///< Offline controller vehicle - Vehicle* _managerVehicle; ///< Either active vehicle or _controllerVehicle if none bool _flyView; }; diff --git a/src/MissionManager/PlanMasterController.cc b/src/MissionManager/PlanMasterController.cc index 3e61d6f2c..953e7f467 100644 --- a/src/MissionManager/PlanMasterController.cc +++ b/src/MissionManager/PlanMasterController.cc @@ -37,24 +37,36 @@ const char* PlanMasterController::kJsonGeoFenceObjectKey = "geoFence"; const char* PlanMasterController::kJsonRallyPointsObjectKey = "rallyPoints"; PlanMasterController::PlanMasterController(QObject* parent) - : QObject (parent) - , _multiVehicleMgr (qgcApp()->toolbox()->multiVehicleManager()) - , _controllerVehicle (new Vehicle( - static_cast(qgcApp()->toolbox()->settingsManager()->appSettings()->offlineEditingFirmwareType()->rawValue().toInt()), - static_cast(qgcApp()->toolbox()->settingsManager()->appSettings()->offlineEditingVehicleType()->rawValue().toInt()), - qgcApp()->toolbox()->firmwarePluginManager())) - , _managerVehicle (_controllerVehicle) - , _flyView (true) - , _offline (true) - , _missionController (this) - , _geoFenceController (this) - , _rallyPointController (this) - , _loadGeoFence (false) - , _loadRallyPoints (false) - , _sendGeoFence (false) - , _sendRallyPoints (false) - , _deleteWhenSendCompleted (false) - , _planCreators (nullptr) + : QObject (parent) + , _multiVehicleMgr (qgcApp()->toolbox()->multiVehicleManager()) + , _controllerVehicle (new Vehicle( + static_cast(qgcApp()->toolbox()->settingsManager()->appSettings()->offlineEditingFirmwareType()->rawValue().toInt()), + static_cast(qgcApp()->toolbox()->settingsManager()->appSettings()->offlineEditingVehicleType()->rawValue().toInt()), + qgcApp()->toolbox()->firmwarePluginManager(), + this)) + , _managerVehicle (_controllerVehicle) + , _missionController (this) + , _geoFenceController (this) + , _rallyPointController (this) +{ + _commonInit(); +} + +#ifdef QT_DEBUG +PlanMasterController::PlanMasterController(MAV_AUTOPILOT firmwareType, MAV_TYPE vehicleType, QObject* parent) + : QObject (parent) + , _multiVehicleMgr (qgcApp()->toolbox()->multiVehicleManager()) + , _controllerVehicle (new Vehicle(firmwareType, vehicleType, qgcApp()->toolbox()->firmwarePluginManager())) + , _managerVehicle (_controllerVehicle) + , _missionController (this) + , _geoFenceController (this) + , _rallyPointController (this) +{ + _commonInit(); +} +#endif + +void PlanMasterController::_commonInit(void) { connect(&_missionController, &MissionController::dirtyChanged, this, &PlanMasterController::dirtyChanged); connect(&_geoFenceController, &GeoFenceController::dirtyChanged, this, &PlanMasterController::dirtyChanged); @@ -67,8 +79,16 @@ PlanMasterController::PlanMasterController(QObject* parent) connect(&_missionController, &MissionController::syncInProgressChanged, this, &PlanMasterController::syncInProgressChanged); connect(&_geoFenceController, &GeoFenceController::syncInProgressChanged, this, &PlanMasterController::syncInProgressChanged); connect(&_rallyPointController, &RallyPointController::syncInProgressChanged, this, &PlanMasterController::syncInProgressChanged); + + // Offline vehicle can change firmware/vehicle type + connect(_controllerVehicle, &Vehicle::capabilityBitsChanged, this, &PlanMasterController::_updateSupportsTerrain); + connect(_controllerVehicle, &Vehicle::vehicleTypeChanged, this, &PlanMasterController::_updateSupportsTerrain); + connect(_controllerVehicle, &Vehicle::vehicleTypeChanged, this, &PlanMasterController::_updatePlanCreatorsList); + + _updateSupportsTerrain(); } + PlanMasterController::~PlanMasterController() { @@ -81,8 +101,8 @@ void PlanMasterController::start(bool flyView) _geoFenceController.start(_flyView); _rallyPointController.start(_flyView); - connect(_multiVehicleMgr, &MultiVehicleManager::activeVehicleChanged, this, &PlanMasterController::_activeVehicleChanged); _activeVehicleChanged(_multiVehicleMgr->activeVehicle()); + connect(_multiVehicleMgr, &MultiVehicleManager::activeVehicleChanged, this, &PlanMasterController::_activeVehicleChanged); _updatePlanCreatorsList(); @@ -115,22 +135,17 @@ void PlanMasterController::_activeVehicleChanged(Vehicle* activeVehicle) qCDebug(PlanMasterControllerLog) << "_activeVehicleChanged" << activeVehicle; if (_managerVehicle) { - // Disconnect old vehicle - disconnect(_managerVehicle->missionManager(), &MissionManager::newMissionItemsAvailable, this, &PlanMasterController::_loadMissionComplete); - disconnect(_managerVehicle->geoFenceManager(), &GeoFenceManager::loadComplete, this, &PlanMasterController::_loadGeoFenceComplete); - disconnect(_managerVehicle->rallyPointManager(), &RallyPointManager::loadComplete, this, &PlanMasterController::_loadRallyPointsComplete); - disconnect(_managerVehicle->missionManager(), &MissionManager::sendComplete, this, &PlanMasterController::_sendMissionComplete); - disconnect(_managerVehicle->geoFenceManager(), &GeoFenceManager::sendComplete, this, &PlanMasterController::_sendGeoFenceComplete); - disconnect(_managerVehicle->rallyPointManager(), &RallyPointManager::sendComplete, this, &PlanMasterController::_sendRallyPointsComplete); - disconnect(_managerVehicle, &Vehicle::vehicleTypeChanged, this, &PlanMasterController::_updatePlanCreatorsList); + // Disconnect old vehicle. Be careful of wildcarding disconnect too much since _managerVehicle may equal _controllerVehicle + disconnect(_managerVehicle->missionManager(), nullptr, nullptr, nullptr); + disconnect(_managerVehicle->geoFenceManager(), nullptr, nullptr, nullptr); + disconnect(_managerVehicle->rallyPointManager(), nullptr, nullptr, nullptr); + disconnect(_managerVehicle, &Vehicle::capabilityBitsChanged, this, &PlanMasterController::_updateSupportsTerrain); } bool newOffline = false; if (activeVehicle == nullptr) { // Since there is no longer an active vehicle we use the offline controller vehicle as the manager vehicle _managerVehicle = _controllerVehicle; - // The vehicle type can change on the offline vehicle. Keep the creators list in sync with that. - connect(_managerVehicle, &Vehicle::vehicleTypeChanged, this, &PlanMasterController::_updatePlanCreatorsList); newOffline = true; } else { newOffline = false; @@ -150,9 +165,10 @@ void PlanMasterController::_activeVehicleChanged(Vehicle* activeVehicle) connect(_managerVehicle->rallyPointManager(), &RallyPointManager::sendComplete, this, &PlanMasterController::_sendRallyPointsComplete); } - _missionController.managerVehicleChanged(_managerVehicle); - _geoFenceController.managerVehicleChanged(_managerVehicle); - _rallyPointController.managerVehicleChanged(_managerVehicle); + // Change in capabilities will affect terrain support + connect(_managerVehicle, &Vehicle::capabilityBitsChanged, this, &PlanMasterController::_updateSupportsTerrain); + + emit managerVehicleChanged(_managerVehicle); // Vehicle changed so we need to signal everything _offline = newOffline; @@ -160,6 +176,7 @@ void PlanMasterController::_activeVehicleChanged(Vehicle* activeVehicle) emit syncInProgressChanged(); emit dirtyChanged(dirty()); emit offlineChanged(offline()); + _updateSupportsTerrain(); if (!_flyView) { if (!offline()) { @@ -618,3 +635,12 @@ void PlanMasterController::_updatePlanCreatorsList(void) } } } + +void PlanMasterController::_updateSupportsTerrain(void) +{ + bool supportsTerrain = _managerVehicle->capabilityBits() & MAV_PROTOCOL_CAPABILITY_TERRAIN; + if (supportsTerrain != _supportsTerrain) { + _supportsTerrain = supportsTerrain; + emit supportsTerrainChanged(supportsTerrain); + } +} diff --git a/src/MissionManager/PlanMasterController.h b/src/MissionManager/PlanMasterController.h index 3ec1274d3..681c4f02d 100644 --- a/src/MissionManager/PlanMasterController.h +++ b/src/MissionManager/PlanMasterController.h @@ -28,12 +28,18 @@ class PlanMasterController : public QObject public: PlanMasterController(QObject* parent = nullptr); +#ifdef QT_DEBUG + // Used by test code to create master controll with specific firmware/vehicle type + PlanMasterController(MAV_AUTOPILOT firmwareType, MAV_TYPE vehicleType, QObject* parent = nullptr); +#endif + ~PlanMasterController(); + Q_PROPERTY(Vehicle* controllerVehicle READ controllerVehicle CONSTANT) ///< Offline controller vehicle + Q_PROPERTY(Vehicle* managerVehicle READ managerVehicle NOTIFY managerVehicleChanged) ///< Either active vehicle or _controllerVehicle if no active vehicle Q_PROPERTY(MissionController* missionController READ missionController CONSTANT) Q_PROPERTY(GeoFenceController* geoFenceController READ geoFenceController CONSTANT) Q_PROPERTY(RallyPointController* rallyPointController READ rallyPointController CONSTANT) - Q_PROPERTY(Vehicle* controllerVehicle MEMBER _controllerVehicle CONSTANT) Q_PROPERTY(bool offline READ offline NOTIFY offlineChanged) ///< true: controller is not connected to an active vehicle Q_PROPERTY(bool containsItems READ containsItems NOTIFY containsItemsChanged) ///< true: Elemement is non-empty Q_PROPERTY(bool syncInProgress READ syncInProgress NOTIFY syncInProgressChanged) ///< true: Information is currently being saved/sent, false: no active save/send in progress @@ -44,6 +50,7 @@ public: Q_PROPERTY(QStringList loadNameFilters READ loadNameFilters CONSTANT) ///< File filter list loading plan files Q_PROPERTY(QStringList saveNameFilters READ saveNameFilters CONSTANT) ///< File filter list saving plan files Q_PROPERTY(QmlObjectListModel* planCreators MEMBER _planCreators NOTIFY planCreatorsChanged) + Q_PROPERTY(bool supportsTerrain READ supportsTerrain NOTIFY supportsTerrainChanged) /// Should be called immediately upon Component.onCompleted. Q_INVOKABLE void start(bool flyView); @@ -86,6 +93,7 @@ public: QStringList loadNameFilters (void) const; QStringList saveNameFilters (void) const; bool isEmpty (void) const; + bool supportsTerrain (void) const { return _supportsTerrain; } QJsonDocument saveToJson (); @@ -105,6 +113,8 @@ signals: void offlineChanged (bool offlineEditing); void currentPlanFileChanged (); void planCreatorsChanged (QmlObjectListModel* planCreators); + void managerVehicleChanged (Vehicle* managerVehicle); + void supportsTerrainChanged (bool supportsTerrain); private slots: void _activeVehicleChanged (Vehicle* activeVehicle); @@ -115,27 +125,29 @@ private slots: void _sendGeoFenceComplete (void); void _sendRallyPointsComplete (void); void _updatePlanCreatorsList (void); + void _updateSupportsTerrain (void); #if defined(QGC_AIRMAP_ENABLED) void _startFlightPlanning (void); #endif private: + void _commonInit (void); void _showPlanFromManagerVehicle(void); - MultiVehicleManager* _multiVehicleMgr; - Vehicle* _controllerVehicle; ///< Offline controller vehicle - Vehicle* _managerVehicle; ///< Either active vehicle or _controllerVehicle if none - bool _flyView; - bool _offline; + MultiVehicleManager* _multiVehicleMgr = nullptr; + Vehicle* _controllerVehicle = nullptr; ///< Offline controller vehicle + Vehicle* _managerVehicle = nullptr; ///< Either active vehicle or _controllerVehicle if none + bool _flyView = true; + bool _offline = true; MissionController _missionController; GeoFenceController _geoFenceController; RallyPointController _rallyPointController; - bool _loadGeoFence; - bool _loadRallyPoints; - bool _sendGeoFence; - bool _sendRallyPoints; + bool _loadGeoFence = false; + bool _loadRallyPoints = false; + bool _sendGeoFence = false; + bool _sendRallyPoints = false; QString _currentPlanFile; - bool _deleteWhenSendCompleted; - QmlObjectListModel* _planCreators; - + bool _deleteWhenSendCompleted = false; + QmlObjectListModel* _planCreators = nullptr; + bool _supportsTerrain = false; }; diff --git a/src/MissionManager/RallyPointController.cc b/src/MissionManager/RallyPointController.cc index 7004edb18..9a85904d0 100644 --- a/src/MissionManager/RallyPointController.cc +++ b/src/MissionManager/RallyPointController.cc @@ -34,15 +34,11 @@ const char* RallyPointController::_jsonFileTypeValue = "RallyPoints"; const char* RallyPointController::_jsonPointsKey = "points"; RallyPointController::RallyPointController(PlanMasterController* masterController, QObject* parent) - : PlanElementController(masterController, parent) - , _rallyPointManager(_managerVehicle->rallyPointManager()) - , _dirty(false) - , _currentRallyPoint(nullptr) - , _itemsRequested(false) + : PlanElementController (masterController, parent) + , _managerVehicle (masterController->managerVehicle()) + , _rallyPointManager (masterController->managerVehicle()->rallyPointManager()) { connect(&_points, &QmlObjectListModel::countChanged, this, &RallyPointController::_updateContainsItems); - - managerVehicleChanged(_managerVehicle); } RallyPointController::~RallyPointController() @@ -50,7 +46,17 @@ RallyPointController::~RallyPointController() } -void RallyPointController::managerVehicleChanged(Vehicle* managerVehicle) +void RallyPointController::start(bool flyView) +{ + qCDebug(GeoFenceControllerLog) << "start flyView" << flyView; + + _managerVehicleChanged(_masterController->managerVehicle()); + connect(_masterController, &PlanMasterController::managerVehicleChanged, this, &RallyPointController::_managerVehicleChanged); + + PlanElementController::start(flyView); +} + +void RallyPointController::_managerVehicleChanged(Vehicle* managerVehicle) { if (_managerVehicle) { _rallyPointManager->disconnect(this); diff --git a/src/MissionManager/RallyPointController.h b/src/MissionManager/RallyPointController.h index b903da6fa..6f93c2365 100644 --- a/src/MissionManager/RallyPointController.h +++ b/src/MissionManager/RallyPointController.h @@ -36,6 +36,7 @@ public: Q_INVOKABLE void addPoint (QGeoCoordinate point); Q_INVOKABLE void removePoint (QObject* rallyPoint); + void start (bool flyView) final; bool supported (void) const final; void save (QJsonObject& json) final; bool load (const QJsonObject& json, QString& errorString) final; @@ -47,7 +48,6 @@ public: bool dirty (void) const final { return _dirty; } void setDirty (bool dirty) final; bool containsItems (void) const final; - void managerVehicleChanged (Vehicle* managerVehicle) final; bool showPlanFromManagerVehicle (void) final; QmlObjectListModel* points (void) { return &_points; } @@ -62,23 +62,24 @@ signals: void loadComplete(void); private slots: - void _managerLoadComplete(void); - void _managerSendComplete(bool error); - void _managerRemoveAllComplete(bool error); - void _setFirstPointCurrent(void); - void _updateContainsItems(void); + void _managerLoadComplete (void); + void _managerSendComplete (bool error); + void _managerRemoveAllComplete (bool error); + void _setFirstPointCurrent (void); + void _updateContainsItems (void); + void _managerVehicleChanged (Vehicle* managerVehicle); private: - RallyPointManager* _rallyPointManager; - bool _dirty; + Vehicle* _managerVehicle = nullptr; + RallyPointManager* _rallyPointManager = nullptr; + bool _dirty = false; QmlObjectListModel _points; - QObject* _currentRallyPoint; - bool _itemsRequested; + QObject* _currentRallyPoint = nullptr; + bool _itemsRequested = false; - static const int _jsonCurrentVersion = 2; - - static const char* _jsonFileTypeValue; - static const char* _jsonPointsKey; + static const int _jsonCurrentVersion = 2; + static const char* _jsonFileTypeValue; + static const char* _jsonPointsKey; }; #endif diff --git a/src/MissionManager/Section.h b/src/MissionManager/Section.h index 40d0249c9..42c920d54 100644 --- a/src/MissionManager/Section.h +++ b/src/MissionManager/Section.h @@ -15,15 +15,17 @@ Q_DECLARE_LOGGING_CATEGORY(SectionLog) +class PlanMasterController; + // A Section encapsulates a set of mission commands which can be associated with another simple mission item. class Section : public QObject { Q_OBJECT public: - Section(Vehicle* vehicle, QObject* parent = nullptr) - : QObject(parent) - , _vehicle(vehicle) + Section(PlanMasterController* masterController, QObject* parent = nullptr) + : QObject (parent) + , _masterController (masterController) { } @@ -62,5 +64,5 @@ signals: void itemCountChanged (int itemCount); protected: - Vehicle* _vehicle; + PlanMasterController* _masterController = nullptr; }; diff --git a/src/MissionManager/SectionTest.cc b/src/MissionManager/SectionTest.cc index 676909079..2efe86478 100644 --- a/src/MissionManager/SectionTest.cc +++ b/src/MissionManager/SectionTest.cc @@ -37,7 +37,7 @@ void SectionTest::init(void) 70.1234567, true, // autoContinue false); // isCurrentItem - _simpleItem = new SimpleMissionItem(_offlineVehicle, false /* flyView */, missionItem, this); + _simpleItem = new SimpleMissionItem(_masterController, false /* flyView */, missionItem, this); } void SectionTest::cleanup(void) @@ -62,13 +62,13 @@ void SectionTest::_commonScanTest(Section* section) QmlObjectListModel waypointVisualItems; MissionItem waypointItem(0, MAV_CMD_NAV_WAYPOINT, MAV_FRAME_GLOBAL_RELATIVE_ALT, 0, 0, 0, 0, 0, 0, 0, true, false); - SimpleMissionItem simpleItem(_offlineVehicle, false /* flyView */, waypointItem, this); + SimpleMissionItem simpleItem(_masterController, false /* flyView */, waypointItem, this); waypointVisualItems.append(&simpleItem); waypointVisualItems.append(&simpleItem); waypointVisualItems.append(&simpleItem); QmlObjectListModel complexVisualItems; - SurveyComplexItem surveyItem(_offlineVehicle, false /* fly View */, QString() /* kmlFile */, this /* parent */); + SurveyComplexItem surveyItem(_masterController, false /* fly View */, QString() /* kmlFile */, this /* parent */); complexVisualItems.append(&surveyItem); // This tests the common cases which should not lead to scan succeess diff --git a/src/MissionManager/SimpleMissionItem.cc b/src/MissionManager/SimpleMissionItem.cc index a5b853144..16fd42259 100644 --- a/src/MissionManager/SimpleMissionItem.cc +++ b/src/MissionManager/SimpleMissionItem.cc @@ -19,6 +19,7 @@ #include "MissionCommandUIInfo.h" #include "QGroundControlQmlGlobal.h" #include "SettingsManager.h" +#include "PlanMasterController.h" FactMetaData* SimpleMissionItem::_altitudeMetaData = nullptr; FactMetaData* SimpleMissionItem::_commandMetaData = nullptr; @@ -51,16 +52,10 @@ static const struct EnumInfo_s _rgMavFrameInfo[] = { { "MAV_FRAME_GLOBAL_TERRAIN_ALT_INT", MAV_FRAME_GLOBAL_TERRAIN_ALT_INT }, }; -SimpleMissionItem::SimpleMissionItem(Vehicle* vehicle, bool flyView, QObject* parent) - : VisualMissionItem (vehicle, flyView, parent) - , _rawEdit (false) - , _dirty (false) - , _ignoreDirtyChangeSignals (false) - , _speedSection (nullptr) - , _cameraSection (nullptr) +SimpleMissionItem::SimpleMissionItem(PlanMasterController* masterController, bool flyView, QObject* parent) + : VisualMissionItem (masterController, flyView, parent) , _commandTree (qgcApp()->toolbox()->missionCommandTree()) , _supportedCommandFact (0, "Command:", FactMetaData::valueTypeUint32) - , _altitudeMode (QGroundControlQmlGlobal::AltitudeModeRelative) , _altitudeFact (0, "Altitude", FactMetaData::valueTypeDouble) , _amslAltAboveTerrainFact (0, "Alt above terrain", FactMetaData::valueTypeDouble) , _param1MetaData (FactMetaData::valueTypeDouble) @@ -70,7 +65,6 @@ SimpleMissionItem::SimpleMissionItem(Vehicle* vehicle, bool flyView, QObject* pa , _param5MetaData (FactMetaData::valueTypeDouble) , _param6MetaData (FactMetaData::valueTypeDouble) , _param7MetaData (FactMetaData::valueTypeDouble) - , _syncingHeadingDegreesAndParam4 (false) { _editorQml = QStringLiteral("qrc:/qml/SimpleItemEditor.qml"); @@ -84,14 +78,9 @@ SimpleMissionItem::SimpleMissionItem(Vehicle* vehicle, bool flyView, QObject* pa setDirty(false); } -SimpleMissionItem::SimpleMissionItem(Vehicle* vehicle, bool flyView, const MissionItem& missionItem, QObject* parent) - : VisualMissionItem (vehicle, flyView, parent) +SimpleMissionItem::SimpleMissionItem(PlanMasterController* masterController, bool flyView, const MissionItem& missionItem, QObject* parent) + : VisualMissionItem (masterController, flyView, parent) , _missionItem (missionItem) - , _rawEdit (false) - , _dirty (false) - , _ignoreDirtyChangeSignals (false) - , _speedSection (nullptr) - , _cameraSection (nullptr) , _commandTree (qgcApp()->toolbox()->missionCommandTree()) , _supportedCommandFact (0, "Command:", FactMetaData::valueTypeUint32) , _altitudeFact (0, "Altitude", FactMetaData::valueTypeDouble) @@ -103,7 +92,6 @@ SimpleMissionItem::SimpleMissionItem(Vehicle* vehicle, bool flyView, const Missi , _param5MetaData (FactMetaData::valueTypeDouble) , _param6MetaData (FactMetaData::valueTypeDouble) , _param7MetaData (FactMetaData::valueTypeDouble) - , _syncingHeadingDegreesAndParam4 (false) { _editorQml = QStringLiteral("qrc:/qml/SimpleItemEditor.qml"); @@ -203,9 +191,6 @@ void SimpleMissionItem::_connectSignals(void) // Propogate signals from MissionItem up to SimpleMissionItem connect(&_missionItem, &MissionItem::sequenceNumberChanged, this, &SimpleMissionItem::sequenceNumberChanged); connect(&_missionItem, &MissionItem::specifiedFlightSpeedChanged, this, &SimpleMissionItem::specifiedFlightSpeedChanged); - - // Firmware type change can affect supportsTerrainFrame - connect(_vehicle, &Vehicle::firmwareTypeChanged, this, &SimpleMissionItem::supportsTerrainFrameChanged); } void SimpleMissionItem::_setupMetaData(void) @@ -335,7 +320,7 @@ bool SimpleMissionItem::load(const QJsonObject& json, int sequenceNumber, QStrin bool SimpleMissionItem::isStandaloneCoordinate(void) const { - const MissionCommandUIInfo* uiInfo = _commandTree->getUIInfo(_vehicle, (MAV_CMD)command()); + const MissionCommandUIInfo* uiInfo = _commandTree->getUIInfo(_controllerVehicle, (MAV_CMD)command()); if (uiInfo) { return uiInfo->isStandaloneCoordinate(); } else { @@ -345,7 +330,7 @@ bool SimpleMissionItem::isStandaloneCoordinate(void) const bool SimpleMissionItem::specifiesCoordinate(void) const { - const MissionCommandUIInfo* uiInfo = _commandTree->getUIInfo(_vehicle, (MAV_CMD)command()); + const MissionCommandUIInfo* uiInfo = _commandTree->getUIInfo(_controllerVehicle, (MAV_CMD)command()); if (uiInfo) { return uiInfo->specifiesCoordinate(); } else { @@ -355,7 +340,7 @@ bool SimpleMissionItem::specifiesCoordinate(void) const bool SimpleMissionItem::specifiesAltitudeOnly(void) const { - const MissionCommandUIInfo* uiInfo = _commandTree->getUIInfo(_vehicle, (MAV_CMD)command()); + const MissionCommandUIInfo* uiInfo = _commandTree->getUIInfo(_controllerVehicle, (MAV_CMD)command()); if (uiInfo) { return uiInfo->specifiesAltitudeOnly(); } else { @@ -365,7 +350,7 @@ bool SimpleMissionItem::specifiesAltitudeOnly(void) const QString SimpleMissionItem::commandDescription(void) const { - const MissionCommandUIInfo* uiInfo = _commandTree->getUIInfo(_vehicle, (MAV_CMD)command()); + const MissionCommandUIInfo* uiInfo = _commandTree->getUIInfo(_controllerVehicle, (MAV_CMD)command()); if (uiInfo) { return uiInfo->description(); } else { @@ -376,7 +361,7 @@ QString SimpleMissionItem::commandDescription(void) const QString SimpleMissionItem::commandName(void) const { - const MissionCommandUIInfo* uiInfo = _commandTree->getUIInfo(_vehicle, (MAV_CMD)command()); + const MissionCommandUIInfo* uiInfo = _commandTree->getUIInfo(_controllerVehicle, (MAV_CMD)command()); if (uiInfo) { return uiInfo->friendlyName(); } else { @@ -446,7 +431,7 @@ void SimpleMissionItem::_rebuildTextFieldFacts(void) Fact* rgParamFacts[7] = { &_missionItem._param1Fact, &_missionItem._param2Fact, &_missionItem._param3Fact, &_missionItem._param4Fact, &_missionItem._param5Fact, &_missionItem._param6Fact, &_missionItem._param7Fact }; FactMetaData* rgParamMetaData[7] = { &_param1MetaData, &_param2MetaData, &_param3MetaData, &_param4MetaData, &_param5MetaData, &_param6MetaData, &_param7MetaData }; - const MissionCommandUIInfo* uiInfo = _commandTree->getUIInfo(_vehicle, command); + const MissionCommandUIInfo* uiInfo = _commandTree->getUIInfo(_controllerVehicle, command); for (int i=1; i<=7; i++) { bool showUI; @@ -485,7 +470,7 @@ void SimpleMissionItem::_rebuildNaNFacts(void) Fact* rgParamFacts[7] = { &_missionItem._param1Fact, &_missionItem._param2Fact, &_missionItem._param3Fact, &_missionItem._param4Fact, &_missionItem._param5Fact, &_missionItem._param6Fact, &_missionItem._param7Fact }; FactMetaData* rgParamMetaData[7] = { &_param1MetaData, &_param2MetaData, &_param3MetaData, &_param4MetaData, &_param5MetaData, &_param6MetaData, &_param7MetaData }; - const MissionCommandUIInfo* uiInfo = _commandTree->getUIInfo(_vehicle, command); + const MissionCommandUIInfo* uiInfo = _commandTree->getUIInfo(_controllerVehicle, command); for (int i=1; i<=7; i++) { bool showUI; @@ -493,10 +478,10 @@ void SimpleMissionItem::_rebuildNaNFacts(void) if (showUI && paramInfo && paramInfo->nanUnchanged()) { // Show hide Heading field on waypoint based on vehicle yaw to next waypoint setting. This needs to come from the actual vehicle if it exists - // and not _vehicle which is always offline. + // and not _controllerVehicle which is always offline. Vehicle* firmwareVehicle = qgcApp()->toolbox()->multiVehicleManager()->activeVehicle(); if (!firmwareVehicle) { - firmwareVehicle = _vehicle; + firmwareVehicle = _controllerVehicle; } bool hideWaypointHeading = (command == MAV_CMD_NAV_WAYPOINT || command == MAV_CMD_NAV_TAKEOFF) && (i == 4) && firmwareVehicle->firmwarePlugin()->vehicleYawsToNextWaypointInMission(firmwareVehicle); if (hideWaypointHeading) { @@ -543,7 +528,7 @@ void SimpleMissionItem::_rebuildComboBoxFacts(void) for (int i=1; i<=7; i++) { bool showUI; - const MissionCmdParamInfo* paramInfo = _commandTree->getUIInfo(_vehicle, command)->getParamInfo(i, showUI); + const MissionCmdParamInfo* paramInfo = _commandTree->getUIInfo(_controllerVehicle, command)->getParamInfo(i, showUI); if (showUI && paramInfo && paramInfo->enumStrings().count() != 0) { Fact* paramFact = rgParamFacts[i-1]; @@ -569,7 +554,7 @@ void SimpleMissionItem::_rebuildFacts(void) bool SimpleMissionItem::friendlyEditAllowed(void) const { - const MissionCommandUIInfo* uiInfo = _commandTree->getUIInfo(_vehicle, static_cast(command())); + const MissionCommandUIInfo* uiInfo = _commandTree->getUIInfo(_controllerVehicle, static_cast(command())); if (uiInfo && uiInfo->friendlyEdit()) { if (!_missionItem.autoContinue()) { return false; @@ -580,10 +565,8 @@ bool SimpleMissionItem::friendlyEditAllowed(void) const switch (frame) { case MAV_FRAME_GLOBAL: case MAV_FRAME_GLOBAL_RELATIVE_ALT: - return true; - case MAV_FRAME_GLOBAL_TERRAIN_ALT: - return supportsTerrainFrame(); + return true; default: return false; } @@ -740,7 +723,7 @@ void SimpleMissionItem::_setDefaultsForCommand(void) } MAV_CMD command = static_cast(this->command()); - const MissionCommandUIInfo* uiInfo = _commandTree->getUIInfo(_vehicle, command); + const MissionCommandUIInfo* uiInfo = _commandTree->getUIInfo(_controllerVehicle, command); if (uiInfo) { for (int i=1; i<=7; i++) { bool showUI; @@ -786,7 +769,7 @@ void SimpleMissionItem::_sendFriendlyEditAllowedChanged(void) QString SimpleMissionItem::category(void) const { - return _commandTree->getUIInfo(_vehicle, static_cast(command()))->category(); + return _commandTree->getUIInfo(_controllerVehicle, static_cast(command()))->category(); } void SimpleMissionItem::setCommand(int command) @@ -847,12 +830,10 @@ void SimpleMissionItem::_possibleVehicleYawChanged(void) } } -bool SimpleMissionItem::scanForSections(QmlObjectListModel* visualItems, int scanIndex, Vehicle* vehicle) +bool SimpleMissionItem::scanForSections(QmlObjectListModel* visualItems, int scanIndex, PlanMasterController* /*masterController*/) { bool sectionFound = false; - Q_UNUSED(vehicle); - if (_cameraSection->available()) { sectionFound |= _cameraSection->scanForSection(visualItems, scanIndex); } @@ -877,8 +858,8 @@ void SimpleMissionItem::_updateOptionalSections(void) // Add new sections - _cameraSection = new CameraSection(_vehicle, this); - _speedSection = new SpeedSection(_vehicle, this); + _cameraSection = new CameraSection(_masterController, this); + _speedSection = new SpeedSection(_masterController, this); if (static_cast(command()) == MAV_CMD_NAV_WAYPOINT) { _cameraSection->setAvailable(true); _speedSection->setAvailable(true); @@ -931,7 +912,7 @@ void SimpleMissionItem::appendMissionItems(QList& items, QObject* void SimpleMissionItem::applyNewAltitude(double newAltitude) { MAV_CMD command = static_cast(this->command()); - const MissionCommandUIInfo* uiInfo = _commandTree->getUIInfo(_vehicle, command); + const MissionCommandUIInfo* uiInfo = _commandTree->getUIInfo(_controllerVehicle, command); if (uiInfo->specifiesCoordinate() || uiInfo->specifiesAltitudeOnly()) { switch (static_cast(this->command())) { diff --git a/src/MissionManager/SimpleMissionItem.h b/src/MissionManager/SimpleMissionItem.h index ff065dca5..3ca54977d 100644 --- a/src/MissionManager/SimpleMissionItem.h +++ b/src/MissionManager/SimpleMissionItem.h @@ -24,8 +24,8 @@ class SimpleMissionItem : public VisualMissionItem Q_OBJECT public: - SimpleMissionItem(Vehicle* vehicle, bool flyView, QObject* parent); - SimpleMissionItem(Vehicle* vehicle, bool flyView, const MissionItem& missionItem, QObject* parent); + SimpleMissionItem(PlanMasterController* masterController, bool flyView, QObject* parent); + SimpleMissionItem(PlanMasterController* masterController, bool flyView, const MissionItem& missionItem, QObject* parent); //SimpleMissionItem(const SimpleMissionItem& other, bool flyView, QObject* parent); ~SimpleMissionItem(); @@ -38,7 +38,6 @@ public: Q_PROPERTY(QGroundControlQmlGlobal::AltitudeMode altitudeMode READ altitudeMode WRITE setAltitudeMode NOTIFY altitudeModeChanged) Q_PROPERTY(Fact* amslAltAboveTerrain READ amslAltAboveTerrain CONSTANT) ///< Actual AMSL altitude for item if altitudeMode == AltitudeAboveTerrain Q_PROPERTY(int command READ command WRITE setCommand NOTIFY commandChanged) - Q_PROPERTY(bool supportsTerrainFrame READ supportsTerrainFrame NOTIFY supportsTerrainFrameChanged) /// Optional sections Q_PROPERTY(QObject* speedSection READ speedSection NOTIFY speedSectionChanged) @@ -59,7 +58,7 @@ public: /// @param scanIndex Index to start scanning from /// @param vehicle Vehicle associated with this mission /// @return true: section found, scanIndex updated - bool scanForSections(QmlObjectListModel* visualItems, int scanIndex, Vehicle* vehicle); + bool scanForSections(QmlObjectListModel* visualItems, int scanIndex, PlanMasterController* masterController); // Property accesors @@ -72,7 +71,6 @@ public: QGroundControlQmlGlobal::AltitudeMode altitudeMode(void) const { return _altitudeMode; } Fact* altitude (void) { return &_altitudeFact; } Fact* amslAltAboveTerrain (void) { return &_amslAltAboveTerrainFact; } - bool supportsTerrainFrame(void) const { return _vehicle->supportsTerrainFrame(); } CameraSection* cameraSection (void) { return _cameraSection; } SpeedSection* speedSection (void) { return _speedSection; } @@ -140,7 +138,6 @@ signals: void cameraSectionChanged (QObject* cameraSection); void speedSectionChanged (QObject* cameraSection); void altitudeModeChanged (void); - void supportsTerrainFrameChanged(void); private slots: void _setDirty (void); @@ -166,19 +163,19 @@ private: void _rebuildComboBoxFacts (void); MissionItem _missionItem; - bool _rawEdit; - bool _dirty; - bool _ignoreDirtyChangeSignals; + bool _rawEdit = false; + bool _dirty = false; + bool _ignoreDirtyChangeSignals = false; QGeoCoordinate _mapCenterHint; + SpeedSection* _speedSection = nullptr; + CameraSection* _cameraSection = nullptr; - SpeedSection* _speedSection; - CameraSection* _cameraSection; - - MissionCommandTree* _commandTree; + MissionCommandTree* _commandTree = nullptr; + bool _syncingHeadingDegreesAndParam4 = false; ///< true: already in a sync signal, prevents signal loop Fact _supportedCommandFact; - QGroundControlQmlGlobal::AltitudeMode _altitudeMode; + QGroundControlQmlGlobal::AltitudeMode _altitudeMode = QGroundControlQmlGlobal::AltitudeModeRelative; Fact _altitudeFact; Fact _amslAltAboveTerrainFact; @@ -201,8 +198,6 @@ private: FactMetaData _param6MetaData; FactMetaData _param7MetaData; - bool _syncingHeadingDegreesAndParam4; ///< true: already in a sync signal, prevents signal loop - static const char* _jsonAltitudeModeKey; static const char* _jsonAltitudeKey; static const char* _jsonAMSLAltAboveTerrainKey; diff --git a/src/MissionManager/SimpleMissionItemTest.cc b/src/MissionManager/SimpleMissionItemTest.cc index a5b023d77..caa8c5204 100644 --- a/src/MissionManager/SimpleMissionItemTest.cc +++ b/src/MissionManager/SimpleMissionItemTest.cc @@ -12,6 +12,7 @@ #include "QGCApplication.h" #include "QGroundControlQmlGlobal.h" #include "SettingsManager.h" +#include "PlanMasterController.h" const SimpleMissionItemTest::ItemInfo_t SimpleMissionItemTest::_rgItemInfo[] = { { MAV_CMD_NAV_WAYPOINT, MAV_FRAME_GLOBAL_RELATIVE_ALT }, @@ -92,7 +93,7 @@ void SimpleMissionItemTest::init(void) 70.1234567, true, // autoContinue false); // isCurrentItem - _simpleItem = new SimpleMissionItem(_offlineVehicle, false /* flyView */, missionItem, this); + _simpleItem = new SimpleMissionItem(_masterController, false /* flyView */, missionItem, this); // It's important top check that the right signals are emitted at the right time since that drives ui change. // It's also important to check that things are not being over-signalled when they should not be, since that can lead @@ -111,7 +112,7 @@ void SimpleMissionItemTest::cleanup(void) void SimpleMissionItemTest::_testEditorFacts(void) { - Vehicle* vehicle = new Vehicle(MAV_AUTOPILOT_PX4, MAV_TYPE_FIXED_WING, qgcApp()->toolbox()->firmwarePluginManager()); + PlanMasterController fwMasterController(MAV_AUTOPILOT_PX4, MAV_TYPE_FIXED_WING); for (size_t i=0; irawValue().toDouble(), expected->altValue); } } - - delete vehicle; } void SimpleMissionItemTest::_testDefaultValues(void) { - SimpleMissionItem item(_offlineVehicle, false /* flyView */, nullptr); + SimpleMissionItem item(_masterController, false /* flyView */, nullptr); item.missionItem().setCommand(MAV_CMD_NAV_WAYPOINT); item.missionItem().setFrame(MAV_FRAME_GLOBAL_RELATIVE_ALT); diff --git a/src/MissionManager/SpeedSection.cc b/src/MissionManager/SpeedSection.cc index ec47ed86d..945d289ff 100644 --- a/src/MissionManager/SpeedSection.cc +++ b/src/MissionManager/SpeedSection.cc @@ -11,13 +11,14 @@ #include "JsonHelper.h" #include "FirmwarePlugin.h" #include "SimpleMissionItem.h" +#include "PlanMasterController.h" const char* SpeedSection::_flightSpeedName = "FlightSpeed"; QMap SpeedSection::_metaDataMap; -SpeedSection::SpeedSection(Vehicle* vehicle, QObject* parent) - : Section (vehicle, parent) +SpeedSection::SpeedSection(PlanMasterController* masterController, QObject* parent) + : Section (masterController, parent) , _available (false) , _dirty (false) , _specifyFlightSpeed (false) @@ -28,10 +29,10 @@ SpeedSection::SpeedSection(Vehicle* vehicle, QObject* parent) } double flightSpeed = 0; - if (_vehicle->multiRotor()) { - flightSpeed = _vehicle->defaultHoverSpeed(); + if (_masterController->controllerVehicle()->multiRotor()) { + flightSpeed = _masterController->controllerVehicle()->defaultHoverSpeed(); } else { - flightSpeed = _vehicle->defaultCruiseSpeed(); + flightSpeed = _masterController->controllerVehicle()->defaultCruiseSpeed(); } _metaDataMap[_flightSpeedName]->setRawDefaultValue(flightSpeed); @@ -53,7 +54,7 @@ bool SpeedSection::settingsSpecified(void) const void SpeedSection::setAvailable(bool available) { if (available != _available) { - if (available && (_vehicle->multiRotor() || _vehicle->fixedWing())) { + if (available && (_masterController->controllerVehicle()->multiRotor() || _masterController->controllerVehicle()->fixedWing())) { _available = available; emit availableChanged(available); } @@ -91,7 +92,7 @@ void SpeedSection::appendSectionItems(QList& items, QObject* missi MissionItem* item = new MissionItem(seqNum++, MAV_CMD_DO_CHANGE_SPEED, MAV_FRAME_MISSION, - _vehicle->multiRotor() ? 1 /* groundspeed */ : 0 /* airspeed */, // Change airspeed or groundspeed + _masterController->controllerVehicle()->multiRotor() ? 1 /* groundspeed */ : 0 /* airspeed */, // Change airspeed or groundspeed _flightSpeedFact.rawValue().toDouble(), -1, // No throttle change 0, // Absolute speed change @@ -119,9 +120,9 @@ bool SpeedSection::scanForSection(QmlObjectListModel* visualItems, int scanIndex // See SpeedSection::appendMissionItems for specs on what consitutes a known speed setting if (missionItem.command() == MAV_CMD_DO_CHANGE_SPEED && missionItem.param3() == -1 && missionItem.param4() == 0 && missionItem.param5() == 0 && missionItem.param6() == 0 && missionItem.param7() == 0) { - if (_vehicle->multiRotor() && missionItem.param1() != 1) { + if (_masterController->controllerVehicle()->multiRotor() && missionItem.param1() != 1) { return false; - } else if (_vehicle->fixedWing() && missionItem.param1() != 0) { + } else if (_masterController->controllerVehicle()->fixedWing() && missionItem.param1() != 0) { return false; } visualItems->removeAt(scanIndex)->deleteLater(); diff --git a/src/MissionManager/SpeedSection.h b/src/MissionManager/SpeedSection.h index 176419854..128248f29 100644 --- a/src/MissionManager/SpeedSection.h +++ b/src/MissionManager/SpeedSection.h @@ -13,12 +13,14 @@ #include "FactSystem.h" #include "QmlObjectListModel.h" +class PlanMasterController; + class SpeedSection : public Section { Q_OBJECT public: - SpeedSection(Vehicle* vehicle, QObject* parent = nullptr); + SpeedSection(PlanMasterController* masterController, QObject* parent = nullptr); Q_PROPERTY(bool specifyFlightSpeed READ specifyFlightSpeed WRITE setSpecifyFlightSpeed NOTIFY specifyFlightSpeedChanged) Q_PROPERTY(Fact* flightSpeed READ flightSpeed CONSTANT) diff --git a/src/MissionManager/SpeedSectionTest.cc b/src/MissionManager/SpeedSectionTest.cc index 92b0bbf72..b7a327618 100644 --- a/src/MissionManager/SpeedSectionTest.cc +++ b/src/MissionManager/SpeedSectionTest.cc @@ -8,6 +8,7 @@ ****************************************************************************/ #include "SpeedSectionTest.h" +#include "PlanMasterController.h" SpeedSectionTest::SpeedSectionTest(void) : _spySpeed(nullptr) @@ -134,7 +135,7 @@ void SpeedSectionTest::_checkAvailable(void) 70.1234567, true, // autoContinue false); // isCurrentItem - SimpleMissionItem* item = new SimpleMissionItem(_offlineVehicle, false /* flyView */, missionItem, this); + SimpleMissionItem* item = new SimpleMissionItem(_masterController, false /* flyView */, missionItem, this); QVERIFY(item->speedSection()); QCOMPARE(item->speedSection()->available(), false); } @@ -176,7 +177,7 @@ void SpeedSectionTest::_testAppendSectionItems(void) _speedSection->appendSectionItems(rgMissionItems, this, seqNum); QCOMPARE(rgMissionItems.count(), 1); QCOMPARE(seqNum, 1); - MissionItem expectedSpeedItem(0, MAV_CMD_DO_CHANGE_SPEED, MAV_FRAME_MISSION, _offlineVehicle->multiRotor() ? 1 : 0, _speedSection->flightSpeed()->rawValue().toDouble(), -1, 0, 0, 0, 0, true, false); + MissionItem expectedSpeedItem(0, MAV_CMD_DO_CHANGE_SPEED, MAV_FRAME_MISSION, _controllerVehicle->multiRotor() ? 1 : 0, _speedSection->flightSpeed()->rawValue().toDouble(), -1, 0, 0, 0, 0, true, false); _missionItemsEqual(*rgMissionItems[0], expectedSpeedItem); } @@ -192,8 +193,8 @@ void SpeedSectionTest::_testScanForSection(void) // Check for a scan success double flightSpeed = 10.123456; - MissionItem validSpeedItem(0, MAV_CMD_DO_CHANGE_SPEED, MAV_FRAME_MISSION, _offlineVehicle->multiRotor() ? 1 : 0, flightSpeed, -1, 0, 0, 0, 0, true, false); - SimpleMissionItem simpleItem(_offlineVehicle, false /* flyView */, validSpeedItem, nullptr); + MissionItem validSpeedItem(0, MAV_CMD_DO_CHANGE_SPEED, MAV_FRAME_MISSION, _controllerVehicle->multiRotor() ? 1 : 0, flightSpeed, -1, 0, 0, 0, 0, true, false); + SimpleMissionItem simpleItem(_masterController, false /* flyView */, validSpeedItem, nullptr); MissionItem& simpleMissionItem = simpleItem.missionItem(); visualItems.append(&simpleItem); scanIndex = 0; @@ -209,7 +210,7 @@ void SpeedSectionTest::_testScanForSection(void) // Flight speed command but incorrect settings simpleMissionItem = validSpeedItem; - simpleMissionItem.setParam1(_offlineVehicle->multiRotor() ? 0 : 1); + simpleMissionItem.setParam1(_controllerVehicle->multiRotor() ? 0 : 1); visualItems.append(&simpleItem); QCOMPARE(_speedSection->scanForSection(&visualItems, scanIndex), false); QCOMPARE(visualItems.count(), 1); @@ -264,7 +265,7 @@ void SpeedSectionTest::_testScanForSection(void) // Valid item in wrong position MissionItem waypointMissionItem(0, MAV_CMD_NAV_WAYPOINT, MAV_FRAME_GLOBAL_RELATIVE_ALT, 0, 0, 0, 0, 0, 0, 0, true, false); - SimpleMissionItem simpleWaypointItem(_offlineVehicle, false /* flyView */, waypointMissionItem, nullptr); + SimpleMissionItem simpleWaypointItem(_masterController, false /* flyView */, waypointMissionItem, nullptr); simpleMissionItem = validSpeedItem; visualItems.append(&simpleWaypointItem); visualItems.append(&simpleMissionItem); diff --git a/src/MissionManager/StructureScanComplexItem.cc b/src/MissionManager/StructureScanComplexItem.cc index e56fee065..187442f5e 100644 --- a/src/MissionManager/StructureScanComplexItem.cc +++ b/src/MissionManager/StructureScanComplexItem.cc @@ -16,6 +16,7 @@ #include "SettingsManager.h" #include "AppSettings.h" #include "QGCQGeoCoordinate.h" +#include "PlanMasterController.h" #include @@ -32,15 +33,15 @@ const char* StructureScanComplexItem::startFromTopName = "StartFromTo const char* StructureScanComplexItem::jsonComplexItemTypeValue = "StructureScan"; const char* StructureScanComplexItem::_jsonCameraCalcKey = "CameraCalc"; -StructureScanComplexItem::StructureScanComplexItem(Vehicle* vehicle, bool flyView, const QString& kmlOrShpFile, QObject* parent) - : ComplexMissionItem (vehicle, flyView, parent) +StructureScanComplexItem::StructureScanComplexItem(PlanMasterController* masterController, bool flyView, const QString& kmlOrShpFile, QObject* parent) + : ComplexMissionItem (masterController, flyView, parent) , _metaDataMap (FactMetaData::createMapFromJsonFile(QStringLiteral(":/json/StructureScan.SettingsGroup.json"), this /* QObject parent */)) , _sequenceNumber (0) , _entryVertex (0) , _ignoreRecalc (false) , _scanDistance (0.0) , _cameraShots (0) - , _cameraCalc (vehicle, settingsGroup) + , _cameraCalc (masterController, settingsGroup) , _scanBottomAltFact (settingsGroup, _metaDataMap[scanBottomAltName]) , _structureHeightFact (settingsGroup, _metaDataMap[structureHeightName]) , _layersFact (settingsGroup, _metaDataMap[layersName]) diff --git a/src/MissionManager/StructureScanComplexItem.h b/src/MissionManager/StructureScanComplexItem.h index c57c0777d..2060eb853 100644 --- a/src/MissionManager/StructureScanComplexItem.h +++ b/src/MissionManager/StructureScanComplexItem.h @@ -20,15 +20,16 @@ Q_DECLARE_LOGGING_CATEGORY(StructureScanComplexItemLog) +class PlanMasterController; + class StructureScanComplexItem : public ComplexMissionItem { Q_OBJECT public: - /// @param vehicle Vehicle which this is being contructed for /// @param flyView true: Created for use in the Fly View, false: Created for use in the Plan View /// @param kmlOrSHPFile Polygon comes from this file, empty for default polygon - StructureScanComplexItem(Vehicle* vehicle, bool flyView, const QString& kmlOrSHPFile, QObject* parent); + StructureScanComplexItem(PlanMasterController* masterController, bool flyView, const QString& kmlOrSHPFile, QObject* parent); Q_PROPERTY(CameraCalc* cameraCalc READ cameraCalc CONSTANT) Q_PROPERTY(Fact* entranceAlt READ entranceAlt CONSTANT) diff --git a/src/MissionManager/StructureScanComplexItemTest.cc b/src/MissionManager/StructureScanComplexItemTest.cc index 7a3d4dd86..6cc31e91e 100644 --- a/src/MissionManager/StructureScanComplexItemTest.cc +++ b/src/MissionManager/StructureScanComplexItemTest.cc @@ -9,9 +9,9 @@ #include "StructureScanComplexItemTest.h" #include "QGCApplication.h" +#include "PlanMasterController.h" StructureScanComplexItemTest::StructureScanComplexItemTest(void) - : _offlineVehicle(nullptr) { _polyPoints << QGeoCoordinate(47.633550640000003, -122.08982199) << QGeoCoordinate(47.634129020000003, -122.08887249) << QGeoCoordinate(47.633619320000001, -122.08811074) << QGeoCoordinate(47.633189139999999, -122.08900124); @@ -23,8 +23,9 @@ void StructureScanComplexItemTest::init(void) _rgSignals[dirtyChangedIndex] = SIGNAL(dirtyChanged(bool)); - _offlineVehicle = new Vehicle(MAV_AUTOPILOT_PX4, MAV_TYPE_QUADROTOR, qgcApp()->toolbox()->firmwarePluginManager(), this); - _structureScanItem = new StructureScanComplexItem(_offlineVehicle, false /* flyView */, QString() /* kmlFile */, this /* parent */); + _masterController = new PlanMasterController(this); + _controllerVehicle = _masterController->controllerVehicle(); + _structureScanItem = new StructureScanComplexItem(new PlanMasterController(this), false /* flyView */, QString() /* kmlFile */, this /* parent */); _structureScanItem->setDirty(false); _multiSpy = new MultiSignalSpy(); @@ -35,7 +36,6 @@ void StructureScanComplexItemTest::init(void) void StructureScanComplexItemTest::cleanup(void) { delete _structureScanItem; - delete _offlineVehicle; } void StructureScanComplexItemTest::_testDirty(void) @@ -114,7 +114,7 @@ void StructureScanComplexItemTest::_testSaveLoad(void) _structureScanItem->save(items); QString errorString; - StructureScanComplexItem* newItem = new StructureScanComplexItem(_offlineVehicle, false /* flyView */, QString() /* kmlFile */, this /* parent */); + StructureScanComplexItem* newItem = new StructureScanComplexItem(new PlanMasterController(this), false /* flyView */, QString() /* kmlFile */, this /* parent */); QVERIFY(newItem->load(items[0].toObject(), 10, errorString)); QVERIFY(errorString.isEmpty()); _validateItem(newItem); diff --git a/src/MissionManager/StructureScanComplexItemTest.h b/src/MissionManager/StructureScanComplexItemTest.h index 965cc1074..276e4af82 100644 --- a/src/MissionManager/StructureScanComplexItemTest.h +++ b/src/MissionManager/StructureScanComplexItemTest.h @@ -12,6 +12,7 @@ #include "UnitTest.h" #include "MultiSignalSpy.h" #include "StructureScanComplexItem.h" +#include "PlanMasterController.h" class StructureScanComplexItemTest : public UnitTest { @@ -45,8 +46,9 @@ private: static const size_t _cSignals = maxSignalIndex; const char* _rgSignals[_cSignals]; - Vehicle* _offlineVehicle; - MultiSignalSpy* _multiSpy; - StructureScanComplexItem* _structureScanItem; + PlanMasterController* _masterController = nullptr; + Vehicle* _controllerVehicle = nullptr; + MultiSignalSpy* _multiSpy = nullptr; + StructureScanComplexItem* _structureScanItem = nullptr; QList _polyPoints; }; diff --git a/src/MissionManager/SurveyComplexItem.cc b/src/MissionManager/SurveyComplexItem.cc index f54a8f432..82815ce96 100644 --- a/src/MissionManager/SurveyComplexItem.cc +++ b/src/MissionManager/SurveyComplexItem.cc @@ -16,6 +16,7 @@ #include "QGCQGeoCoordinate.h" #include "SettingsManager.h" #include "AppSettings.h" +#include "PlanMasterController.h" #include @@ -61,8 +62,8 @@ const char* SurveyComplexItem::_jsonV3Refly90DegreesKey = "refly90 const char* SurveyComplexItem::_jsonFlyAlternateTransectsKey = "flyAlternateTransects"; const char* SurveyComplexItem::_jsonSplitConcavePolygonsKey = "splitConcavePolygons"; -SurveyComplexItem::SurveyComplexItem(Vehicle* vehicle, bool flyView, const QString& kmlOrShpFile, QObject* parent) - : TransectStyleComplexItem (vehicle, flyView, settingsGroup, parent) +SurveyComplexItem::SurveyComplexItem(PlanMasterController* masterController, bool flyView, const QString& kmlOrShpFile, QObject* parent) + : TransectStyleComplexItem (masterController, flyView, settingsGroup, parent) , _metaDataMap (FactMetaData::createMapFromJsonFile(QStringLiteral(":/json/Survey.SettingsGroup.json"), this)) , _gridAngleFact (settingsGroup, _metaDataMap[gridAngleName]) , _flyAlternateTransectsFact(settingsGroup, _metaDataMap[flyAlternateTransectsName]) @@ -73,7 +74,7 @@ SurveyComplexItem::SurveyComplexItem(Vehicle* vehicle, bool flyView, const QStri // If the user hasn't changed turnaround from the default (which is a fixed wing default) and we are multi-rotor set the multi-rotor default. // NULL check since object creation during unit testing passes NULL for vehicle - if (_vehicle && _vehicle->multiRotor() && _turnAroundDistanceFact.rawValue().toDouble() == _turnAroundDistanceFact.rawDefaultValue().toDouble()) { + if (_controllerVehicle && _controllerVehicle->multiRotor() && _turnAroundDistanceFact.rawValue().toDouble() == _turnAroundDistanceFact.rawDefaultValue().toDouble()) { // Note this is set to 10 meters to work around a problem with PX4 Pro turnaround behavior. Don't change unless firmware gets better as well. _turnAroundDistanceFact.setRawValue(10); } diff --git a/src/MissionManager/SurveyComplexItem.h b/src/MissionManager/SurveyComplexItem.h index 466222e7c..b2d59aece 100644 --- a/src/MissionManager/SurveyComplexItem.h +++ b/src/MissionManager/SurveyComplexItem.h @@ -16,15 +16,16 @@ Q_DECLARE_LOGGING_CATEGORY(SurveyComplexItemLog) +class PlanMasterController; + class SurveyComplexItem : public TransectStyleComplexItem { Q_OBJECT public: - /// @param vehicle Vehicle which this is being contructed for /// @param flyView true: Created for use in the Fly View, false: Created for use in the Plan View /// @param kmlOrShpFile Polygon comes from this file, empty for default polygon - SurveyComplexItem(Vehicle* vehicle, bool flyView, const QString& kmlOrShpFile, QObject* parent); + SurveyComplexItem(PlanMasterController* masterController, bool flyView, const QString& kmlOrShpFile, QObject* parent); Q_PROPERTY(Fact* gridAngle READ gridAngle CONSTANT) Q_PROPERTY(Fact* flyAlternateTransects READ flyAlternateTransects CONSTANT) diff --git a/src/MissionManager/SurveyComplexItemTest.cc b/src/MissionManager/SurveyComplexItemTest.cc index f0b3eb947..f812d7bcb 100644 --- a/src/MissionManager/SurveyComplexItemTest.cc +++ b/src/MissionManager/SurveyComplexItemTest.cc @@ -11,7 +11,6 @@ #include "QGCApplication.h" SurveyComplexItemTest::SurveyComplexItemTest(void) - : _offlineVehicle(nullptr) { _polyPoints << QGeoCoordinate(47.633550640000003, -122.08982199) << QGeoCoordinate(47.634129020000003, -122.08887249) << QGeoCoordinate(47.633619320000001, -122.08811074) << QGeoCoordinate(47.633189139999999, -122.08900124); @@ -28,8 +27,9 @@ void SurveyComplexItemTest::init(void) _rgSurveySignals[surveyRefly90DegreesChangedIndex] = SIGNAL(refly90DegreesChanged(bool)); _rgSurveySignals[surveyDirtyChangedIndex] = SIGNAL(dirtyChanged(bool)); - _offlineVehicle = new Vehicle(MAV_AUTOPILOT_PX4, MAV_TYPE_QUADROTOR, qgcApp()->toolbox()->firmwarePluginManager(), this); - _surveyItem = new SurveyComplexItem(_offlineVehicle, false /* flyView */, QString() /* kmlFile */, this /* parent */); + _masterController = new PlanMasterController(this); + _controllerVehicle = _masterController->controllerVehicle(); + _surveyItem = new SurveyComplexItem(_masterController, false /* flyView */, QString() /* kmlFile */, this /* parent */); _surveyItem->turnAroundDistance()->setRawValue(0); // Unit test written for no turnaround distance _surveyItem->setDirty(false); _mapPolygon = _surveyItem->surveyAreaPolygon(); @@ -46,7 +46,6 @@ void SurveyComplexItemTest::init(void) void SurveyComplexItemTest::cleanup(void) { delete _surveyItem; - delete _offlineVehicle; delete _multiSpy; } diff --git a/src/MissionManager/SurveyComplexItemTest.h b/src/MissionManager/SurveyComplexItemTest.h index 8d51ebd97..b2a3728e0 100644 --- a/src/MissionManager/SurveyComplexItemTest.h +++ b/src/MissionManager/SurveyComplexItemTest.h @@ -13,6 +13,7 @@ #include "TCPLink.h" #include "MultiSignalSpy.h" #include "SurveyComplexItem.h" +#include "PlanMasterController.h" #include @@ -63,9 +64,10 @@ private: static const size_t _cSurveySignals = surveyMaxSignalIndex; const char* _rgSurveySignals[_cSurveySignals]; - Vehicle* _offlineVehicle; - MultiSignalSpy* _multiSpy; - SurveyComplexItem* _surveyItem; - QGCMapPolygon* _mapPolygon; + PlanMasterController* _masterController = nullptr; + Vehicle* _controllerVehicle = nullptr; + MultiSignalSpy* _multiSpy = nullptr; + SurveyComplexItem* _surveyItem = nullptr; + QGCMapPolygon* _mapPolygon = nullptr; QList _polyPoints; }; diff --git a/src/MissionManager/TakeoffMissionItem.cc b/src/MissionManager/TakeoffMissionItem.cc index a03306ed8..08400bf9e 100644 --- a/src/MissionManager/TakeoffMissionItem.cc +++ b/src/MissionManager/TakeoffMissionItem.cc @@ -18,24 +18,25 @@ #include "MissionCommandUIInfo.h" #include "QGroundControlQmlGlobal.h" #include "SettingsManager.h" +#include "PlanMasterController.h" -TakeoffMissionItem::TakeoffMissionItem(Vehicle* vehicle, bool flyView, MissionSettingsItem* settingsItem, QObject* parent) - : SimpleMissionItem (vehicle, flyView, parent) +TakeoffMissionItem::TakeoffMissionItem(PlanMasterController* masterController, bool flyView, MissionSettingsItem* settingsItem, QObject* parent) + : SimpleMissionItem (masterController, flyView, parent) , _settingsItem (settingsItem) { _init(); } -TakeoffMissionItem::TakeoffMissionItem(MAV_CMD takeoffCmd, Vehicle* vehicle, bool flyView, MissionSettingsItem* settingsItem, QObject* parent) - : SimpleMissionItem (vehicle, flyView, parent) +TakeoffMissionItem::TakeoffMissionItem(MAV_CMD takeoffCmd, PlanMasterController* masterController, bool flyView, MissionSettingsItem* settingsItem, QObject* parent) + : SimpleMissionItem (masterController, flyView, parent) , _settingsItem (settingsItem) { setCommand(takeoffCmd); _init(); } -TakeoffMissionItem::TakeoffMissionItem(const MissionItem& missionItem, Vehicle* vehicle, bool flyView, MissionSettingsItem* settingsItem, QObject* parent) - : SimpleMissionItem (vehicle, flyView, missionItem, parent) +TakeoffMissionItem::TakeoffMissionItem(const MissionItem& missionItem, PlanMasterController* masterController, bool flyView, MissionSettingsItem* settingsItem, QObject* parent) + : SimpleMissionItem (masterController, flyView, missionItem, parent) , _settingsItem (settingsItem) { _init(); @@ -115,7 +116,7 @@ bool TakeoffMissionItem::isTakeoffCommand(MAV_CMD command) void TakeoffMissionItem::_initLaunchTakeoffAtSameLocation(void) { if (specifiesCoordinate()) { - if (_vehicle->fixedWing() || _vehicle->vtol()) { + if (_controllerVehicle->fixedWing() || _controllerVehicle->vtol()) { setLaunchTakeoffAtSameLocation(false); } else { // PX4 specifies a coordinate for takeoff even for non fixed wing. But it makes more sense to not have a coordinate diff --git a/src/MissionManager/TakeoffMissionItem.h b/src/MissionManager/TakeoffMissionItem.h index d4f3ce92f..13a1cb654 100644 --- a/src/MissionManager/TakeoffMissionItem.h +++ b/src/MissionManager/TakeoffMissionItem.h @@ -12,6 +12,8 @@ #include "SimpleMissionItem.h" #include "MissionSettingsItem.h" +class PlanMasterController; + /// Takeoff mission item is a special case of a SimpleMissionItem which supports Launch Location display/editing /// which is tied to home position. class TakeoffMissionItem : public SimpleMissionItem @@ -19,9 +21,9 @@ class TakeoffMissionItem : public SimpleMissionItem Q_OBJECT public: - TakeoffMissionItem(Vehicle* vehicle, bool flyView, MissionSettingsItem* settingsItem, QObject* parent); - TakeoffMissionItem(MAV_CMD takeoffCmd, Vehicle* vehicle, bool flyView, MissionSettingsItem* settingsItem, QObject* parent); - TakeoffMissionItem(const MissionItem& missionItem, Vehicle* vehicle, bool flyView, MissionSettingsItem* settingsItem, QObject* parent); + TakeoffMissionItem(PlanMasterController* masterController, bool flyView, MissionSettingsItem* settingsItem, QObject* parent); + TakeoffMissionItem(MAV_CMD takeoffCmd, PlanMasterController* masterController, bool flyView, MissionSettingsItem* settingsItem, QObject* parent); + TakeoffMissionItem(const MissionItem& missionItem, PlanMasterController* masterController, bool flyView, MissionSettingsItem* settingsItem, QObject* parent); Q_PROPERTY(QGeoCoordinate launchCoordinate READ launchCoordinate WRITE setLaunchCoordinate NOTIFY launchCoordinateChanged) Q_PROPERTY(bool launchTakeoffAtSameLocation READ launchTakeoffAtSameLocation WRITE setLaunchTakeoffAtSameLocation NOTIFY launchTakeoffAtSameLocationChanged) diff --git a/src/MissionManager/TransectStyleComplexItem.cc b/src/MissionManager/TransectStyleComplexItem.cc index ca21c6baf..4fbe9a3dd 100644 --- a/src/MissionManager/TransectStyleComplexItem.cc +++ b/src/MissionManager/TransectStyleComplexItem.cc @@ -39,18 +39,18 @@ const char* TransectStyleComplexItem::_jsonCameraShotsKey = "Cam const int TransectStyleComplexItem::_terrainQueryTimeoutMsecs = 1000; -TransectStyleComplexItem::TransectStyleComplexItem(Vehicle* vehicle, bool flyView, QString settingsGroup, QObject* parent) - : ComplexMissionItem (vehicle, flyView, parent) +TransectStyleComplexItem::TransectStyleComplexItem(PlanMasterController* masterController, bool flyView, QString settingsGroup, QObject* parent) + : ComplexMissionItem (masterController, flyView, parent) , _sequenceNumber (0) , _terrainPolyPathQuery (nullptr) , _ignoreRecalc (false) , _complexDistance (0) , _cameraShots (0) - , _cameraCalc (vehicle, settingsGroup) + , _cameraCalc (masterController, settingsGroup) , _followTerrain (false) , _loadedMissionItemsParent (nullptr) , _metaDataMap (FactMetaData::createMapFromJsonFile(QStringLiteral(":/json/TransectStyle.SettingsGroup.json"), this)) - , _turnAroundDistanceFact (settingsGroup, _metaDataMap[_vehicle->multiRotor() ? turnAroundDistanceMultiRotorName : turnAroundDistanceName]) + , _turnAroundDistanceFact (settingsGroup, _metaDataMap[_controllerVehicle->multiRotor() ? turnAroundDistanceMultiRotorName : turnAroundDistanceName]) , _cameraTriggerInTurnAroundFact (settingsGroup, _metaDataMap[cameraTriggerInTurnAroundName]) , _hoverAndCaptureFact (settingsGroup, _metaDataMap[hoverAndCaptureName]) , _refly90DegreesFact (settingsGroup, _metaDataMap[refly90DegreesName]) @@ -347,7 +347,7 @@ double TransectStyleComplexItem::_turnaroundDistance(void) const bool TransectStyleComplexItem::hoverAndCaptureAllowed(void) const { - return _vehicle->multiRotor() || _vehicle->vtol(); + return _controllerVehicle->multiRotor() || _controllerVehicle->vtol(); } void TransectStyleComplexItem::_rebuildTransects(void) diff --git a/src/MissionManager/TransectStyleComplexItem.h b/src/MissionManager/TransectStyleComplexItem.h index 9fa6f9579..f1d3c26bb 100644 --- a/src/MissionManager/TransectStyleComplexItem.h +++ b/src/MissionManager/TransectStyleComplexItem.h @@ -20,12 +20,14 @@ Q_DECLARE_LOGGING_CATEGORY(TransectStyleComplexItemLog) +class PlanMasterController; + class TransectStyleComplexItem : public ComplexMissionItem { Q_OBJECT public: - TransectStyleComplexItem(Vehicle* vehicle, bool flyView, QString settignsGroup, QObject* parent); + TransectStyleComplexItem(PlanMasterController* masterController, bool flyView, QString settignsGroup, QObject* parent); Q_PROPERTY(QGCMapPolygon* surveyAreaPolygon READ surveyAreaPolygon CONSTANT) Q_PROPERTY(CameraCalc* cameraCalc READ cameraCalc CONSTANT) diff --git a/src/MissionManager/TransectStyleComplexItemTest.cc b/src/MissionManager/TransectStyleComplexItemTest.cc index b7ef64ba8..a2b75eb99 100644 --- a/src/MissionManager/TransectStyleComplexItemTest.cc +++ b/src/MissionManager/TransectStyleComplexItemTest.cc @@ -11,7 +11,6 @@ #include "QGCApplication.h" TransectStyleComplexItemTest::TransectStyleComplexItemTest(void) - : _offlineVehicle(nullptr) { _polygonVertices << QGeoCoordinate(47.633550640000003, -122.08982199) << QGeoCoordinate(47.634129020000003, -122.08887249) @@ -23,8 +22,9 @@ void TransectStyleComplexItemTest::init(void) { UnitTest::init(); - _offlineVehicle = new Vehicle(MAV_AUTOPILOT_PX4, MAV_TYPE_QUADROTOR, qgcApp()->toolbox()->firmwarePluginManager(), this); - _transectStyleItem = new TransectStyleItem(_offlineVehicle, this); + _masterController = new PlanMasterController(this); + _controllerVehicle = _masterController->controllerVehicle(); + _transectStyleItem = new TransectStyleItem(_masterController, this); _transectStyleItem->cameraTriggerInTurnAround()->setRawValue(false); _transectStyleItem->cameraCalc()->cameraName()->setRawValue(_transectStyleItem->cameraCalc()->customCameraName()); _transectStyleItem->cameraCalc()->valueSetIsDistance()->setRawValue(true); @@ -49,7 +49,6 @@ void TransectStyleComplexItemTest::init(void) void TransectStyleComplexItemTest::cleanup(void) { delete _transectStyleItem; - delete _offlineVehicle; delete _multiSpy; } @@ -224,8 +223,8 @@ void TransectStyleComplexItemTest::_testAltMode(void) QVERIFY(!_transectStyleItem->followTerrain()); } -TransectStyleItem::TransectStyleItem(Vehicle* vehicle, QObject* parent) - : TransectStyleComplexItem (vehicle, false /* flyView */, QStringLiteral("UnitTestTransect"), parent) +TransectStyleItem::TransectStyleItem(PlanMasterController* masterController, QObject* parent) + : TransectStyleComplexItem (masterController, false /* flyView */, QStringLiteral("UnitTestTransect"), parent) , rebuildTransectsPhase1Called (false) , recalcComplexDistanceCalled (false) , recalcCameraShotsCalled (false) diff --git a/src/MissionManager/TransectStyleComplexItemTest.h b/src/MissionManager/TransectStyleComplexItemTest.h index 178778c3c..52356e216 100644 --- a/src/MissionManager/TransectStyleComplexItemTest.h +++ b/src/MissionManager/TransectStyleComplexItemTest.h @@ -12,6 +12,7 @@ #include "UnitTest.h" #include "MultiSignalSpy.h" #include "CorridorScanComplexItem.h" +#include "PlanMasterController.h" #include @@ -72,10 +73,11 @@ private: static const size_t _cSignals = maxSignalIndex; const char* _rgSignals[_cSignals]; - Vehicle* _offlineVehicle; - MultiSignalSpy* _multiSpy; + PlanMasterController* _masterController = nullptr; + Vehicle* _controllerVehicle = nullptr; + MultiSignalSpy* _multiSpy = nullptr; QList _polygonVertices; - TransectStyleItem* _transectStyleItem; + TransectStyleItem* _transectStyleItem = nullptr; }; class TransectStyleItem : public TransectStyleComplexItem @@ -83,7 +85,7 @@ class TransectStyleItem : public TransectStyleComplexItem Q_OBJECT public: - TransectStyleItem(Vehicle* vehicle, QObject* parent = nullptr); + TransectStyleItem(PlanMasterController* masterController, QObject* parent = nullptr); // Overrides from ComplexMissionItem QString mapVisualQML (void) const final { return QString(); } diff --git a/src/MissionManager/VisualMissionItem.cc b/src/MissionManager/VisualMissionItem.cc index 4cbac41bc..013f3f746 100644 --- a/src/MissionManager/VisualMissionItem.cc +++ b/src/MissionManager/VisualMissionItem.cc @@ -17,22 +17,23 @@ #include "JsonHelper.h" #include "TerrainQuery.h" #include "TakeoffMissionItem.h" +#include "PlanMasterController.h" const char* VisualMissionItem::jsonTypeKey = "type"; const char* VisualMissionItem::jsonTypeSimpleItemValue = "SimpleItem"; const char* VisualMissionItem::jsonTypeComplexItemValue = "ComplexItem"; -VisualMissionItem::VisualMissionItem(Vehicle* vehicle, bool flyView, QObject* parent) - : QObject (parent) - , _vehicle (vehicle) - , _flyView (flyView) +VisualMissionItem::VisualMissionItem(PlanMasterController* masterController, bool flyView, QObject* parent) + : QObject (parent) + , _flyView (flyView) + , _masterController (masterController) + , _controllerVehicle(masterController->controllerVehicle()) { _commonInit(); } VisualMissionItem::VisualMissionItem(const VisualMissionItem& other, bool flyView, QObject* parent) : QObject (parent) - , _vehicle (nullptr) , _flyView (flyView) { *this = other; @@ -43,7 +44,8 @@ VisualMissionItem::VisualMissionItem(const VisualMissionItem& other, bool flyVie void VisualMissionItem::_commonInit(void) { // Don't get terrain altitude information for submarines or boats - if (_vehicle->vehicleType() != MAV_TYPE_SUBMARINE && _vehicle->vehicleType() != MAV_TYPE_SURFACE_BOAT) { + Vehicle* controllerVehicle = _masterController->controllerVehicle(); + if (controllerVehicle->vehicleType() != MAV_TYPE_SUBMARINE && controllerVehicle->vehicleType() != MAV_TYPE_SURFACE_BOAT) { _updateTerrainTimer.setInterval(500); _updateTerrainTimer.setSingleShot(true); connect(&_updateTerrainTimer, &QTimer::timeout, this, &VisualMissionItem::_reallyUpdateTerrainAltitude); @@ -54,7 +56,8 @@ void VisualMissionItem::_commonInit(void) const VisualMissionItem& VisualMissionItem::operator=(const VisualMissionItem& other) { - _vehicle = other._vehicle; + _masterController = other._masterController; + _controllerVehicle = other._controllerVehicle; setIsCurrentItem(other._isCurrentItem); setDirty(other._dirty); diff --git a/src/MissionManager/VisualMissionItem.h b/src/MissionManager/VisualMissionItem.h index 1e5675797..e19734c53 100644 --- a/src/MissionManager/VisualMissionItem.h +++ b/src/MissionManager/VisualMissionItem.h @@ -26,6 +26,7 @@ #include "MissionController.h" class MissionItem; +class PlanMasterController; // Abstract base class for all Simple and Complex visual mission objects. class VisualMissionItem : public QObject @@ -33,7 +34,7 @@ class VisualMissionItem : public QObject Q_OBJECT public: - VisualMissionItem(Vehicle* vehicle, bool flyView, QObject* parent); + VisualMissionItem(PlanMasterController* masterController, bool flyView, QObject* parent); VisualMissionItem(const VisualMissionItem& other, bool flyView, QObject* parent); ~VisualMissionItem(); @@ -47,7 +48,6 @@ public: }; Q_ENUM(ReadyForSaveState) - Q_PROPERTY(Vehicle* vehicle READ vehicle CONSTANT) Q_PROPERTY(bool homePosition READ homePosition CONSTANT) ///< true: This item is being used as a home position indicator Q_PROPERTY(QGeoCoordinate coordinate READ coordinate WRITE setCoordinate NOTIFY coordinateChanged) ///< This is the entry point for a waypoint line into the item. For a simple item it is also the location of the item Q_PROPERTY(double terrainAltitude READ terrainAltitude NOTIFY terrainAltitudeChanged) ///< The altitude of terrain at the coordinate position, NaN if not known @@ -70,7 +70,6 @@ public: Q_PROPERTY(bool isTakeoffItem READ isTakeoffItem NOTIFY isTakeoffItemChanged) ///< true: Takeoff item special case Q_PROPERTY(QString editorQml MEMBER _editorQml CONSTANT) ///< Qml code for editing this item Q_PROPERTY(QString mapVisualQML READ mapVisualQML CONSTANT) ///< QMl code for map visuals - Q_PROPERTY(QmlObjectListModel* childItems READ childItems CONSTANT) Q_PROPERTY(double specifiedFlightSpeed READ specifiedFlightSpeed NOTIFY specifiedFlightSpeedChanged) ///< NaN for not specified Q_PROPERTY(double specifiedGimbalYaw READ specifiedGimbalYaw NOTIFY specifiedGimbalYawChanged) ///< NaN for not specified Q_PROPERTY(double specifiedGimbalPitch READ specifiedGimbalPitch NOTIFY specifiedGimbalPitchChanged) ///< NaN for not specified @@ -79,10 +78,12 @@ public: Q_PROPERTY(double missionVehicleYaw READ missionVehicleYaw NOTIFY missionVehicleYawChanged) ///< Expected vehicle yaw at this point in mission Q_PROPERTY(bool flyView READ flyView CONSTANT) Q_PROPERTY(bool wizardMode READ wizardMode WRITE setWizardMode NOTIFY wizardModeChanged) - Q_PROPERTY(ReadyForSaveState readyForSaveState READ readyForSaveState NOTIFY readyForSaveStateChanged) - Q_PROPERTY(VisualMissionItem* parentItem READ parentItem WRITE setParentItem NOTIFY parentItemChanged) - Q_PROPERTY(QGCGeoBoundingCube* boundingCube READ boundingCube NOTIFY boundingCubeChanged) + Q_PROPERTY(PlanMasterController* masterController READ masterController CONSTANT) + Q_PROPERTY(ReadyForSaveState readyForSaveState READ readyForSaveState NOTIFY readyForSaveStateChanged) + Q_PROPERTY(VisualMissionItem* parentItem READ parentItem WRITE setParentItem NOTIFY parentItemChanged) + Q_PROPERTY(QmlObjectListModel* childItems READ childItems CONSTANT) + Q_PROPERTY(QGCGeoBoundingCube* boundingCube READ boundingCube NOTIFY boundingCubeChanged) // The following properties are calculated/set by the MissionController recalc methods @@ -123,7 +124,7 @@ public: void setHomePositionSpecialCase (bool homePositionSpecialCase) { _homePositionSpecialCase = homePositionSpecialCase; } - Vehicle* vehicle(void) { return _vehicle; } + PlanMasterController* masterController(void) { return _masterController; } // Pure virtuals which must be provides by derived classes @@ -229,23 +230,25 @@ signals: void exitCoordinateSameAsEntryChanged (bool exitCoordinateSameAsEntry); protected: - Vehicle* _vehicle; - bool _flyView = false; - bool _isCurrentItem = false; - bool _hasCurrentChildItem = false; - bool _dirty = false; - bool _homePositionSpecialCase = false; ///< true: This item is being used as a ui home position indicator - bool _wizardMode = false; ///< true: Item editor is showing wizard completion panel - double _terrainAltitude = qQNaN(); ///< Altitude of terrain at coordinate position, NaN for not known - double _altDifference = 0; ///< Difference in altitude from previous waypoint - double _altPercent = 0; ///< Percent of total altitude change in mission - double _terrainPercent = qQNaN(); ///< Percent of terrain altitude for coordinate - bool _terrainCollision = false; ///< true: item collides with terrain - double _azimuth = 0; ///< Azimuth to previous waypoint - double _distance = 0; ///< Distance to previous waypoint - QString _editorQml; ///< Qml resource for editing item - double _missionGimbalYaw = qQNaN(); - double _missionVehicleYaw = qQNaN(); + bool _flyView = false; + bool _isCurrentItem = false; + bool _hasCurrentChildItem = false; + bool _dirty = false; + bool _homePositionSpecialCase = false; ///< true: This item is being used as a ui home position indicator + bool _wizardMode = false; ///< true: Item editor is showing wizard completion panel + double _terrainAltitude = qQNaN(); ///< Altitude of terrain at coordinate position, NaN for not known + double _altDifference = 0; ///< Difference in altitude from previous waypoint + double _altPercent = 0; ///< Percent of total altitude change in mission + double _terrainPercent = qQNaN(); ///< Percent of terrain altitude for coordinate + bool _terrainCollision = false; ///< true: item collides with terrain + double _azimuth = 0; ///< Azimuth to previous waypoint + double _distance = 0; ///< Distance to previous waypoint + QString _editorQml; ///< Qml resource for editing item + double _missionGimbalYaw = qQNaN(); + double _missionVehicleYaw = qQNaN(); + + PlanMasterController* _masterController = nullptr; + Vehicle* _controllerVehicle = nullptr; VisualMissionItem* _parentItem = nullptr; QGCGeoBoundingCube _boundingCube; ///< The bounding "cube" of this element. diff --git a/src/MissionManager/VisualMissionItemTest.cc b/src/MissionManager/VisualMissionItemTest.cc index 6101b9cbc..8579b5ba8 100644 --- a/src/MissionManager/VisualMissionItemTest.cc +++ b/src/MissionManager/VisualMissionItemTest.cc @@ -10,9 +10,9 @@ #include "VisualMissionItemTest.h" #include "SimpleMissionItem.h" #include "QGCApplication.h" +#include "PlanMasterController.h" VisualMissionItemTest::VisualMissionItemTest(void) - : _offlineVehicle(nullptr) { } @@ -20,10 +20,9 @@ VisualMissionItemTest::VisualMissionItemTest(void) void VisualMissionItemTest::init(void) { UnitTest::init(); - _offlineVehicle = new Vehicle(MAV_AUTOPILOT_PX4, - MAV_TYPE_QUADROTOR, - qgcApp()->toolbox()->firmwarePluginManager(), - this); + + _masterController = new PlanMasterController(MAV_AUTOPILOT_PX4, MAV_TYPE_QUADROTOR, this); + _controllerVehicle = _masterController->controllerVehicle(); rgVisualItemSignals[altDifferenceChangedIndex] = SIGNAL(altDifferenceChanged(double)); rgVisualItemSignals[altPercentChangedIndex] = SIGNAL(altPercentChanged(double)); @@ -54,7 +53,7 @@ void VisualMissionItemTest::init(void) void VisualMissionItemTest::cleanup(void) { - _offlineVehicle->deleteLater(); + _masterController->deleteLater(); UnitTest::cleanup(); } diff --git a/src/MissionManager/VisualMissionItemTest.h b/src/MissionManager/VisualMissionItemTest.h index cee94037d..f84181885 100644 --- a/src/MissionManager/VisualMissionItemTest.h +++ b/src/MissionManager/VisualMissionItemTest.h @@ -16,6 +16,8 @@ #include +class PlanMasterController; + /// Unit test for SimpleMissionItem class VisualMissionItemTest : public UnitTest { @@ -90,5 +92,6 @@ protected: static const size_t cVisualItemSignals = maxSignalIndex; const char* rgVisualItemSignals[cVisualItemSignals]; - Vehicle* _offlineVehicle; + PlanMasterController* _masterController = nullptr; + Vehicle* _controllerVehicle = nullptr; }; diff --git a/src/PlanView/SimpleItemEditor.qml b/src/PlanView/SimpleItemEditor.qml index fe64ea646..3b53d3afa 100644 --- a/src/PlanView/SimpleItemEditor.qml +++ b/src/PlanView/SimpleItemEditor.qml @@ -19,7 +19,8 @@ Rectangle { property bool _specifiesAltitude: missionItem.specifiesAltitude property real _margin: ScreenTools.defaultFontPixelHeight / 2 - property bool _supportsTerrainFrame: missionItem + property bool _supportsTerrainFrame: missionItem.masterController.supportsTerrain + property var _controllerVehicle: missionItem.masterController.controllerVehicle property string _altModeRelativeHelpText: qsTr("Altitude relative to launch altitude") property string _altModeAbsoluteHelpText: qsTr("Altitude above mean sea level") @@ -66,7 +67,7 @@ Rectangle { visible: missionItem.isTakeoffItem && missionItem.wizardMode // Hack special case for takeoff item QGCLabel { - text: qsTr("Move 'T' Takeoff to the %1 location.").arg(missionItem.vehicle.vtol ? qsTr("desired") : qsTr("climbout")) + text: qsTr("Move 'T' Takeoff to the %1 location.").arg(_controllerVehicle.vtol ? qsTr("desired") : qsTr("climbout")) Layout.fillWidth: true wrapMode: Text.WordWrap visible: !initialClickLabel.visible @@ -76,7 +77,7 @@ Rectangle { text: qsTr("Ensure clear of obstacles and into the wind.") Layout.fillWidth: true wrapMode: Text.WordWrap - visible: !initialClickLabel.visible && !missionItem.vehicle.vtol + visible: !initialClickLabel.visible && !_controllerVehicle.vtol } QGCButton { @@ -214,7 +215,7 @@ Rectangle { text: qsTr("Terrain Frame") checkable: true checked: missionItem.altitudeMode === QGroundControl.AltitudeModeTerrainFrame - visible: missionItem.altitudeMode === QGroundControl.AltitudeModeTerrainFrame + visible: _supportsTerrainFrame && (missionItem.specifiesCoordinate || missionItem.specifiesAltitudeOnly) onTriggered: missionItem.altitudeMode = QGroundControl.AltitudeModeTerrainFrame } } diff --git a/src/QmlControls/QGroundControl/FlightDisplay/qmldir b/src/QmlControls/QGroundControl/FlightDisplay/qmldir index c06aacc9e..fbb1c6060 100644 --- a/src/QmlControls/QGroundControl/FlightDisplay/qmldir +++ b/src/QmlControls/QGroundControl/FlightDisplay/qmldir @@ -14,3 +14,4 @@ PreFlightGPSCheck 1.0 PreFlightGPSCheck.qml PreFlightRCCheck 1.0 PreFlightRCCheck.qml PreFlightSensorsHealthCheck 1.0 PreFlightSensorsHealthCheck.qml PreFlightSoundCheck 1.0 PreFlightSoundCheck.qml +TerrainProgress 1.0 TerrainProgress.qml diff --git a/src/Terrain/TerrainQuery.cc b/src/Terrain/TerrainQuery.cc index 703b90153..bb5a44712 100644 --- a/src/Terrain/TerrainQuery.cc +++ b/src/Terrain/TerrainQuery.cc @@ -341,7 +341,7 @@ void TerrainTileManager::addCoordinateQuery(TerrainOfflineAirMapQuery* terrainQu bool error; QList altitudes; - if (!_getAltitudesForCoordinates(coordinates, altitudes, error)) { + if (!getAltitudesForCoordinates(coordinates, altitudes, error)) { qCDebug(TerrainQueryLog) << "TerrainTileManager::addPathQuery queue count" << _requestQueue.count(); QueuedRequestInfo_t queuedRequestInfo = { terrainQueryInterface, QueryMode::QueryModeCoordinates, 0, 0, coordinates }; _requestQueue.append(queuedRequestInfo); @@ -380,7 +380,7 @@ void TerrainTileManager::addPathQuery(TerrainOfflineAirMapQuery* terrainQueryInt bool error; QList altitudes; - if (!_getAltitudesForCoordinates(coordinates, altitudes, error)) { + if (!getAltitudesForCoordinates(coordinates, altitudes, error)) { qCDebug(TerrainQueryLog) << "TerrainTileManager::addPathQuery queue count" << _requestQueue.count(); QueuedRequestInfo_t queuedRequestInfo = { terrainQueryInterface, QueryMode::QueryModePath, latStep, lonStep, coordinates }; _requestQueue.append(queuedRequestInfo); @@ -400,13 +400,13 @@ void TerrainTileManager::addPathQuery(TerrainOfflineAirMapQuery* terrainQueryInt /// Either returns altitudes from cache or queues database request /// @param[out] error true: altitude not returned due to error, false: altitudes returned /// @return true: altitude returned (check error as well), false: database query queued (altitudes not returned) -bool TerrainTileManager::_getAltitudesForCoordinates(const QList& coordinates, QList& altitudes, bool& error) +bool TerrainTileManager::getAltitudesForCoordinates(const QList& coordinates, QList& altitudes, bool& error) { error = false; for (const QGeoCoordinate& coordinate: coordinates) { QString tileHash = _getTileHash(coordinate); - qCDebug(TerrainQueryLog) << "TerrainTileManager::_getAltitudesForCoordinates hash:coordinate" << tileHash << coordinate; + qCDebug(TerrainQueryLog) << "TerrainTileManager::getAltitudesForCoordinates hash:coordinate" << tileHash << coordinate; _tilesMutex.lock(); if (_tiles.contains(tileHash)) { @@ -414,20 +414,20 @@ bool TerrainTileManager::_getAltitudesForCoordinates(const QList double elevation = _tiles[tileHash].elevation(coordinate); if (qIsNaN(elevation)) { error = true; - qCWarning(TerrainQueryLog) << "TerrainTileManager::_getAltitudesForCoordinates Internal Error: negative elevation in tile cache"; + qCWarning(TerrainQueryLog) << "TerrainTileManager::getAltitudesForCoordinates Internal Error: missing elevation in tile cache"; } else { - qCDebug(TerrainQueryLog) << "TerrainTileManager::_getAltitudesForCoordinates returning elevation from tile cache" << elevation; + qCDebug(TerrainQueryLog) << "TerrainTileManager::getAltitudesForCoordinates returning elevation from tile cache" << elevation; } altitudes.push_back(elevation); } else { - qCWarning(TerrainQueryLog) << "TerrainTileManager::_getAltitudesForCoordinates Internal Error: coordinate not in tile region"; + qCWarning(TerrainQueryLog) << "TerrainTileManager::getAltitudesForCoordinates Internal Error: coordinate not in tile region"; altitudes.push_back(qQNaN()); error = true; } } else { if (_state != State::Downloading) { QNetworkRequest request = getQGCMapEngine()->urlFactory()->getTileURL("Airmap Elevation", getQGCMapEngine()->urlFactory()->long2tileX("Airmap Elevation",coordinate.longitude(), 1), getQGCMapEngine()->urlFactory()->lat2tileY("Airmap Elevation", coordinate.latitude(), 1), 1, &_networkManager); - qCDebug(TerrainQueryLog) << "TerrainTileManager::_getAltitudesForCoordinates query from database" << request.url(); + qCDebug(TerrainQueryLog) << "TerrainTileManager::getAltitudesForCoordinates query from database" << request.url(); QGeoTileSpec spec; spec.setX(getQGCMapEngine()->urlFactory()->long2tileX("Airmap Elevation", coordinate.longitude(), 1)); spec.setY(getQGCMapEngine()->urlFactory()->lat2tileY("Airmap Elevation", coordinate.latitude(), 1)); @@ -512,7 +512,7 @@ void TerrainTileManager::_terrainDone(QByteArray responseBytes, QNetworkReply::N QList altitudes; QueuedRequestInfo_t& requestInfo = _requestQueue[i]; - if (_getAltitudesForCoordinates(requestInfo.coordinates, altitudes, error)) { + if (getAltitudesForCoordinates(requestInfo.coordinates, altitudes, error)) { if (requestInfo.queryMode == QueryMode::QueryModeCoordinates) { if (error) { QList noAltitudes; @@ -698,6 +698,11 @@ void TerrainAtCoordinateQuery::requestData(const QList& coordina _TerrainAtCoordinateBatchManager->addQuery(this, coordinates); } +bool TerrainAtCoordinateQuery::getAltitudesForCoordinates(const QList& coordinates, QList& altitudes, bool& error) +{ + return _terrainTileManager->getAltitudesForCoordinates(coordinates, altitudes, error); +} + void TerrainAtCoordinateQuery::_signalTerrainData(bool success, QList& heights) { emit terrainDataReceived(success, heights); diff --git a/src/Terrain/TerrainQuery.h b/src/Terrain/TerrainQuery.h index e7ef68a12..a288596c0 100644 --- a/src/Terrain/TerrainQuery.h +++ b/src/Terrain/TerrainQuery.h @@ -115,8 +115,9 @@ class TerrainTileManager : public QObject { public: TerrainTileManager(void); - void addCoordinateQuery (TerrainOfflineAirMapQuery* terrainQueryInterface, const QList& coordinates); - void addPathQuery (TerrainOfflineAirMapQuery* terrainQueryInterface, const QGeoCoordinate& startPoint, const QGeoCoordinate& endPoint); + void addCoordinateQuery (TerrainOfflineAirMapQuery* terrainQueryInterface, const QList& coordinates); + void addPathQuery (TerrainOfflineAirMapQuery* terrainQueryInterface, const QGeoCoordinate& startPoint, const QGeoCoordinate& endPoint); + bool getAltitudesForCoordinates (const QList& coordinates, QList& altitudes, bool& error); private slots: void _terrainDone (QByteArray responseBytes, QNetworkReply::NetworkError error); @@ -141,7 +142,6 @@ private: } QueuedRequestInfo_t; void _tileFailed (void); - bool _getAltitudesForCoordinates (const QList& coordinates, QList& altitudes, bool& error); QString _getTileHash (const QGeoCoordinate& coordinate); QList _requestQueue; @@ -207,6 +207,11 @@ public: /// @param coordinates to query void requestData(const QList& coordinates); + /// Either returns altitudes from cache or queues database request + /// @param[out] error true: altitude not returned due to error, false: altitudes returned + /// @return true: altitude returned (check error as well), false: database query queued (altitudes not returned) + static bool getAltitudesForCoordinates(const QList& coordinates, QList& altitudes, bool& error); + // Internal method void _signalTerrainData(bool success, QList& heights); diff --git a/src/Vehicle/TerrainFactGroup.cc b/src/Vehicle/TerrainFactGroup.cc new file mode 100644 index 000000000..0c1e5b9dd --- /dev/null +++ b/src/Vehicle/TerrainFactGroup.cc @@ -0,0 +1,22 @@ +/**************************************************************************** + * + * (c) 2009-2020 QGROUNDCONTROL PROJECT + * + * QGroundControl is licensed according to the terms in the file + * COPYING.md in the root of the source code directory. + * + ****************************************************************************/ + +#include "TerrainFactGroup.h" + +const char* TerrainFactGroup::_blocksPendingFactName = "blocksPending"; +const char* TerrainFactGroup::_blocksLoadedFactName = "blocksLoaded"; + +TerrainFactGroup::TerrainFactGroup(QObject* parent) + : FactGroup (1000, ":/json/Vehicle/TerrainFactGroup.json", parent) + , _blocksPendingFact(0, _blocksPendingFactName, FactMetaData::valueTypeDouble) + , _blocksLoadedFact (0, _blocksLoadedFactName, FactMetaData::valueTypeDouble) +{ + _addFact(&_blocksPendingFact, _blocksPendingFactName); + _addFact(&_blocksLoadedFact, _blocksLoadedFactName); +} diff --git a/src/Vehicle/TerrainFactGroup.h b/src/Vehicle/TerrainFactGroup.h new file mode 100644 index 000000000..224201fb2 --- /dev/null +++ b/src/Vehicle/TerrainFactGroup.h @@ -0,0 +1,35 @@ +/**************************************************************************** + * + * (c) 2009-2020 QGROUNDCONTROL PROJECT + * + * QGroundControl is licensed according to the terms in the file + * COPYING.md in the root of the source code directory. + * + ****************************************************************************/ + +#pragma once + +#include "FactGroup.h" + +#include + +class TerrainFactGroup : public FactGroup +{ + Q_OBJECT + +public: + TerrainFactGroup(QObject* parent = nullptr); + + Q_PROPERTY(Fact* blocksPending READ blocksPending CONSTANT) + Q_PROPERTY(Fact* blocksLoaded READ blocksLoaded CONSTANT) + + Fact* blocksPending () { return &_blocksPendingFact; } + Fact* blocksLoaded () { return &_blocksLoadedFact; } + + static const char* _blocksPendingFactName; + static const char* _blocksLoadedFactName; + +private: + Fact _blocksPendingFact; + Fact _blocksLoadedFact; +}; diff --git a/src/Vehicle/TerrainFactGroup.json b/src/Vehicle/TerrainFactGroup.json new file mode 100644 index 000000000..d29047346 --- /dev/null +++ b/src/Vehicle/TerrainFactGroup.json @@ -0,0 +1,16 @@ +[ +{ + "name": "blocksPending", + "shortDescription": "Blocks Pending", + "type": "double", + "decimalPlaces": 0, + "default": 0 +}, +{ + "name": "blocksLoaded", + "shortDescription": "Blocks Loaded", + "type": "double", + "decimalPlaces": 0, + "default": 0 +} +] diff --git a/src/Vehicle/TerrainProtocolHandler.cc b/src/Vehicle/TerrainProtocolHandler.cc new file mode 100644 index 000000000..76b10a614 --- /dev/null +++ b/src/Vehicle/TerrainProtocolHandler.cc @@ -0,0 +1,152 @@ +/**************************************************************************** + * + * (c) 2009-2020 QGROUNDCONTROL PROJECT + * + * QGroundControl is licensed according to the terms in the file + * COPYING.md in the root of the source code directory. + * + ****************************************************************************/ + +#include "TerrainProtocolHandler.h" +#include "TerrainQuery.h" +#include "QGCApplication.h" + +QGC_LOGGING_CATEGORY(TerrainProtocolHandlerLog, "TerrainProtocolHandlerLog") + +TerrainProtocolHandler::TerrainProtocolHandler(Vehicle* vehicle, TerrainFactGroup* terrainFactGroup, QObject *parent) + : QObject (parent) + , _vehicle (vehicle) + , _terrainFactGroup (terrainFactGroup) +{ + _terrainDataSendTimer.setSingleShot(false); + _terrainDataSendTimer.setInterval(1000.0/12.0); + connect(&_terrainDataSendTimer, &QTimer::timeout, this, &TerrainProtocolHandler::_sendNextTerrainData); +} + +bool TerrainProtocolHandler::mavlinkMessageReceived(const mavlink_message_t message) +{ + switch (message.msgid) { + case MAVLINK_MSG_ID_TERRAIN_REQUEST: + _handleTerrainRequest(message); + return false; + case MAVLINK_MSG_ID_TERRAIN_REPORT: + _handleTerrainReport(message); + return false; + default: + return true; + } +} + +void TerrainProtocolHandler::_handleTerrainRequest(const mavlink_message_t& message) +{ + _terrainRequestActive = true; + mavlink_msg_terrain_request_decode(&message, &_currentTerrainRequest); + _sendNextTerrainData(); +} + +void TerrainProtocolHandler::_handleTerrainReport(const mavlink_message_t& message) +{ + mavlink_terrain_report_t terrainReport; + mavlink_msg_terrain_report_decode(&message, &terrainReport); + + _terrainFactGroup->blocksPending()->setRawValue(terrainReport.pending); + _terrainFactGroup->blocksLoaded()->setRawValue(terrainReport.loaded); + + if (TerrainProtocolHandlerLog().isDebugEnabled()) { + + bool error; + QGeoCoordinate coord(static_cast(terrainReport.lat) / 1e7, static_cast(terrainReport.lon) / 1e7); + QList altitudes; + QList coordinates; + coordinates.append(coord); + bool altAvailable = (TerrainAtCoordinateQuery::getAltitudesForCoordinates(coordinates, altitudes, error)); + QString vehicleAlt = terrainReport.spacing ? QStringLiteral("%1").arg(terrainReport.terrain_height) : QStringLiteral("n/a"); + QString qgcAlt = error ? QStringLiteral("error") : + (altAvailable ? QStringLiteral("%1").arg(altitudes[0]) : QStringLiteral("n/a")); + qDebug() << "TERRAIN_REPORT" << coord << QStringLiteral("Vehicle(%1) QGC(%2)").arg(vehicleAlt).arg(qgcAlt); + } +} + +void TerrainProtocolHandler::_sendNextTerrainData(void) +{ + if (!_terrainRequestActive) { + return; + } + + QGeoCoordinate terrainRequestCoordSWCorner(static_cast(_currentTerrainRequest.lat) / 1e7, static_cast(_currentTerrainRequest.lon) / 1e7); + int spacingBetweenGrids = _currentTerrainRequest.grid_spacing * 4; + + // Each TERRAIN_DATA sent to vehicle contains a 4x4 grid of heights + // TERRAIN_REQUEST.mask has a bit for each entry in an 8x7 grid + // gridBit = 0 refers to the the sw corner of the 8x7 grid + + bool bitFound = false; + for (int rowIndex=0; rowIndex<7; rowIndex++) { + for (int colIndex=0; colIndex<8; colIndex++) { + uint8_t gridBit = (rowIndex * 8) + colIndex; + uint64_t checkBit = 1ull << gridBit; + if (_currentTerrainRequest.mask & checkBit) { + // Move east and then north to generate the coordinate for sw corner of the specific gridBit + QGeoCoordinate swCorner = terrainRequestCoordSWCorner.atDistanceAndAzimuth(spacingBetweenGrids * colIndex, 90); + swCorner = swCorner.atDistanceAndAzimuth(spacingBetweenGrids * rowIndex, 0); + _sendTerrainData(swCorner, gridBit); + bitFound = true; + break; + } + } + if (bitFound) { + break; + } + } + + if (bitFound) { + // Kick timer to send next possible TERRAIN_DATA to vehicle + _terrainDataSendTimer.start(); + } else { + _terrainRequestActive = false; + } +} + +void TerrainProtocolHandler::_sendTerrainData(const QGeoCoordinate& swCorner, uint8_t gridBit) +{ + QList coordinates; + for (int rowIndex=0; rowIndex<4; rowIndex++) { + for (int colIndex=0; colIndex<4; colIndex++) { + // Move east and then north to generate the coordinate for grid point + QGeoCoordinate coord = swCorner.atDistanceAndAzimuth(_currentTerrainRequest.grid_spacing * colIndex, 90); + coord = coord.atDistanceAndAzimuth(_currentTerrainRequest.grid_spacing * rowIndex, 0); + coordinates.append(coord); + } + } + + // Query terrain system for altitudes. If it has them available it will return them. If not they will be queued for download. + bool error = false; + QList altitudes; + if (TerrainAtCoordinateQuery::getAltitudesForCoordinates(coordinates, altitudes, error)) { + if (error) { + qCWarning(TerrainProtocolHandlerLog) << "_sendTerrainData TerrainAtCoordinateQuery::getAltitudesForCoordinates failed"; + } else { + // Only clear the bit if the query succeeds. Otherwise just let it try again on the next timer tick + uint64_t removeBit = ~(1ull << gridBit); + _currentTerrainRequest.mask &= removeBit; + int altIndex = 0; + int16_t terrainData[16]; + for (const double& altitude : altitudes) { + terrainData[altIndex++] = static_cast(altitude); + } + + mavlink_message_t msg; + mavlink_msg_terrain_data_pack_chan( + qgcApp()->toolbox()->mavlinkProtocol()->getSystemId(), + qgcApp()->toolbox()->mavlinkProtocol()->getComponentId(), + _vehicle->priorityLink()->mavlinkChannel(), + &msg, + _currentTerrainRequest.lat, + _currentTerrainRequest.lon, + _currentTerrainRequest.grid_spacing, + gridBit, + terrainData); + _vehicle->sendMessageOnLink(_vehicle->priorityLink(), msg); + } + } +} diff --git a/src/Vehicle/TerrainProtocolHandler.h b/src/Vehicle/TerrainProtocolHandler.h new file mode 100644 index 000000000..717f8a944 --- /dev/null +++ b/src/Vehicle/TerrainProtocolHandler.h @@ -0,0 +1,46 @@ +/**************************************************************************** + * + * (c) 2009-2020 QGROUNDCONTROL PROJECT + * + * QGroundControl is licensed according to the terms in the file + * COPYING.md in the root of the source code directory. + * + ****************************************************************************/ + +#pragma once + +#include "Vehicle.h" +#include "QGCMAVLink.h" +#include "QGCLoggingCategory.h" + +#include +#include + +class TerrainFactGroup; + +Q_DECLARE_LOGGING_CATEGORY(TerrainProtocolHandlerLog) + +class TerrainProtocolHandler : public QObject +{ + Q_OBJECT + +public: + explicit TerrainProtocolHandler(Vehicle* vehicle, TerrainFactGroup* terrainFactGroup, QObject *parent = nullptr); + + /// @return true: Allow vehicle to continue processing, false: Vehicle should not process message + bool mavlinkMessageReceived(const mavlink_message_t message); + +private slots: + void _sendNextTerrainData(void); + +private: + void _handleTerrainRequest (const mavlink_message_t& message); + void _handleTerrainReport (const mavlink_message_t& message); + void _sendTerrainData (const QGeoCoordinate& swCorner, uint8_t gridBit); + + Vehicle* _vehicle; + TerrainFactGroup* _terrainFactGroup; + bool _terrainRequestActive = false; + mavlink_terrain_request_t _currentTerrainRequest; + QTimer _terrainDataSendTimer; +}; diff --git a/src/Vehicle/Vehicle.cc b/src/Vehicle/Vehicle.cc index 6b8b6237f..9fcbc6eb8 100644 --- a/src/Vehicle/Vehicle.cc +++ b/src/Vehicle/Vehicle.cc @@ -45,6 +45,7 @@ #include "VehicleObjectAvoidance.h" #include "TrajectoryPoints.h" #include "QGCGeo.h" +#include "TerrainProtocolHandler.h" #if defined(QGC_AIRMAP_ENABLED) #include "AirspaceVehicleManager.h" @@ -91,6 +92,7 @@ const char* Vehicle::_temperatureFactGroupName = "temperature"; const char* Vehicle::_clockFactGroupName = "clock"; const char* Vehicle::_distanceSensorFactGroupName = "distanceSensor"; const char* Vehicle::_estimatorStatusFactGroupName = "estimatorStatus"; +const char* Vehicle::_terrainFactGroupName = "terrain"; // Standard connected vehicle Vehicle::Vehicle(LinkInterface* link, @@ -149,7 +151,9 @@ Vehicle::Vehicle(LinkInterface* link, , _mavlinkProtocolRequestComplete(false) , _maxProtoVersion(0) , _vehicleCapabilitiesKnown(false) - , _capabilityBits(0) + , _capabilityBits(firmwareType == MAV_AUTOPILOT_ARDUPILOTMEGA ? + MAV_PROTOCOL_CAPABILITY_MISSION_INT | MAV_PROTOCOL_CAPABILITY_COMMAND_INT | MAV_PROTOCOL_CAPABILITY_TERRAIN | MAV_PROTOCOL_CAPABILITY_MISSION_FENCE | MAV_PROTOCOL_CAPABILITY_MISSION_RALLY : // Hack workound for ArduPilot startup problems + 0) , _highLatencyLink(false) , _receivingAttitudeQuaternion(false) , _cameras(nullptr) @@ -222,6 +226,8 @@ Vehicle::Vehicle(LinkInterface* link, , _clockFactGroup(this) , _distanceSensorFactGroup(this) , _estimatorStatusFactGroup(this) + , _terrainFactGroup(this) + , _terrainProtocolHandler(new TerrainProtocolHandler(this, &_terrainFactGroup, this)) { connect(_joystickManager, &JoystickManager::activeJoystickChanged, this, &Vehicle::_loadSettings); connect(qgcApp()->toolbox()->multiVehicleManager(), &MultiVehicleManager::activeVehicleAvailableChanged, this, &Vehicle::_loadSettings); @@ -429,6 +435,9 @@ Vehicle::Vehicle(MAV_AUTOPILOT firmwareType, connect(_settingsManager->appSettings()->offlineEditingCruiseSpeed(), &Fact::rawValueChanged, this, &Vehicle::_offlineCruiseSpeedSettingChanged); connect(_settingsManager->appSettings()->offlineEditingHoverSpeed(), &Fact::rawValueChanged, this, &Vehicle::_offlineHoverSpeedSettingChanged); + // This add correct terrain capability bit + _offlineFirmwareTypeSettingChanged(_settingsManager->appSettings()->offlineEditingFirmwareType()->rawValue()); + _firmwarePlugin->initializeVehicle(this); } @@ -505,6 +514,7 @@ void Vehicle::_commonInit() _addFactGroup(&_clockFactGroup, _clockFactGroupName); _addFactGroup(&_distanceSensorFactGroup, _distanceSensorFactGroupName); _addFactGroup(&_estimatorStatusFactGroup, _estimatorStatusFactGroupName); + _addFactGroup(&_terrainFactGroup, _terrainFactGroupName); // Add firmware-specific fact groups, if provided QMap* fwFactGroups = _firmwarePlugin->factGroups(); @@ -578,7 +588,13 @@ void Vehicle::_offlineFirmwareTypeSettingChanged(QVariant value) { _firmwareType = static_cast(value.toInt()); _firmwarePlugin = _firmwarePluginManager->firmwarePluginForAutopilot(_firmwareType, _vehicleType); + if (_firmwareType == MAV_AUTOPILOT_ARDUPILOTMEGA) { + _capabilityBits |= MAV_PROTOCOL_CAPABILITY_TERRAIN; + } else { + _capabilityBits &= ~MAV_PROTOCOL_CAPABILITY_TERRAIN; + } emit firmwareTypeChanged(); + emit capabilityBitsChanged(_capabilityBits); } void Vehicle::_offlineVehicleTypeSettingChanged(QVariant value) @@ -692,6 +708,10 @@ void Vehicle::_mavlinkMessageReceived(LinkInterface* link, mavlink_message_t mes return; } + if (!_terrainProtocolHandler->mavlinkMessageReceived(message)) { + return; + } + switch (message.msgid) { case MAVLINK_MSG_ID_HOME_POSITION: _handleHomePosition(message); @@ -1300,6 +1320,7 @@ void Vehicle::_setCapabilities(uint64_t capabilityBits) qCDebug(VehicleLog) << QString("Vehicle %1 MISSION_COMMAND_INT").arg(_capabilityBits & MAV_PROTOCOL_CAPABILITY_COMMAND_INT ? supports : doesNotSupport); qCDebug(VehicleLog) << QString("Vehicle %1 GeoFence").arg(_capabilityBits & MAV_PROTOCOL_CAPABILITY_MISSION_FENCE ? supports : doesNotSupport); qCDebug(VehicleLog) << QString("Vehicle %1 RallyPoints").arg(_capabilityBits & MAV_PROTOCOL_CAPABILITY_MISSION_RALLY ? supports : doesNotSupport); + qCDebug(VehicleLog) << QString("Vehicle %1 Terrain").arg(_capabilityBits & MAV_PROTOCOL_CAPABILITY_TERRAIN ? supports : doesNotSupport); } void Vehicle::_handleAutopilotVersion(LinkInterface *link, mavlink_message_t& message) @@ -2906,7 +2927,7 @@ bool Vehicle::supportsMotorInterference() const bool Vehicle::supportsTerrainFrame() const { - return _firmwarePlugin->supportsTerrainFrame(); + return _capabilityBits & MAV_PROTOCOL_CAPABILITY_TERRAIN; } QString Vehicle::vehicleTypeName() const { diff --git a/src/Vehicle/Vehicle.h b/src/Vehicle/Vehicle.h index 7b56c0778..e4f9191da 100644 --- a/src/Vehicle/Vehicle.h +++ b/src/Vehicle/Vehicle.h @@ -21,6 +21,7 @@ #include "UASMessageHandler.h" #include "SettingsFact.h" #include "QGCMapCircle.h" +#include "TerrainFactGroup.h" class UAS; class UASInterface; @@ -38,6 +39,7 @@ class QGCCameraManager; class Joystick; class VehicleObjectAvoidance; class TrajectoryPoints; +class TerrainProtocolHandler; #if defined(QGC_AIRMAP_ENABLED) class AirspaceVehicleManager; @@ -619,7 +621,7 @@ public: Q_PROPERTY(QString hobbsMeter READ hobbsMeter NOTIFY hobbsMeterChanged) Q_PROPERTY(bool vtolInFwdFlight READ vtolInFwdFlight WRITE setVtolInFwdFlight NOTIFY vtolInFwdFlightChanged) Q_PROPERTY(bool highLatencyLink READ highLatencyLink NOTIFY highLatencyLinkChanged) - Q_PROPERTY(bool supportsTerrainFrame READ supportsTerrainFrame NOTIFY firmwareTypeChanged) + Q_PROPERTY(bool supportsTerrainFrame READ supportsTerrainFrame NOTIFY capabilityBitsChanged) Q_PROPERTY(QString priorityLinkName READ priorityLinkName WRITE setPriorityLinkByName NOTIFY priorityLinkNameChanged) Q_PROPERTY(QVariantList links READ links NOTIFY linksChanged) Q_PROPERTY(LinkInterface* priorityLink READ priorityLink NOTIFY priorityLinkNameChanged) @@ -682,6 +684,7 @@ public: Q_PROPERTY(FactGroup* clock READ clockFactGroup CONSTANT) Q_PROPERTY(FactGroup* setpoint READ setpointFactGroup CONSTANT) Q_PROPERTY(FactGroup* estimatorStatus READ estimatorStatusFactGroup CONSTANT) + Q_PROPERTY(FactGroup* terrain READ terrainFactGroup CONSTANT) Q_PROPERTY(int firmwareMajorVersion READ firmwareMajorVersion NOTIFY firmwareVersionChanged) Q_PROPERTY(int firmwareMinorVersion READ firmwareMinorVersion NOTIFY firmwareVersionChanged) @@ -996,6 +999,7 @@ public: FactGroup* setpointFactGroup () { return &_setpointFactGroup; } FactGroup* distanceSensorFactGroup () { return &_distanceSensorFactGroup; } FactGroup* estimatorStatusFactGroup () { return &_estimatorStatusFactGroup; } + FactGroup* terrainFactGroup () { return &_terrainFactGroup; } void setConnectionLostEnabled(bool connectionLostEnabled); @@ -1573,6 +1577,9 @@ private: VehicleSetpointFactGroup _setpointFactGroup; VehicleDistanceSensorFactGroup _distanceSensorFactGroup; VehicleEstimatorStatusFactGroup _estimatorStatusFactGroup; + TerrainFactGroup _terrainFactGroup; + + TerrainProtocolHandler* _terrainProtocolHandler = nullptr; static const char* _rollFactName; static const char* _pitchFactName; @@ -1603,6 +1610,7 @@ private: static const char* _clockFactGroupName; static const char* _distanceSensorFactGroupName; static const char* _estimatorStatusFactGroupName; + static const char* _terrainFactGroupName; static const int _vehicleUIUpdateRateMSecs = 100; diff --git a/src/comm/MockLink.cc b/src/comm/MockLink.cc index 4ffd1c071..1d5311fcd 100644 --- a/src/comm/MockLink.cc +++ b/src/comm/MockLink.cc @@ -998,7 +998,7 @@ void MockLink::_respondWithAutopilotVersion(void) _vehicleComponentId, _mavlinkChannel, &msg, - MAV_PROTOCOL_CAPABILITY_MAVLINK2 | (_firmwareType == MAV_AUTOPILOT_ARDUPILOTMEGA ? 0 : MAV_PROTOCOL_CAPABILITY_MISSION_FENCE | MAV_PROTOCOL_CAPABILITY_MISSION_RALLY), + MAV_PROTOCOL_CAPABILITY_MAVLINK2 | MAV_PROTOCOL_CAPABILITY_MISSION_FENCE | MAV_PROTOCOL_CAPABILITY_MISSION_RALLY | (_firmwareType == MAV_AUTOPILOT_ARDUPILOTMEGA ? MAV_PROTOCOL_CAPABILITY_TERRAIN : 0), flightVersion, // flight_sw_version, 0, // middleware_sw_version, 0, // os_sw_version, -- 2.22.0