From 138bdfa997321edfdf250ab6277919466b5d59d1 Mon Sep 17 00:00:00 2001 From: DonLakeFlyer Date: Tue, 15 Sep 2020 10:52:33 -0700 Subject: [PATCH] Landing Pattern: Loiter to alt optional support --- qgroundcontrol.pro | 2 + src/MissionManager/CameraSection.h | 4 + src/MissionManager/CameraSectionTest.cc | 78 ++- src/MissionManager/CameraSectionTest.h | 9 +- .../FWLandingPattern.FactMetaData.json | 62 +- src/MissionManager/FWLandingPatternTest.cc | 150 +---- src/MissionManager/FWLandingPatternTest.h | 30 +- .../FixedWingLandingComplexItem.cc | 484 ++------------- .../FixedWingLandingComplexItem.h | 89 +-- src/MissionManager/LandingComplexItem.cc | 553 +++++++++++++++++- src/MissionManager/LandingComplexItem.h | 173 ++++-- src/MissionManager/LandingComplexItemTest.cc | 378 ++++++++++++ src/MissionManager/LandingComplexItemTest.h | 123 ++++ src/MissionManager/MissionController.cc | 6 +- src/MissionManager/VTOLLandingComplexItem.cc | 447 +------------- src/MissionManager/VTOLLandingComplexItem.h | 83 +-- .../VTOLLandingPattern.FactMetaData.json | 20 +- src/MissionManager/VisualMissionItemTest.cc | 4 +- src/MissionManager/VisualMissionItemTest.h | 2 +- src/PlanView/FWLandingPatternEditor.qml | 30 +- src/PlanView/FWLandingPatternMapVisual.qml | 85 ++- src/PlanView/VTOLLandingPatternEditor.qml | 38 +- src/PlanView/VTOLLandingPatternMapVisual.qml | 45 +- src/qgcunittest/MultiSignalSpy.cc | 2 +- src/qgcunittest/UnitTest.cc | 5 + src/qgcunittest/UnitTest.h | 2 + src/qgcunittest/UnitTestList.cc | 2 + 27 files changed, 1549 insertions(+), 1357 deletions(-) create mode 100644 src/MissionManager/LandingComplexItemTest.cc create mode 100644 src/MissionManager/LandingComplexItemTest.h diff --git a/qgroundcontrol.pro b/qgroundcontrol.pro index d2d0548a0..31eabff65 100644 --- a/qgroundcontrol.pro +++ b/qgroundcontrol.pro @@ -472,6 +472,7 @@ DebugBuild { PX4FirmwarePlugin { PX4FirmwarePluginFactory { APMFirmwarePlugin { src/MissionManager/CameraSectionTest.h \ src/MissionManager/CorridorScanComplexItemTest.h \ src/MissionManager/FWLandingPatternTest.h \ + src/MissionManager/LandingComplexItemTest.h \ src/MissionManager/MissionCommandTreeEditorTest.h \ src/MissionManager/MissionCommandTreeTest.h \ src/MissionManager/MissionControllerManagerTest.h \ @@ -519,6 +520,7 @@ DebugBuild { PX4FirmwarePlugin { PX4FirmwarePluginFactory { APMFirmwarePlugin { src/MissionManager/CameraSectionTest.cc \ src/MissionManager/CorridorScanComplexItemTest.cc \ src/MissionManager/FWLandingPatternTest.cc \ + src/MissionManager/LandingComplexItemTest.cc \ src/MissionManager/MissionCommandTreeEditorTest.cc \ src/MissionManager/MissionCommandTreeTest.cc \ src/MissionManager/MissionControllerManagerTest.cc \ diff --git a/src/MissionManager/CameraSection.h b/src/MissionManager/CameraSection.h index 0d9375c79..ed410ecd7 100644 --- a/src/MissionManager/CameraSection.h +++ b/src/MissionManager/CameraSection.h @@ -17,6 +17,8 @@ #define VIDEO_CAPTURE_STATUS_INTERVAL 0.2 //-- Send capture status every 5 seconds class PlanMasterController; +class CameraSectionTest; + class CameraSection : public Section { @@ -131,4 +133,6 @@ private: static const char* _cameraPhotoIntervalDistanceName; static const char* _cameraPhotoIntervalTimeName; static const char* _cameraModeName; + + friend CameraSectionTest; }; diff --git a/src/MissionManager/CameraSectionTest.cc b/src/MissionManager/CameraSectionTest.cc index c415f8afd..ccc1463e8 100644 --- a/src/MissionManager/CameraSectionTest.cc +++ b/src/MissionManager/CameraSectionTest.cc @@ -12,6 +12,8 @@ #include "MissionCommandTree.h" #include "MissionCommandUIInfo.h" +#include + CameraSectionTest::CameraSectionTest(void) : _spyCamera (nullptr) , _spySection (nullptr) @@ -44,11 +46,24 @@ void CameraSectionTest::init(void) _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), + MissionItem(0, + MAV_CMD_DO_MOUNT_CONTROL, + MAV_FRAME_MISSION, + 10.1234, 0, 20.1234, // pitch, roll, yaw + 0, 0, 0, // alt, lat, lon (all 0 since unused) + MAV_MOUNT_MODE_MAVLINK_TARGETING, // control gimbal with pitch, roll, yaw settings + true, false), this); _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), + MissionItem(0, + MAV_CMD_IMAGE_START_CAPTURE, + MAV_FRAME_MISSION, + 0, // Reserved, must be 0 + 48, // time interval + 0, // 0 = capture forever + NAN, NAN, NAN, NAN, // Reserved + true, false), this); _validDistanceItem = new SimpleMissionItem(_masterController, false, // flyView @@ -615,12 +630,12 @@ void CameraSectionTest::_testScanForGimbalSection(void) Mission Param #5 WIP: latitude in degrees * 1E7, set if appropriate mount mode. Mission Param #6 WIP: longitude in degrees * 1E7, set if appropriate mount mode. Mission Param #7 MAV_MOUNT_MODE enum value -*/ + */ // Gimbal command but incorrect settings SimpleMissionItem invalidSimpleItem(_masterController, false /* flyView */, _validGimbalItem->missionItem(), nullptr); - invalidSimpleItem.missionItem().setParam2(10); // roll is not supported + invalidSimpleItem.missionItem().setParam2(10); // roll is not supported, should be 0 visualItems.append(&invalidSimpleItem); QCOMPARE(_cameraSection->scanForSection(&visualItems, scanIndex), false); QCOMPARE(visualItems.count(), 1); @@ -629,7 +644,7 @@ void CameraSectionTest::_testScanForGimbalSection(void) visualItems.clear(); invalidSimpleItem.missionItem() = _validGimbalItem->missionItem(); - invalidSimpleItem.missionItem().setParam4(10); // alt is not supported + invalidSimpleItem.missionItem().setParam4(10); // alt is not supported, should be 0 visualItems.append(&invalidSimpleItem); QCOMPARE(_cameraSection->scanForSection(&visualItems, scanIndex), false); QCOMPARE(visualItems.count(), 1); @@ -638,7 +653,7 @@ void CameraSectionTest::_testScanForGimbalSection(void) visualItems.clear(); invalidSimpleItem.missionItem() = _validGimbalItem->missionItem(); - invalidSimpleItem.missionItem().setParam5(10); // lat is not supported + invalidSimpleItem.missionItem().setParam5(10); // lat is not supported, should be 0 visualItems.append(&invalidSimpleItem); QCOMPARE(_cameraSection->scanForSection(&visualItems, scanIndex), false); QCOMPARE(visualItems.count(), 1); @@ -647,7 +662,7 @@ void CameraSectionTest::_testScanForGimbalSection(void) visualItems.clear(); invalidSimpleItem.missionItem() = _validGimbalItem->missionItem(); - invalidSimpleItem.missionItem().setParam6(10); // lon is not supported + invalidSimpleItem.missionItem().setParam6(10); // lon is not supported, should be 0 visualItems.append(&invalidSimpleItem); QCOMPARE(_cameraSection->scanForSection(&visualItems, scanIndex), false); QCOMPARE(visualItems.count(), 1); @@ -709,14 +724,27 @@ void CameraSectionTest::_testScanForCameraModeSection(void) */ // Mode command but incorrect settings + 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); - QCOMPARE(visualItems.count(), 1); - QCOMPARE(_cameraSection->specifyCameraMode(), false); - QCOMPARE(_cameraSection->settingsSpecified(), false); - visualItems.clear(); + std::function rgSetParamFns[] = { + &MissionItem::setParam1, + &MissionItem::setParam2, + &MissionItem::setParam3, + &MissionItem::setParam4, + &MissionItem::setParam5, + &MissionItem::setParam6, + &MissionItem::setParam7 + }; + + for (int fnIndex=2; fnIndex<7; fnIndex++) { + rgSetParamFns[fnIndex](invalidSimpleItem.missionItem(), 0); // should be NaN + visualItems.append(&invalidSimpleItem); + QCOMPARE(_cameraSection->scanForSection(&visualItems, scanIndex), false); + QCOMPARE(visualItems.count(), 1); + QCOMPARE(_cameraSection->specifyCameraMode(), false); + QCOMPARE(_cameraSection->settingsSpecified(), false); + visualItems.clear(); + } } void CameraSectionTest::_testScanForPhotoIntervalTimeSection(void) @@ -1135,3 +1163,25 @@ SimpleMissionItem* CameraSectionTest::createValidStopTimeItem(PlanMasterControll MissionItem(1, MAV_CMD_IMAGE_STOP_CAPTURE, MAV_FRAME_MISSION, 0, qQNaN(), qQNaN(), qQNaN(), qQNaN(), qQNaN(), qQNaN(), true, false), parent); } + +SimpleMissionItem* CameraSectionTest::createInvalidStopVideoItem(PlanMasterController* masterController, QObject* parent) +{ + SimpleMissionItem* invalidSimpleItem = createValidStopVideoItem(masterController, parent); + invalidSimpleItem->missionItem().setParam1(10); // must be 0 to be valid for scan + return invalidSimpleItem; +} + + +SimpleMissionItem* CameraSectionTest::createInvalidStopDistanceItem(PlanMasterController* masterController, QObject* parent) +{ + SimpleMissionItem* invalidSimpleItem = createValidStopDistanceItem(masterController, parent); + invalidSimpleItem->missionItem().setParam2(-1); // Should be 0 + return invalidSimpleItem; +} + +SimpleMissionItem* CameraSectionTest::createInvalidStopTimeItem(PlanMasterController* masterController, QObject* parent) +{ + SimpleMissionItem* invalidSimpleItem = createValidStopTimeItem(masterController, parent); + invalidSimpleItem->missionItem().setParam1(1); // Should be 0 + return invalidSimpleItem; +} diff --git a/src/MissionManager/CameraSectionTest.h b/src/MissionManager/CameraSectionTest.h index 5b57d907d..6760b83be 100644 --- a/src/MissionManager/CameraSectionTest.h +++ b/src/MissionManager/CameraSectionTest.h @@ -24,9 +24,12 @@ public: void init(void) override; void cleanup(void) override; - static SimpleMissionItem* createValidStopVideoItem (PlanMasterController* masterController, QObject* parent); - static SimpleMissionItem* createValidStopDistanceItem(PlanMasterController* masterController, QObject* parent); - static SimpleMissionItem* createValidStopTimeItem (PlanMasterController* masterController, QObject* parent); + static SimpleMissionItem* createValidStopVideoItem (PlanMasterController* masterController, QObject* parent); + static SimpleMissionItem* createValidStopDistanceItem (PlanMasterController* masterController, QObject* parent); + static SimpleMissionItem* createValidStopTimeItem (PlanMasterController* masterController, QObject* parent); + static SimpleMissionItem* createInvalidStopVideoItem (PlanMasterController* masterController, QObject* parent); + static SimpleMissionItem* createInvalidStopDistanceItem (PlanMasterController* masterController, QObject* parent); + static SimpleMissionItem* createInvalidStopTimeItem (PlanMasterController* masterController, QObject* parent); private slots: void _testDirty (void); diff --git a/src/MissionManager/FWLandingPattern.FactMetaData.json b/src/MissionManager/FWLandingPattern.FactMetaData.json index 212c049cd..1badb5390 100644 --- a/src/MissionManager/FWLandingPattern.FactMetaData.json +++ b/src/MissionManager/FWLandingPattern.FactMetaData.json @@ -5,75 +5,87 @@ [ { "name": "LandingDistance", - "shortDesc": "Distance between landing and loiter points.", + "shortDesc": "Distance between approach and land points.", "type": "double", "units": "m", "min": 10, "decimalPlaces": 1, - "default": 300.0 + "default": 300.0 }, { "name": "LandingHeading", - "shortDesc": "Heading from loiter point to land point.", + "shortDesc": "Heading from approach to land point.", "type": "double", "units": "deg", "min": 0.0, "max": 360.0, "decimalPlaces": 0, - "default": 270.0 + "default": 270.0 }, { - "name": "LoiterAltitude", - "shortDesc": "Aircraft will proceed to the loiter point and loiter downwards until it reaches this approach altitude. Once altitude is reached the aircraft will proceed to land.", + "name": "FinalApproachAltitude", + "shortDesc": "Altitude to begin landing approach from.", "type": "double", "units": "m", "decimalPlaces": 1, - "default": 40.0 + "default": 40.0 }, { "name": "LoiterRadius", - "shortDesc": "Loiter radius.", + "shortDesc": "Loiter radius.", "type": "double", "decimalPlaces": 1, "min": 1, "units": "m", - "default": 75.0 + "default": 75.0 +}, +{ + "name": "LoiterClockwise", + "shortDesc": "Loiter clockwise around the final approach point.", + "type": "bool", + "default": true }, { "name": "LandingAltitude", - "shortDesc": "Altitude for landing point.", + "shortDesc": "Altitude for landing point.", "type": "double", "units": "m", "decimalPlaces": 1, - "default": 0.0 + "default": 0.0 }, { "name": "GlideSlope", - "shortDesc": "The glide slope between the loiter and landing point.", + "shortDesc": "The glide slope between the loiter and landing point.", "type": "double", "units": "deg", "min": 0.1, "max": 90, "decimalPlaces": 1, - "default": 6.0 + "default": 6.0 +}, +{ + "name": "ValueSetIsDistance", + "shortDesc": "Value controller approach point is distance", + "type": "bool", + "default": false }, { - "name": "ValueSetIsDistance", - "shortDesc": "Value controller loiter point is distance", - "type": "bool", - "default": false + "name": "UseLoiterToAlt", + "shortDesc": "Use a loiter to altitude item for final appoach. Otherwise use a regular waypoint.", + "type": "bool", + "default": true }, { - "name": "StopTakingPhotos", - "shortDesc": "Stop taking photos", - "type": "bool", - "default": true + "name": "StopTakingPhotos", + "shortDesc": "Stop taking photos", + "type": "bool", + "default": true }, { - "name": "StopTakingVideo", - "shortDesc": "Stop taking video", - "type": "bool", - "default": true + "name": "StopTakingVideo", + "shortDesc": "Stop taking video", + "type": "bool", + "default": true } ] } diff --git a/src/MissionManager/FWLandingPatternTest.cc b/src/MissionManager/FWLandingPatternTest.cc index ee969f0ff..7e662dbf8 100644 --- a/src/MissionManager/FWLandingPatternTest.cc +++ b/src/MissionManager/FWLandingPatternTest.cc @@ -22,18 +22,15 @@ void FWLandingPatternTest::init(void) { VisualMissionItemTest::init(); - rgSignals[dirtyChangedIndex] = SIGNAL(dirtyChanged(bool)); - _fwItem = new FixedWingLandingComplexItem(_masterController, false /* flyView */, this); - _multiSpy = new MultiSignalSpy(); - QCOMPARE(_multiSpy->init(_fwItem, rgSignals, cSignals), true); + _createSpy(_fwItem, &_viSpy); // Start in a clean state QVERIFY(!_fwItem->dirty()); _fwItem->setLandingCoordinate(QGeoCoordinate(47, -122)); _fwItem->setDirty(false); QVERIFY(!_fwItem->dirty()); - _multiSpy->clearAllSignals(); + _viSpy->clearAllSignals(); _validStopVideoItem = CameraSectionTest::createValidStopTimeItem(_masterController, this); _validStopDistanceItem = CameraSectionTest::createValidStopTimeItem(_masterController, this); @@ -43,7 +40,7 @@ void FWLandingPatternTest::init(void) void FWLandingPatternTest::cleanup(void) { delete _fwItem; - delete _multiSpy; + delete _viSpy; delete _validStopVideoItem; delete _validStopDistanceItem; delete _validStopTimeItem; @@ -57,142 +54,35 @@ void FWLandingPatternTest::_testDefaults(void) QCOMPARE(_fwItem->stopTakingVideo()->rawValue().toBool(), true); } -void FWLandingPatternTest::_testItemCount(void) -{ - QList items; - - _fwItem->stopTakingPhotos()->setRawValue(true); - _fwItem->stopTakingVideo()->setRawValue(true); - _fwItem->appendMissionItems(items, this); - QCOMPARE(items.count(), 3 + CameraSection::stopTakingPhotosCommandCount() + CameraSection::stopTakingVideoCommandCount()); - QCOMPARE(items.count() - 1, _fwItem->lastSequenceNumber()); - items.clear(); - - _fwItem->stopTakingPhotos()->setRawValue(false); - _fwItem->stopTakingVideo()->setRawValue(false); - _fwItem->appendMissionItems(items, this); - QCOMPARE(items.count(), 3); - QCOMPARE(items.count() - 1, _fwItem->lastSequenceNumber()); - items.clear(); -} - -void FWLandingPatternTest::_testAppendSectionItems(void) -{ - QList rgMissionItems; - - // Create the set of appended mission items - _fwItem->stopTakingPhotos()->setRawValue(true); - _fwItem->stopTakingVideo()->setRawValue(true); - _fwItem->appendMissionItems(rgMissionItems, this); - - // Convert to visual items - QmlObjectListModel* simpleItems = new QmlObjectListModel(this); - - for (MissionItem* item: rgMissionItems) { - SimpleMissionItem* simpleItem = new SimpleMissionItem(_masterController, false /* flyView */, false /* forLoad */, 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 */, _masterController)); - QCOMPARE(simpleItems->count(), 1); - _validateItem(simpleItems->value(0)); - - // Reset - simpleItems->deleteLater(); - rgMissionItems.clear(); - - // Check for no stop camera actions - _fwItem->stopTakingPhotos()->setRawValue(false); - _fwItem->stopTakingVideo()->setRawValue(false); - _fwItem->appendMissionItems(rgMissionItems, this); - simpleItems = new QmlObjectListModel(this); - for (MissionItem* item: rgMissionItems) { - SimpleMissionItem* simpleItem = new SimpleMissionItem(_masterController, false /* flyView */, false /* forLoad */, simpleItems); - simpleItem->missionItem() = *item; - simpleItems->append(simpleItem); - } - QVERIFY(FixedWingLandingComplexItem::scanForItem(simpleItems, false /* flyView */, _masterController)); - QCOMPARE(simpleItems->count(), 1); - _validateItem(simpleItems->value(0)); - - // Reset - simpleItems->deleteLater(); - rgMissionItems.clear(); -} - void FWLandingPatternTest::_testDirty(void) { _fwItem->setDirty(true); QVERIFY(_fwItem->dirty()); - QVERIFY(_multiSpy->checkOnlySignalByMask(dirtyChangedMask)); - QVERIFY(_multiSpy->pullBoolFromSignalIndex(dirtyChangedIndex)); + QVERIFY(_viSpy->checkOnlySignalByMask(dirtyChangedMask)); + QVERIFY(_viSpy->pullBoolFromSignalIndex(dirtyChangedIndex)); _fwItem->setDirty(false); - _multiSpy->clearAllSignals(); + _viSpy->clearAllSignals(); // These facts should set dirty when changed QList rgFacts; - rgFacts << _fwItem->loiterAltitude() - << _fwItem->landingHeading() - << _fwItem->loiterRadius() - << _fwItem->landingAltitude() - << _fwItem->landingDistance() - << _fwItem->glideSlope() - << _fwItem->stopTakingPhotos() - << _fwItem->stopTakingVideo() + rgFacts << _fwItem->glideSlope() << _fwItem->valueSetIsDistance(); for(Fact* fact: rgFacts) { qDebug() << fact->name(); QVERIFY(!_fwItem->dirty()); - if (fact->typeIsBool()) { - fact->setRawValue(!fact->rawValue().toBool()); - } else { - fact->setRawValue(fact->rawValue().toDouble() + 1); - } - QVERIFY(_multiSpy->checkSignalByMask(dirtyChangedMask)); - QVERIFY(_multiSpy->pullBoolFromSignalIndex(dirtyChangedIndex)); - _fwItem->setDirty(false); - _multiSpy->clearAllSignals(); - } - rgFacts.clear(); - - // These bool properties should set dirty when changed - QList rgBoolNames; - rgBoolNames << "loiterClockwise" - << "altitudesAreRelative"; - const QMetaObject* metaObject = _fwItem->metaObject(); - for(const char* boolName: rgBoolNames) { - qDebug() << boolName; - QVERIFY(!_fwItem->dirty()); - QMetaProperty boolProp = metaObject->property(metaObject->indexOfProperty(boolName)); - QVERIFY(boolProp.write(_fwItem, !boolProp.read(_fwItem).toBool())); - QVERIFY(_multiSpy->checkSignalByMask(dirtyChangedMask)); - QVERIFY(_multiSpy->pullBoolFromSignalIndex(dirtyChangedIndex)); + changeFactValue(fact); + QVERIFY(_viSpy->checkSignalByMask(dirtyChangedMask)); + QVERIFY(_viSpy->pullBoolFromSignalIndex(dirtyChangedIndex)); _fwItem->setDirty(false); - _multiSpy->clearAllSignals(); + _viSpy->clearAllSignals(); } rgFacts.clear(); - - // These coordinates should set dirty when changed - QVERIFY(!_fwItem->dirty()); - _fwItem->setLoiterCoordinate(_fwItem->loiterCoordinate().atDistanceAndAzimuth(1, 0)); - QVERIFY(_multiSpy->checkSignalByMask(dirtyChangedMask)); - QVERIFY(_multiSpy->pullBoolFromSignalIndex(dirtyChangedIndex)); - _fwItem->setDirty(false); - _multiSpy->clearAllSignals(); - QVERIFY(!_fwItem->dirty()); - _fwItem->setLoiterCoordinate(_fwItem->landingCoordinate().atDistanceAndAzimuth(1, 0)); - QVERIFY(_multiSpy->checkSignalByMask(dirtyChangedMask)); - QVERIFY(_multiSpy->pullBoolFromSignalIndex(dirtyChangedIndex)); - _fwItem->setDirty(false); - _multiSpy->clearAllSignals(); } void FWLandingPatternTest::_testSaveLoad(void) { - QJsonArray items; + QJsonArray items; + _fwItem->save(items); QString errorString; @@ -211,20 +101,6 @@ void FWLandingPatternTest::_validateItem(FixedWingLandingComplexItem* newItem) { QVERIFY(newItem); - QVERIFY(fuzzyCompareLatLon(newItem->loiterCoordinate(), _fwItem->loiterCoordinate())); - QVERIFY(fuzzyCompareLatLon(newItem->loiterTangentCoordinate(), _fwItem->loiterTangentCoordinate())); - QVERIFY(fuzzyCompareLatLon(newItem->landingCoordinate(), _fwItem->landingCoordinate())); - - QCOMPARE(newItem->stopTakingPhotos()->rawValue().toBool(), _fwItem->stopTakingPhotos()->rawValue().toBool()); - QCOMPARE(newItem->stopTakingVideo()->rawValue().toBool(), _fwItem->stopTakingVideo()->rawValue().toBool()); - QCOMPARE(newItem->loiterAltitude()->rawValue().toInt(), _fwItem->loiterAltitude()->rawValue().toInt()); - QCOMPARE(newItem->loiterRadius()->rawValue().toInt(), _fwItem->loiterRadius()->rawValue().toInt()); - QCOMPARE(newItem->landingAltitude()->rawValue().toInt(), _fwItem->landingAltitude()->rawValue().toInt()); - QCOMPARE(newItem->landingHeading()->rawValue().toInt(), _fwItem->landingHeading()->rawValue().toInt()); - QCOMPARE(newItem->landingDistance()->rawValue().toInt(), _fwItem->landingDistance()->rawValue().toInt()); QCOMPARE(newItem->glideSlope()->rawValue().toInt(), _fwItem->glideSlope()->rawValue().toInt()); QCOMPARE(newItem->valueSetIsDistance()->rawValue().toBool(), _fwItem->valueSetIsDistance()->rawValue().toBool()); - QCOMPARE(newItem->_loiterClockwise, _fwItem->_loiterClockwise); - QCOMPARE(newItem->_altitudesAreRelative, _fwItem->_altitudesAreRelative); - QCOMPARE(newItem->_landingCoordSet, _fwItem->_landingCoordSet); } diff --git a/src/MissionManager/FWLandingPatternTest.h b/src/MissionManager/FWLandingPatternTest.h index 9a5dda9d1..3d9168f1b 100644 --- a/src/MissionManager/FWLandingPatternTest.h +++ b/src/MissionManager/FWLandingPatternTest.h @@ -25,30 +25,16 @@ public: void cleanup(void) override; private slots: - void _testDirty (void); - void _testItemCount (void); - void _testDefaults (void); - void _testAppendSectionItems (void); - void _testSaveLoad (void); + void _testDirty (void); + void _testDefaults (void); + void _testSaveLoad (void); private: void _validateItem(FixedWingLandingComplexItem* newItem); - enum { - dirtyChangedIndex = 0, - maxSignalIndex, - }; - - enum { - dirtyChangedMask = 1 << dirtyChangedIndex, - }; - - static const size_t cSignals = maxSignalIndex; - const char* rgSignals[cSignals]; - - FixedWingLandingComplexItem* _fwItem = nullptr; - MultiSignalSpy* _multiSpy = nullptr; - SimpleMissionItem* _validStopVideoItem = nullptr; - SimpleMissionItem* _validStopDistanceItem = nullptr; - SimpleMissionItem* _validStopTimeItem = nullptr; + FixedWingLandingComplexItem* _fwItem = nullptr; + MultiSignalSpy* _viSpy = nullptr; + SimpleMissionItem* _validStopVideoItem = nullptr; + SimpleMissionItem* _validStopDistanceItem = nullptr; + SimpleMissionItem* _validStopTimeItem = nullptr; }; diff --git a/src/MissionManager/FixedWingLandingComplexItem.cc b/src/MissionManager/FixedWingLandingComplexItem.cc index 7258f640b..486647360 100644 --- a/src/MissionManager/FixedWingLandingComplexItem.cc +++ b/src/MissionManager/FixedWingLandingComplexItem.cc @@ -21,101 +21,36 @@ QGC_LOGGING_CATEGORY(FixedWingLandingComplexItemLog, "FixedWingLandingComplexIte const QString FixedWingLandingComplexItem::name(tr("Fixed Wing Landing")); -const char* FixedWingLandingComplexItem::settingsGroup = "FixedWingLanding"; -const char* FixedWingLandingComplexItem::jsonComplexItemTypeValue = "fwLandingPattern"; +const char* FixedWingLandingComplexItem::settingsGroup = "FixedWingLanding"; +const char* FixedWingLandingComplexItem::jsonComplexItemTypeValue = "fwLandingPattern"; -const char* FixedWingLandingComplexItem::loiterToLandDistanceName = "LandingDistance"; -const char* FixedWingLandingComplexItem::landingHeadingName = "LandingHeading"; -const char* FixedWingLandingComplexItem::loiterAltitudeName = "LoiterAltitude"; -const char* FixedWingLandingComplexItem::loiterRadiusName = "LoiterRadius"; -const char* FixedWingLandingComplexItem::landingAltitudeName = "LandingAltitude"; -const char* FixedWingLandingComplexItem::glideSlopeName = "GlideSlope"; -const char* FixedWingLandingComplexItem::stopTakingPhotosName = "StopTakingPhotos"; -const char* FixedWingLandingComplexItem::stopTakingVideoName = "StopTakingVideo"; -const char* FixedWingLandingComplexItem::valueSetIsDistanceName = "ValueSetIsDistance"; +const char* FixedWingLandingComplexItem::glideSlopeName = "GlideSlope"; +const char* FixedWingLandingComplexItem::valueSetIsDistanceName = "ValueSetIsDistance"; -const char* FixedWingLandingComplexItem::_jsonLoiterCoordinateKey = "loiterCoordinate"; -const char* FixedWingLandingComplexItem::_jsonLoiterRadiusKey = "loiterRadius"; -const char* FixedWingLandingComplexItem::_jsonLoiterClockwiseKey = "loiterClockwise"; -const char* FixedWingLandingComplexItem::_jsonLandingCoordinateKey = "landCoordinate"; -const char* FixedWingLandingComplexItem::_jsonValueSetIsDistanceKey = "valueSetIsDistance"; -const char* FixedWingLandingComplexItem::_jsonAltitudesAreRelativeKey = "altitudesAreRelative"; -const char* FixedWingLandingComplexItem::_jsonStopTakingPhotosKey = "stopTakingPhotos"; -const char* FixedWingLandingComplexItem::_jsonStopTakingVideoKey = "stopVideoPhotos"; - -// Usage deprecated -const char* FixedWingLandingComplexItem::_jsonFallRateKey = "fallRate"; -const char* FixedWingLandingComplexItem::_jsonLandingAltitudeRelativeKey = "landAltitudeRelative"; -const char* FixedWingLandingComplexItem::_jsonLoiterAltitudeRelativeKey = "loiterAltitudeRelative"; +const char* FixedWingLandingComplexItem::_jsonValueSetIsDistanceKey = "valueSetIsDistance"; FixedWingLandingComplexItem::FixedWingLandingComplexItem(PlanMasterController* masterController, bool flyView, QObject* parent) : LandingComplexItem (masterController, flyView, parent) , _metaDataMap (FactMetaData::createMapFromJsonFile(QStringLiteral(":/json/FWLandingPattern.FactMetaData.json"), this)) - , _landingDistanceFact (settingsGroup, _metaDataMap[loiterToLandDistanceName]) - , _loiterAltitudeFact (settingsGroup, _metaDataMap[loiterAltitudeName]) + , _landingDistanceFact (settingsGroup, _metaDataMap[finalApproachToLandDistanceName]) + , _finalApproachAltitudeFact(settingsGroup, _metaDataMap[finalApproachAltitudeName]) , _loiterRadiusFact (settingsGroup, _metaDataMap[loiterRadiusName]) + , _loiterClockwiseFact (settingsGroup, _metaDataMap[loiterClockwiseName]) , _landingHeadingFact (settingsGroup, _metaDataMap[landingHeadingName]) , _landingAltitudeFact (settingsGroup, _metaDataMap[landingAltitudeName]) , _glideSlopeFact (settingsGroup, _metaDataMap[glideSlopeName]) + , _useLoiterToAltFact (settingsGroup, _metaDataMap[useLoiterToAltName]) , _stopTakingPhotosFact (settingsGroup, _metaDataMap[stopTakingPhotosName]) , _stopTakingVideoFact (settingsGroup, _metaDataMap[stopTakingVideoName]) , _valueSetIsDistanceFact (settingsGroup, _metaDataMap[valueSetIsDistanceName]) { - _editorQml = "qrc:/qml/FWLandingPatternEditor.qml"; - _isIncomplete = false; + _editorQml = "qrc:/qml/FWLandingPatternEditor.qml"; + _isIncomplete = false; _init(); - connect(&_loiterAltitudeFact, &Fact::valueChanged, this, &FixedWingLandingComplexItem::_updateLoiterCoodinateAltitudeFromFact); - connect(&_landingAltitudeFact, &Fact::valueChanged, this, &FixedWingLandingComplexItem::_updateLandingCoodinateAltitudeFromFact); - - connect(&_glideSlopeFact, &Fact::valueChanged, this, &FixedWingLandingComplexItem::_glideSlopeChanged); - - connect(&_stopTakingPhotosFact, &Fact::valueChanged, this, &FixedWingLandingComplexItem::_signalLastSequenceNumberChanged); - connect(&_stopTakingVideoFact, &Fact::valueChanged, this, &FixedWingLandingComplexItem::_signalLastSequenceNumberChanged); - - connect(&_loiterAltitudeFact, &Fact::valueChanged, this, &FixedWingLandingComplexItem::_setDirty); - connect(&_landingAltitudeFact, &Fact::valueChanged, this, &FixedWingLandingComplexItem::_setDirty); - connect(&_landingDistanceFact, &Fact::valueChanged, this, &FixedWingLandingComplexItem::_setDirty); - connect(&_landingHeadingFact, &Fact::valueChanged, this, &FixedWingLandingComplexItem::_setDirty); - connect(&_loiterRadiusFact, &Fact::valueChanged, this, &FixedWingLandingComplexItem::_setDirty); - connect(&_stopTakingPhotosFact, &Fact::valueChanged, this, &FixedWingLandingComplexItem::_setDirty); - connect(&_stopTakingVideoFact, &Fact::valueChanged, this, &FixedWingLandingComplexItem::_setDirty); - connect(&_valueSetIsDistanceFact, &Fact::valueChanged, this, &FixedWingLandingComplexItem::_setDirty); - connect(this, &FixedWingLandingComplexItem::loiterCoordinateChanged, this, &FixedWingLandingComplexItem::_setDirty); - connect(this, &FixedWingLandingComplexItem::landingCoordinateChanged, this, &FixedWingLandingComplexItem::_setDirty); - connect(this, &FixedWingLandingComplexItem::loiterClockwiseChanged, this, &FixedWingLandingComplexItem::_setDirty); - connect(this, &FixedWingLandingComplexItem::altitudesAreRelativeChanged, this, &FixedWingLandingComplexItem::_setDirty); - connect(this, &FixedWingLandingComplexItem::valueSetIsDistanceChanged, this, &FixedWingLandingComplexItem::_setDirty); - - connect(this, &FixedWingLandingComplexItem::altitudesAreRelativeChanged, this, &FixedWingLandingComplexItem::_amslEntryAltChanged); - connect(this, &FixedWingLandingComplexItem::altitudesAreRelativeChanged, this, &FixedWingLandingComplexItem::_amslExitAltChanged); - connect(_missionController, &MissionController::plannedHomePositionChanged, this, &FixedWingLandingComplexItem::_amslEntryAltChanged); - connect(_missionController, &MissionController::plannedHomePositionChanged, this, &FixedWingLandingComplexItem::_amslExitAltChanged); - connect(&_loiterAltitudeFact, &Fact::valueChanged, this, &FixedWingLandingComplexItem::_amslEntryAltChanged); - connect(&_landingAltitudeFact, &Fact::valueChanged, this, &FixedWingLandingComplexItem::_amslExitAltChanged); - connect(this, &FixedWingLandingComplexItem::amslEntryAltChanged, this, &FixedWingLandingComplexItem::maxAMSLAltitudeChanged); - connect(this, &FixedWingLandingComplexItem::amslExitAltChanged, this, &FixedWingLandingComplexItem::minAMSLAltitudeChanged); - - connect(this, &FixedWingLandingComplexItem::landingCoordSetChanged, this, &FixedWingLandingComplexItem::readyForSaveStateChanged); - connect(this, &FixedWingLandingComplexItem::wizardModeChanged, this, &FixedWingLandingComplexItem::readyForSaveStateChanged); - - connect(this, &FixedWingLandingComplexItem::loiterTangentCoordinateChanged, this, &FixedWingLandingComplexItem::_updateFlightPathSegmentsSignal); - connect(this, &FixedWingLandingComplexItem::landingCoordinateChanged, this, &FixedWingLandingComplexItem::_updateFlightPathSegmentsSignal); - connect(&_loiterAltitudeFact, &Fact::valueChanged, this, &FixedWingLandingComplexItem::_updateFlightPathSegmentsSignal); - connect(&_landingAltitudeFact, &Fact::valueChanged, this, &FixedWingLandingComplexItem::_updateFlightPathSegmentsSignal); - connect(this, &FixedWingLandingComplexItem::altitudesAreRelativeChanged, this, &FixedWingLandingComplexItem::_updateFlightPathSegmentsSignal); - connect(_missionController, &MissionController::plannedHomePositionChanged, this, &FixedWingLandingComplexItem::_updateFlightPathSegmentsSignal); - - // The follow is used to compress multiple recalc calls in a row to into a single call. - connect(this, &FixedWingLandingComplexItem::_updateFlightPathSegmentsSignal, this, &FixedWingLandingComplexItem::_updateFlightPathSegmentsDontCallDirectly, Qt::QueuedConnection); - qgcApp()->addCompressedSignal(QMetaMethod::fromSignal(&FixedWingLandingComplexItem::_updateFlightPathSegmentsSignal)); - - if (_masterController->controllerVehicle()->apmFirmware()) { - // ArduPilot does not support camera commands - _stopTakingVideoFact.setRawValue(false); - _stopTakingPhotosFact.setRawValue(false); - } + connect(&_glideSlopeFact, &Fact::valueChanged, this, &FixedWingLandingComplexItem::_glideSlopeChanged); + connect(&_valueSetIsDistanceFact, &Fact::valueChanged, this, &FixedWingLandingComplexItem::_setDirty); if (_valueSetIsDistanceFact.rawValue().toBool()) { _recalcFromHeadingAndDistanceChange(); @@ -125,115 +60,32 @@ FixedWingLandingComplexItem::FixedWingLandingComplexItem(PlanMasterController* m setDirty(false); } -int FixedWingLandingComplexItem::lastSequenceNumber(void) const -{ - // Fixed items are: - // land start, loiter, land - // Optional items are: - // stop photos/video - return _sequenceNumber + 2 + (_stopTakingPhotosFact.rawValue().toBool() ? 2 : 0) + (_stopTakingVideoFact.rawValue().toBool() ? 1 : 0); -} - -void FixedWingLandingComplexItem::setDirty(bool dirty) -{ - if (_dirty != dirty) { - _dirty = dirty; - emit dirtyChanged(_dirty); - } -} - void FixedWingLandingComplexItem::save(QJsonArray& missionItems) { - QJsonObject saveObject; - - saveObject[JsonHelper::jsonVersionKey] = 2; - saveObject[VisualMissionItem::jsonTypeKey] = VisualMissionItem::jsonTypeComplexItemValue; - saveObject[ComplexMissionItem::jsonComplexItemTypeKey] = jsonComplexItemTypeValue; - - QGeoCoordinate coordinate; - QJsonValue jsonCoordinate; - - coordinate = _loiterCoordinate; - coordinate.setAltitude(_loiterAltitudeFact.rawValue().toDouble()); - JsonHelper::saveGeoCoordinate(coordinate, true /* writeAltitude */, jsonCoordinate); - saveObject[_jsonLoiterCoordinateKey] = jsonCoordinate; - - coordinate = _landingCoordinate; - coordinate.setAltitude(_landingAltitudeFact.rawValue().toDouble()); - JsonHelper::saveGeoCoordinate(coordinate, true /* writeAltitude */, jsonCoordinate); - saveObject[_jsonLandingCoordinateKey] = jsonCoordinate; + QJsonObject saveObject = _save(); - saveObject[_jsonLoiterRadiusKey] = _loiterRadiusFact.rawValue().toDouble(); - saveObject[_jsonStopTakingPhotosKey] = _stopTakingPhotosFact.rawValue().toBool(); - saveObject[_jsonStopTakingVideoKey] = _stopTakingVideoFact.rawValue().toBool(); - saveObject[_jsonLoiterClockwiseKey] = _loiterClockwise; - saveObject[_jsonAltitudesAreRelativeKey] = _altitudesAreRelative; - saveObject[_jsonValueSetIsDistanceKey] = _valueSetIsDistanceFact.rawValue().toBool(); + saveObject[JsonHelper::jsonVersionKey] = 2; + saveObject[VisualMissionItem::jsonTypeKey] = VisualMissionItem::jsonTypeComplexItemValue; + saveObject[ComplexMissionItem::jsonComplexItemTypeKey] = jsonComplexItemTypeValue; + saveObject[_jsonValueSetIsDistanceKey] = _valueSetIsDistanceFact.rawValue().toBool(); missionItems.append(saveObject); } -void FixedWingLandingComplexItem::setSequenceNumber(int sequenceNumber) -{ - if (_sequenceNumber != sequenceNumber) { - _sequenceNumber = sequenceNumber; - emit sequenceNumberChanged(sequenceNumber); - emit lastSequenceNumberChanged(lastSequenceNumber()); - } -} - bool FixedWingLandingComplexItem::load(const QJsonObject& complexObject, int sequenceNumber, QString& errorString) { QList keyInfoList = { - { JsonHelper::jsonVersionKey, QJsonValue::Double, true }, - { VisualMissionItem::jsonTypeKey, QJsonValue::String, true }, - { ComplexMissionItem::jsonComplexItemTypeKey, QJsonValue::String, true }, - { _jsonLoiterCoordinateKey, QJsonValue::Array, true }, - { _jsonLoiterRadiusKey, QJsonValue::Double, true }, - { _jsonLoiterClockwiseKey, QJsonValue::Bool, true }, - { _jsonLandingCoordinateKey, QJsonValue::Array, true }, - { _jsonStopTakingPhotosKey, QJsonValue::Bool, false }, - { _jsonStopTakingVideoKey, QJsonValue::Bool, false }, + { JsonHelper::jsonVersionKey, QJsonValue::Double, true }, }; if (!JsonHelper::validateKeys(complexObject, keyInfoList, errorString)) { return false; } - QString itemType = complexObject[VisualMissionItem::jsonTypeKey].toString(); - QString complexType = complexObject[ComplexMissionItem::jsonComplexItemTypeKey].toString(); - if (itemType != VisualMissionItem::jsonTypeComplexItemValue || complexType != jsonComplexItemTypeValue) { - errorString = tr("%1 does not support loading this complex mission item type: %2:%3").arg(qgcApp()->applicationName()).arg(itemType).arg(complexType); - return false; - } - - setSequenceNumber(sequenceNumber); - - _ignoreRecalcSignals = true; - int version = complexObject[JsonHelper::jsonVersionKey].toInt(); if (version == 1) { - QList v1KeyInfoList = { - { _jsonLoiterAltitudeRelativeKey, QJsonValue::Bool, true }, - { _jsonLandingAltitudeRelativeKey, QJsonValue::Bool, true }, - }; - if (!JsonHelper::validateKeys(complexObject, v1KeyInfoList, errorString)) { - return false; - } - - bool loiterAltitudeRelative = complexObject[_jsonLoiterAltitudeRelativeKey].toBool(); - bool landingAltitudeRelative = complexObject[_jsonLandingAltitudeRelativeKey].toBool(); - if (loiterAltitudeRelative != landingAltitudeRelative) { - qgcApp()->showAppMessage(tr("Fixed Wing Landing Pattern: " - "Setting the loiter and landing altitudes with different settings for altitude relative is no longer supported. " - "Both have been set to altitude relative. Be sure to adjust/check your plan prior to flight.")); - _altitudesAreRelative = true; - } else { - _altitudesAreRelative = loiterAltitudeRelative; - } _valueSetIsDistanceFact.setRawValue(true); } else if (version == 2) { QList v2KeyInfoList = { - { _jsonAltitudesAreRelativeKey, QJsonValue::Bool, true }, { _jsonValueSetIsDistanceKey, QJsonValue::Bool, true }, }; if (!JsonHelper::validateKeys(complexObject, v2KeyInfoList, errorString)) { @@ -241,7 +93,6 @@ bool FixedWingLandingComplexItem::load(const QJsonObject& complexObject, int seq return false; } - _altitudesAreRelative = complexObject[_jsonAltitudesAreRelativeKey].toBool(); _valueSetIsDistanceFact.setRawValue(complexObject[_jsonValueSetIsDistanceKey].toBool()); } else { errorString = tr("%1 complex item version %2 not supported").arg(jsonComplexItemTypeValue).arg(version); @@ -249,82 +100,10 @@ bool FixedWingLandingComplexItem::load(const QJsonObject& complexObject, int seq return false; } - QGeoCoordinate coordinate; - if (!JsonHelper::loadGeoCoordinate(complexObject[_jsonLoiterCoordinateKey], true /* altitudeRequired */, coordinate, errorString)) { - return false; - } - _loiterCoordinate = coordinate; - _loiterAltitudeFact.setRawValue(coordinate.altitude()); - - if (!JsonHelper::loadGeoCoordinate(complexObject[_jsonLandingCoordinateKey], true /* altitudeRequired */, coordinate, errorString)) { - return false; - } - _landingCoordinate = coordinate; - _landingAltitudeFact.setRawValue(coordinate.altitude()); - - _loiterRadiusFact.setRawValue(complexObject[_jsonLoiterRadiusKey].toDouble()); - _loiterClockwise = complexObject[_jsonLoiterClockwiseKey].toBool(); - - if (complexObject.contains(_jsonStopTakingPhotosKey)) { - _stopTakingPhotosFact.setRawValue(complexObject[_jsonStopTakingPhotosKey].toBool()); - } else { - _stopTakingPhotosFact.setRawValue(false); - } - if (complexObject.contains(_jsonStopTakingVideoKey)) { - _stopTakingVideoFact.setRawValue(complexObject[_jsonStopTakingVideoKey].toBool()); - } else { - _stopTakingVideoFact.setRawValue(false); - } - - _calcGlideSlope(); - - _landingCoordSet = true; - - _ignoreRecalcSignals = false; - - _recalcFromCoordinateChange(); - emit coordinateChanged(this->coordinate()); // This will kick off terrain query - - return true; -} - -double FixedWingLandingComplexItem::greatestDistanceTo(const QGeoCoordinate &other) const -{ - return qMax(_loiterCoordinate.distanceTo(other),_landingCoordinate.distanceTo(other)); -} - -bool FixedWingLandingComplexItem::specifiesCoordinate(void) const -{ - return true; -} - -MissionItem* FixedWingLandingComplexItem::createDoLandStartItem(int seqNum, QObject* parent) -{ - return new MissionItem(seqNum, // sequence number - MAV_CMD_DO_LAND_START, // MAV_CMD - MAV_FRAME_MISSION, // MAV_FRAME - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, // param 1-7 - true, // autoContinue - false, // isCurrentItem - parent); + return _load(complexObject, sequenceNumber, jsonComplexItemTypeValue, version == 1 /* useDeprecatedRelAltKeys */, errorString); } -MissionItem* FixedWingLandingComplexItem::createLoiterToAltItem(int seqNum, bool altRel, double loiterRadius, double lat, double lon, double alt, QObject* parent) -{ - return new MissionItem(seqNum, - MAV_CMD_NAV_LOITER_TO_ALT, - altRel ? MAV_FRAME_GLOBAL_RELATIVE_ALT : MAV_FRAME_GLOBAL, - 1.0, // Heading required = true - loiterRadius, // Loiter radius - 0.0, // param 3 - unused - 1.0, // Exit crosstrack - tangent of loiter to land point - lat, lon, alt, - true, // autoContinue - false, // isCurrentItem - parent); -} - -MissionItem* FixedWingLandingComplexItem::createLandItem(int seqNum, bool altRel, double lat, double lon, double alt, QObject* parent) +MissionItem* FixedWingLandingComplexItem::_createLandItem(int seqNum, bool altRel, double lat, double lon, double alt, QObject* parent) { return new MissionItem(seqNum, MAV_CMD_NAV_LAND, @@ -337,202 +116,10 @@ MissionItem* FixedWingLandingComplexItem::createLandItem(int seqNum, bool altRel } -void FixedWingLandingComplexItem::appendMissionItems(QList& items, QObject* missionItemParent) -{ - int seqNum = _sequenceNumber; - - // IMPORTANT NOTE: Any changes here must also be taken into account in scanForItem - - MissionItem* item = createDoLandStartItem(seqNum++, missionItemParent); - items.append(item); - - - if (_stopTakingPhotosFact.rawValue().toBool()) { - CameraSection::appendStopTakingPhotos(items, seqNum, missionItemParent); - } - - if (_stopTakingVideoFact.rawValue().toBool()) { - CameraSection::appendStopTakingVideo(items, seqNum, missionItemParent); - } - - double loiterRadius = _loiterRadiusFact.rawValue().toDouble() * (_loiterClockwise ? 1.0 : -1.0); - item = createLoiterToAltItem(seqNum++, - _altitudesAreRelative, - loiterRadius, - _loiterCoordinate.latitude(), _loiterCoordinate.longitude(), _loiterAltitudeFact.rawValue().toDouble(), - missionItemParent); - items.append(item); - - item = createLandItem(seqNum++, - _altitudesAreRelative, - _landingCoordinate.latitude(), _landingCoordinate.longitude(), _landingAltitudeFact.rawValue().toDouble(), - missionItemParent); - items.append(item); -} - -bool FixedWingLandingComplexItem::scanForItem(QmlObjectListModel* visualItems, bool flyView, PlanMasterController* masterController) -{ - qCDebug(FixedWingLandingComplexItemLog) << "FixedWingLandingComplexItem::scanForItem count" << visualItems->count(); - - if (visualItems->count() < 3) { - return false; - } - - // A valid fixed wing landing pattern is comprised of the follow commands in this order at the end of the item list: - // MAV_CMD_DO_LAND_START - required - // Stop taking photos sequence - optional - // Stop taking video sequence - optional - // MAV_CMD_NAV_LOITER_TO_ALT - required - // MAV_CMD_NAV_LAND - required - - // Start looking for the commands in reverse order from the end of the list - - int scanIndex = visualItems->count() - 1; - - if (scanIndex < 0 || scanIndex > visualItems->count() - 1) { - return false; - } - SimpleMissionItem* item = visualItems->value(scanIndex--); - if (!item) { - return false; - } - MissionItem& missionItemLand = item->missionItem(); - if (missionItemLand.command() != MAV_CMD_NAV_LAND || - !(missionItemLand.frame() == MAV_FRAME_GLOBAL_RELATIVE_ALT || missionItemLand.frame() == MAV_FRAME_GLOBAL) || - missionItemLand.param1() != 0 || missionItemLand.param2() != 0 || missionItemLand.param3() != 0 || missionItemLand.param4() != 0) { - return false; - } - MAV_FRAME landPointFrame = missionItemLand.frame(); - - if (scanIndex < 0 || scanIndex > visualItems->count() - 1) { - return false; - } - item = visualItems->value(scanIndex); - if (!item) { - return false; - } - MissionItem& missionItemLoiter = item->missionItem(); - if (missionItemLoiter.command() != MAV_CMD_NAV_LOITER_TO_ALT || - missionItemLoiter.frame() != landPointFrame || - missionItemLoiter.param1() != 1.0 || missionItemLoiter.param3() != 0 || missionItemLoiter.param4() != 1.0) { - return false; - } - - scanIndex -= CameraSection::stopTakingVideoCommandCount(); - bool stopTakingVideo = CameraSection::scanStopTakingVideo(visualItems, scanIndex, false /* removeScannedItems */); - if (!stopTakingVideo) { - scanIndex += CameraSection::stopTakingVideoCommandCount(); - } - - scanIndex -= CameraSection::stopTakingPhotosCommandCount(); - bool stopTakingPhotos = CameraSection::scanStopTakingPhotos(visualItems, scanIndex, false /* removeScannedItems */); - if (!stopTakingPhotos) { - scanIndex += CameraSection::stopTakingPhotosCommandCount(); - } - - scanIndex--; - if (scanIndex < 0 || scanIndex > visualItems->count() - 1) { - return false; - } - item = visualItems->value(scanIndex); - if (!item) { - return false; - } - MissionItem& missionItemDoLandStart = item->missionItem(); - if (missionItemDoLandStart.command() != MAV_CMD_DO_LAND_START || - missionItemDoLandStart.frame() != MAV_FRAME_MISSION || - missionItemDoLandStart.param1() != 0 || missionItemDoLandStart.param2() != 0 || missionItemDoLandStart.param3() != 0 || missionItemDoLandStart.param4() != 0|| missionItemDoLandStart.param5() != 0|| missionItemDoLandStart.param6() != 0) { - return false; - } - - // We made it this far so we do have a Fixed Wing Landing Pattern item at the end of the mission. - // Since we have scanned it we need to remove the items for it fromt the list - int deleteCount = 3; - if (stopTakingPhotos) { - deleteCount += CameraSection::stopTakingPhotosCommandCount(); - } - if (stopTakingVideo) { - deleteCount += CameraSection::stopTakingVideoCommandCount(); - } - int firstItem = visualItems->count() - deleteCount; - while (deleteCount--) { - visualItems->removeAt(firstItem)->deleteLater(); - } - - // Now stuff all the scanned information into the item - - FixedWingLandingComplexItem* complexItem = new FixedWingLandingComplexItem(masterController, flyView, visualItems); - - complexItem->_ignoreRecalcSignals = true; - - complexItem->_altitudesAreRelative = landPointFrame == MAV_FRAME_GLOBAL_RELATIVE_ALT; - complexItem->_loiterRadiusFact.setRawValue(qAbs(missionItemLoiter.param2())); - complexItem->_loiterClockwise = missionItemLoiter.param2() > 0; - complexItem->setLoiterCoordinate(QGeoCoordinate(missionItemLoiter.param5(), missionItemLoiter.param6())); - complexItem->_loiterAltitudeFact.setRawValue(missionItemLoiter.param7()); - - complexItem->_landingCoordinate.setLatitude(missionItemLand.param5()); - complexItem->_landingCoordinate.setLongitude(missionItemLand.param6()); - complexItem->_landingAltitudeFact.setRawValue(missionItemLand.param7()); - - complexItem->_stopTakingPhotosFact.setRawValue(stopTakingPhotos); - complexItem->_stopTakingVideoFact.setRawValue(stopTakingVideo); - - complexItem->_calcGlideSlope(); - - complexItem->_landingCoordSet = true; - - complexItem->_ignoreRecalcSignals = false; - complexItem->_recalcFromCoordinateChange(); - complexItem->setDirty(false); - - visualItems->append(complexItem); - - return true; -} - -double FixedWingLandingComplexItem::_mathematicAngleToHeading(double angle) -{ - double heading = (angle - 90) * -1; - if (heading < 0) { - heading += 360; - } - - return heading; -} - -double FixedWingLandingComplexItem::_headingToMathematicAngle(double heading) -{ - return heading - 90 * -1; -} - -void FixedWingLandingComplexItem::_updateLoiterCoodinateAltitudeFromFact(void) -{ - _loiterCoordinate.setAltitude(_loiterAltitudeFact.rawValue().toDouble()); - emit loiterCoordinateChanged(_loiterCoordinate); - emit coordinateChanged(_loiterCoordinate); -} - -void FixedWingLandingComplexItem::_updateLandingCoodinateAltitudeFromFact(void) -{ - _landingCoordinate.setAltitude(_landingAltitudeFact.rawValue().toDouble()); - emit landingCoordinateChanged(_landingCoordinate); -} - -void FixedWingLandingComplexItem::_setDirty(void) -{ - setDirty(true); -} - -void FixedWingLandingComplexItem::applyNewAltitude(double newAltitude) -{ - _loiterAltitudeFact.setRawValue(newAltitude); -} - void FixedWingLandingComplexItem::_glideSlopeChanged(void) { if (!_ignoreRecalcSignals) { - double landingAltDifference = _loiterAltitudeFact.rawValue().toDouble() - _landingAltitudeFact.rawValue().toDouble(); + double landingAltDifference = _finalApproachAltitudeFact.rawValue().toDouble() - _landingAltitudeFact.rawValue().toDouble(); double glideSlope = _glideSlopeFact.rawValue().toDouble(); _landingDistanceFact.setRawValue(landingAltDifference / qTan(qDegreesToRadians(glideSlope))); } @@ -540,22 +127,12 @@ void FixedWingLandingComplexItem::_glideSlopeChanged(void) void FixedWingLandingComplexItem::_calcGlideSlope(void) { - double landingAltDifference = _loiterAltitudeFact.rawValue().toDouble() - _landingAltitudeFact.rawValue().toDouble(); + double landingAltDifference = _finalApproachAltitudeFact.rawValue().toDouble() - _landingAltitudeFact.rawValue().toDouble(); double landingDistance = _landingDistanceFact.rawValue().toDouble(); _glideSlopeFact.setRawValue(qRadiansToDegrees(qAtan(landingAltDifference / landingDistance))); } -void FixedWingLandingComplexItem::_signalLastSequenceNumberChanged(void) -{ - emit lastSequenceNumberChanged(lastSequenceNumber()); -} - -FixedWingLandingComplexItem::ReadyForSaveState FixedWingLandingComplexItem::readyForSaveState(void) const -{ - return _landingCoordSet && !_wizardMode ? ReadyForSave : NotReadyForSaveData; -} - void FixedWingLandingComplexItem::moveLandingPosition(const QGeoCoordinate& coordinate) { double savedHeading = landingHeading()->rawValue().toDouble(); @@ -566,13 +143,18 @@ void FixedWingLandingComplexItem::moveLandingPosition(const QGeoCoordinate& coor landingDistance()->setRawValue(savedDistance); } -double FixedWingLandingComplexItem::amslEntryAlt(void) const +bool FixedWingLandingComplexItem::_isValidLandItem(const MissionItem& missionItem) { - return _loiterAltitudeFact.rawValue().toDouble() + (_altitudesAreRelative ? _missionController->plannedHomePosition().altitude() : 0); + if (missionItem.command() != MAV_CMD_NAV_LAND || + !(missionItem.frame() == MAV_FRAME_GLOBAL_RELATIVE_ALT || missionItem.frame() == MAV_FRAME_GLOBAL) || + missionItem.param1() != 0 || missionItem.param2() != 0 || missionItem.param3() != 0 || missionItem.param4() != 0) { + return false; + } else { + return true; + } } -double FixedWingLandingComplexItem::amslExitAlt(void) const +bool FixedWingLandingComplexItem::scanForItem(QmlObjectListModel* visualItems, bool flyView, PlanMasterController* masterController) { - return _landingAltitudeFact.rawValue().toDouble() + (_altitudesAreRelative ? _missionController->plannedHomePosition().altitude() : 0); - + return _scanForItem(visualItems, flyView, masterController, _isValidLandItem, _createItem); } diff --git a/src/MissionManager/FixedWingLandingComplexItem.h b/src/MissionManager/FixedWingLandingComplexItem.h index cc653f751..7316fa88e 100644 --- a/src/MissionManager/FixedWingLandingComplexItem.h +++ b/src/MissionManager/FixedWingLandingComplexItem.h @@ -32,114 +32,63 @@ public: Q_INVOKABLE void moveLandingPosition(const QGeoCoordinate& coordinate); // Maintains the current landing distance and heading - Fact* loiterAltitude (void) final { return &_loiterAltitudeFact; } - Fact* loiterRadius (void) final { return &_loiterRadiusFact; } - Fact* landingAltitude (void) final { return &_landingAltitudeFact; } - Fact* landingDistance (void) final { return &_landingDistanceFact; } - Fact* landingHeading (void) final { return &_landingHeadingFact; } - Fact* stopTakingPhotos (void) final { return &_stopTakingPhotosFact; } - Fact* stopTakingVideo (void) final { return &_stopTakingVideoFact; } Fact* glideSlope (void) { return &_glideSlopeFact; } Fact* valueSetIsDistance (void) { return &_valueSetIsDistanceFact; } /// Scans the loaded items for a landing pattern complex item static bool scanForItem(QmlObjectListModel* visualItems, bool flyView, PlanMasterController* masterController); - static MissionItem* createDoLandStartItem (int seqNum, QObject* parent); - static MissionItem* createLoiterToAltItem (int seqNum, bool altRel, double loiterRaidus, double lat, double lon, double alt, QObject* parent); - static MissionItem* createLandItem (int seqNum, bool altRel, double lat, double lon, double alt, QObject* parent); - // Overrides from ComplexMissionItem QString patternName (void) const final { return name; } - int lastSequenceNumber (void) const final; bool load (const QJsonObject& complexObject, int sequenceNumber, QString& errorString) final; - double greatestDistanceTo (const QGeoCoordinate &other) const final; QString mapVisualQML (void) const final { return QStringLiteral("FWLandingPatternMapVisual.qml"); } // Overrides from VisualMissionItem - bool dirty (void) const final { return _dirty; } - bool isSimpleItem (void) const final { return false; } - bool isStandaloneCoordinate (void) const final { return false; } - bool specifiesCoordinate (void) const final; - bool specifiesAltitudeOnly (void) const final { return false; } - QString commandDescription (void) const final { return "Landing Pattern"; } - QString commandName (void) const final { return "Landing Pattern"; } - QString abbreviation (void) const final { return "L"; } - QGeoCoordinate coordinate (void) const final { return _loiterCoordinate; } - QGeoCoordinate exitCoordinate (void) const final { return _landingCoordinate; } - int sequenceNumber (void) const final { return _sequenceNumber; } - double specifiedFlightSpeed (void) final { return std::numeric_limits::quiet_NaN(); } - double specifiedGimbalYaw (void) final { return std::numeric_limits::quiet_NaN(); } - double specifiedGimbalPitch (void) final { return std::numeric_limits::quiet_NaN(); } - void appendMissionItems (QList& items, QObject* missionItemParent) final; - void applyNewAltitude (double newAltitude) final; - double additionalTimeDelay (void) const final { return 0; } - ReadyForSaveState readyForSaveState (void) const final; - bool exitCoordinateSameAsEntry (void) const final { return false; } - void setDirty (bool dirty) final; - void setCoordinate (const QGeoCoordinate& coordinate) final { setLoiterCoordinate(coordinate); } - void setSequenceNumber (int sequenceNumber) final; void save (QJsonArray& missionItems) final; - double amslEntryAlt (void) const final; - double amslExitAlt (void) const final; - double minAMSLAltitude (void) const final { return amslExitAlt(); } - double maxAMSLAltitude (void) const final { return amslEntryAlt(); } static const QString name; static const char* jsonComplexItemTypeValue; static const char* settingsGroup; - static const char* loiterToLandDistanceName; - static const char* loiterAltitudeName; - static const char* loiterRadiusName; - static const char* landingHeadingName; - static const char* landingAltitudeName; static const char* glideSlopeName; - static const char* stopTakingPhotosName; - static const char* stopTakingVideoName; static const char* valueSetIsDistanceName; -signals: - void valueSetIsDistanceChanged (bool valueSetIsDistance); - private slots: - void _updateLoiterCoodinateAltitudeFromFact (void); - void _updateLandingCoodinateAltitudeFromFact (void); - double _mathematicAngleToHeading (double angle); - double _headingToMathematicAngle (double heading); - void _setDirty (void); - void _glideSlopeChanged (void); - void _signalLastSequenceNumberChanged (void); + void _glideSlopeChanged(void); private: - void _calcGlideSlope(void) final; + static LandingComplexItem* _createItem (PlanMasterController* masterController, bool flyView, QObject* parent) { return new FixedWingLandingComplexItem(masterController, flyView, parent); } + static bool _isValidLandItem(const MissionItem& missionItem); + + // Overrides from LandingComplexItem + const Fact* _finalApproachAltitude (void) const final { return &_finalApproachAltitudeFact; } + const Fact* _loiterRadius (void) const final { return &_loiterRadiusFact; } + const Fact* _loiterClockwise (void) const final { return &_loiterClockwiseFact; } + const Fact* _landingAltitude (void) const final { return &_landingAltitudeFact; } + const Fact* _landingDistance (void) const final { return &_landingDistanceFact; } + const Fact* _landingHeading (void) const final { return &_landingHeadingFact; } + const Fact* _useLoiterToAlt (void) const final { return &_useLoiterToAltFact; } + const Fact* _stopTakingPhotos (void) const final { return &_stopTakingPhotosFact; } + const Fact* _stopTakingVideo (void) const final { return &_stopTakingVideoFact; } + void _calcGlideSlope (void) final; + MissionItem* _createLandItem (int seqNum, bool altRel, double lat, double lon, double alt, QObject* parent) final; QMap _metaDataMap; Fact _landingDistanceFact; - Fact _loiterAltitudeFact; + Fact _finalApproachAltitudeFact; Fact _loiterRadiusFact; + Fact _loiterClockwiseFact; Fact _landingHeadingFact; Fact _landingAltitudeFact; Fact _glideSlopeFact; + Fact _useLoiterToAltFact; Fact _stopTakingPhotosFact; Fact _stopTakingVideoFact; Fact _valueSetIsDistanceFact; - static const char* _jsonLoiterCoordinateKey; - static const char* _jsonLoiterRadiusKey; - static const char* _jsonLoiterClockwiseKey; - static const char* _jsonLandingCoordinateKey; static const char* _jsonValueSetIsDistanceKey; - static const char* _jsonAltitudesAreRelativeKey; - static const char* _jsonStopTakingPhotosKey; - static const char* _jsonStopTakingVideoKey; - - // Only in older file formats - static const char* _jsonLandingAltitudeRelativeKey; - static const char* _jsonLoiterAltitudeRelativeKey; - static const char* _jsonFallRateKey; friend FWLandingPatternTest; }; diff --git a/src/MissionManager/LandingComplexItem.cc b/src/MissionManager/LandingComplexItem.cc index b16941a1a..8b75287c0 100644 --- a/src/MissionManager/LandingComplexItem.cc +++ b/src/MissionManager/LandingComplexItem.cc @@ -20,6 +20,36 @@ QGC_LOGGING_CATEGORY(LandingComplexItemLog, "LandingComplexItemLog") +const char* LandingComplexItem::_jsonFinalApproachCoordinateKey = "landingApproachCoordinate"; +const char* LandingComplexItem::_jsonLoiterRadiusKey = "loiterRadius"; +const char* LandingComplexItem::_jsonLoiterClockwiseKey = "loiterClockwise"; +const char* LandingComplexItem::_jsonLandingCoordinateKey = "landCoordinate"; +const char* LandingComplexItem::_jsonAltitudesAreRelativeKey = "altitudesAreRelative"; +const char* LandingComplexItem::_jsonUseLoiterToAltKey = "stopTakingPhotos"; +const char* LandingComplexItem::_jsonStopTakingPhotosKey = "stopTakingPhotos"; +const char* LandingComplexItem::_jsonStopTakingVideoKey = "stopVideoPhotos"; + +const char* LandingComplexItem::finalApproachToLandDistanceName = "LandingDistance"; +const char* LandingComplexItem::landingHeadingName = "LandingHeading"; +const char* LandingComplexItem::finalApproachAltitudeName = "FinalApproachAltitude"; +const char* LandingComplexItem::loiterRadiusName = "LoiterRadius"; +const char* LandingComplexItem::loiterClockwiseName = "LoiterClockwise"; +const char* LandingComplexItem::landingAltitudeName = "LandingAltitude"; +const char* LandingComplexItem::useLoiterToAltName = "UseLoiterToAlt"; +const char* LandingComplexItem::stopTakingPhotosName = "StopTakingPhotos"; +const char* LandingComplexItem::stopTakingVideoName = "StopTakingVideo"; + +// Deprecated keys + +// Support for separate relative alt settings for land/loiter was removed. It now only has a single +// relative alt setting stored in _jsonAltitudesAreRelativeKey. +const char* LandingComplexItem::_jsonDeprecatedLandingAltitudeRelativeKey = "landAltitudeRelative"; +const char* LandingComplexItem::_jsonDeprecatedLoiterAltitudeRelativeKey = "loiterAltitudeRelative"; + +// Name changed from _jsonDeprecatedLoiterCoordinateKey to _jsonFinalApproachCoordinateKey to reflect +// the new support for using either a loiter or just a waypoint as the approach entry point. +const char* LandingComplexItem::_jsonDeprecatedLoiterCoordinateKey = "loiterCoordinate"; + LandingComplexItem::LandingComplexItem(PlanMasterController* masterController, bool flyView, QObject* parent) : ComplexMissionItem (masterController, flyView, parent) { @@ -32,14 +62,66 @@ LandingComplexItem::LandingComplexItem(PlanMasterController* masterController, b void LandingComplexItem::_init(void) { - connect(landingDistance(), &Fact::valueChanged, this, &LandingComplexItem::_recalcFromHeadingAndDistanceChange); - connect(landingHeading(), &Fact::valueChanged, this, &LandingComplexItem::_recalcFromHeadingAndDistanceChange); - - connect(loiterRadius(), &Fact::valueChanged, this, &LandingComplexItem::_recalcFromRadiusChange); - connect(this, &LandingComplexItem::loiterClockwiseChanged, this, &LandingComplexItem::_recalcFromRadiusChange); + if (_masterController->controllerVehicle()->apmFirmware()) { + // ArduPilot does not support camera commands + stopTakingVideo()->setRawValue(false); + stopTakingPhotos()->setRawValue(false); + } - connect(this, &LandingComplexItem::loiterCoordinateChanged, this, &LandingComplexItem::_recalcFromCoordinateChange); - connect(this, &LandingComplexItem::landingCoordinateChanged, this, &LandingComplexItem::_recalcFromCoordinateChange); + connect(landingDistance(), &Fact::valueChanged, this, &LandingComplexItem::_recalcFromHeadingAndDistanceChange); + connect(landingHeading(), &Fact::valueChanged, this, &LandingComplexItem::_recalcFromHeadingAndDistanceChange); + + connect(loiterRadius(), &Fact::valueChanged, this, &LandingComplexItem::_recalcFromRadiusChange); + connect(this, &LandingComplexItem::loiterClockwiseChanged, this, &LandingComplexItem::_recalcFromRadiusChange); + + connect(this, &LandingComplexItem::finalApproachCoordinateChanged,this, &LandingComplexItem::_recalcFromCoordinateChange); + connect(this, &LandingComplexItem::landingCoordinateChanged, this, &LandingComplexItem::_recalcFromCoordinateChange); + connect(this, &LandingComplexItem::useLoiterToAltChanged, this, &LandingComplexItem::_recalcFromCoordinateChange); + + connect(finalApproachAltitude(), &Fact::valueChanged, this, &LandingComplexItem::_setDirty); + connect(landingAltitude(), &Fact::valueChanged, this, &LandingComplexItem::_setDirty); + connect(landingDistance(), &Fact::valueChanged, this, &LandingComplexItem::_setDirty); + connect(landingHeading(), &Fact::valueChanged, this, &LandingComplexItem::_setDirty); + connect(loiterRadius(), &Fact::valueChanged, this, &LandingComplexItem::_setDirty); + connect(loiterClockwise(), &Fact::valueChanged, this, &LandingComplexItem::_setDirty); + connect(useLoiterToAlt(), &Fact::valueChanged, this, &LandingComplexItem::_setDirty); + connect(stopTakingPhotos(), &Fact::valueChanged, this, &LandingComplexItem::_setDirty); + connect(stopTakingVideo(), &Fact::valueChanged, this, &LandingComplexItem::_setDirty); + connect(this, &LandingComplexItem::finalApproachCoordinateChanged,this, &LandingComplexItem::_setDirty); + connect(this, &LandingComplexItem::landingCoordinateChanged, this, &LandingComplexItem::_setDirty); + connect(this, &LandingComplexItem::loiterClockwiseChanged, this, &LandingComplexItem::_setDirty); + connect(this, &LandingComplexItem::altitudesAreRelativeChanged, this, &LandingComplexItem::_setDirty); + connect(this, &LandingComplexItem::useLoiterToAltChanged, this, &LandingComplexItem::_setDirty); + + connect(stopTakingPhotos(), &Fact::valueChanged, this, &LandingComplexItem::_signalLastSequenceNumberChanged); + connect(stopTakingVideo(), &Fact::valueChanged, this, &LandingComplexItem::_signalLastSequenceNumberChanged); + + connect(this, &LandingComplexItem::altitudesAreRelativeChanged, this, &LandingComplexItem::_amslEntryAltChanged); + connect(this, &LandingComplexItem::altitudesAreRelativeChanged, this, &LandingComplexItem::_amslExitAltChanged); + connect(_missionController, &MissionController::plannedHomePositionChanged, this, &LandingComplexItem::_amslEntryAltChanged); + connect(_missionController, &MissionController::plannedHomePositionChanged, this, &LandingComplexItem::_amslExitAltChanged); + connect(finalApproachAltitude(), &Fact::valueChanged, this, &LandingComplexItem::_amslEntryAltChanged); + connect(landingAltitude(), &Fact::valueChanged, this, &LandingComplexItem::_amslExitAltChanged); + connect(this, &LandingComplexItem::amslEntryAltChanged, this, &LandingComplexItem::maxAMSLAltitudeChanged); + connect(this, &LandingComplexItem::amslExitAltChanged, this, &LandingComplexItem::minAMSLAltitudeChanged); + + connect(this, &LandingComplexItem::landingCoordSetChanged, this, &LandingComplexItem::readyForSaveStateChanged); + connect(this, &LandingComplexItem::wizardModeChanged, this, &LandingComplexItem::readyForSaveStateChanged); + + connect(this, &LandingComplexItem::finalApproachCoordinateChanged,this, &LandingComplexItem::complexDistanceChanged); + connect(this, &LandingComplexItem::loiterTangentCoordinateChanged,this, &LandingComplexItem::complexDistanceChanged); + connect(this, &LandingComplexItem::landingCoordinateChanged, this, &LandingComplexItem::complexDistanceChanged); + + connect(this, &LandingComplexItem::loiterTangentCoordinateChanged,this, &LandingComplexItem::_updateFlightPathSegmentsSignal); + connect(this, &LandingComplexItem::finalApproachCoordinateChanged,this, &LandingComplexItem::_updateFlightPathSegmentsSignal); + connect(this, &LandingComplexItem::landingCoordinateChanged, this, &LandingComplexItem::_updateFlightPathSegmentsSignal); + connect(finalApproachAltitude(), &Fact::valueChanged, this, &LandingComplexItem::_updateFlightPathSegmentsSignal); + connect(landingAltitude(), &Fact::valueChanged, this, &LandingComplexItem::_updateFlightPathSegmentsSignal); + connect(this, &LandingComplexItem::altitudesAreRelativeChanged, this, &LandingComplexItem::_updateFlightPathSegmentsSignal); + connect(_missionController, &MissionController::plannedHomePositionChanged, this, &LandingComplexItem::_updateFlightPathSegmentsSignal); + + connect(finalApproachAltitude(), &Fact::valueChanged, this, &LandingComplexItem::_updateFinalApproachCoodinateAltitudeFromFact); + connect(landingAltitude(), &Fact::valueChanged, this, &LandingComplexItem::_updateLandingCoodinateAltitudeFromFact); } void LandingComplexItem::setLandingHeadingToTakeoffHeading() @@ -53,7 +135,7 @@ void LandingComplexItem::setLandingHeadingToTakeoffHeading() double LandingComplexItem::complexDistance(void) const { - return loiterCoordinate().distanceTo(loiterTangentCoordinate()) + loiterTangentCoordinate().distanceTo(landingCoordinate()); + return finalApproachCoordinate().distanceTo(loiterTangentCoordinate()) + loiterTangentCoordinate().distanceTo(landingCoordinate()); } // Never call this method directly. If you want to update the flight segments you emit _updateFlightPathSegmentsSignal() @@ -66,7 +148,7 @@ void LandingComplexItem::_updateFlightPathSegmentsDontCallDirectly(void) _flightPathSegments.beginReset(); _flightPathSegments.clearAndDeleteContents(); - _appendFlightPathSegment(loiterCoordinate(), amslEntryAlt(), loiterTangentCoordinate(), amslEntryAlt()); + _appendFlightPathSegment(finalApproachCoordinate(), amslEntryAlt(), loiterTangentCoordinate(), amslEntryAlt()); _appendFlightPathSegment(loiterTangentCoordinate(), amslEntryAlt(), landingCoordinate(), amslEntryAlt()); _appendFlightPathSegment(landingCoordinate(), amslEntryAlt(), landingCoordinate(), amslExitAlt()); _flightPathSegments.endReset(); @@ -97,12 +179,12 @@ void LandingComplexItem::setLandingCoordinate(const QGeoCoordinate& coordinate) } } -void LandingComplexItem::setLoiterCoordinate(const QGeoCoordinate& coordinate) +void LandingComplexItem::setFinalApproachCoordinate(const QGeoCoordinate& coordinate) { - if (coordinate != _loiterCoordinate) { - _loiterCoordinate = coordinate; + if (coordinate != _finalApproachCoordinate) { + _finalApproachCoordinate = coordinate; emit coordinateChanged(coordinate); - emit loiterCoordinateChanged(coordinate); + emit finalApproachCoordinateChanged(coordinate); } } @@ -139,13 +221,13 @@ void LandingComplexItem::_recalcFromHeadingAndDistanceChange(void) _loiterTangentCoordinate = _landingCoordinate.atDistanceAndAzimuth(landToTangentDistance, heading + 180); // Loiter coord is 90 degrees counter clockwise from tangent coord - _loiterCoordinate = _loiterTangentCoordinate.atDistanceAndAzimuth(radius, heading - 180 - 90); - _loiterCoordinate.setAltitude(loiterAltitude()->rawValue().toDouble()); + _finalApproachCoordinate = _loiterTangentCoordinate.atDistanceAndAzimuth(radius, heading - 180 - 90); + _finalApproachCoordinate.setAltitude(finalApproachAltitude()->rawValue().toDouble()); _ignoreRecalcSignals = true; emit loiterTangentCoordinateChanged(_loiterTangentCoordinate); - emit loiterCoordinateChanged(_loiterCoordinate); - emit coordinateChanged(_loiterCoordinate); + emit finalApproachCoordinateChanged(_finalApproachCoordinate); + emit coordinateChanged(_finalApproachCoordinate); _calcGlideSlope(); _ignoreRecalcSignals = false; } @@ -168,10 +250,10 @@ void LandingComplexItem::_recalcFromRadiusChange(void) double landToTangentDistance = landingDistance()->rawValue().toDouble(); double heading = landingHeading()->rawValue().toDouble(); - double landToLoiterDistance = _landingCoordinate.distanceTo(_loiterCoordinate); + double landToLoiterDistance = _landingCoordinate.distanceTo(_finalApproachCoordinate); if (landToLoiterDistance < radius) { // Degnenerate case: Move tangent to loiter point - _loiterTangentCoordinate = _loiterCoordinate; + _loiterTangentCoordinate = _finalApproachCoordinate; double heading = _landingCoordinate.azimuthTo(_loiterTangentCoordinate); @@ -181,14 +263,14 @@ void LandingComplexItem::_recalcFromRadiusChange(void) _ignoreRecalcSignals = false; } else { double landToLoiterDistance = qSqrt(qPow(radius, 2) + qPow(landToTangentDistance, 2)); - double angleLoiterToTangent = qRadiansToDegrees(qAsin(radius/landToLoiterDistance)) * (_loiterClockwise ? -1 : 1); + double angleLoiterToTangent = qRadiansToDegrees(qAsin(radius/landToLoiterDistance)) * (_loiterClockwise()->rawValue().toBool() ? -1 : 1); - _loiterCoordinate = _landingCoordinate.atDistanceAndAzimuth(landToLoiterDistance, heading + 180 + angleLoiterToTangent); - _loiterCoordinate.setAltitude(loiterAltitude()->rawValue().toDouble()); + _finalApproachCoordinate = _landingCoordinate.atDistanceAndAzimuth(landToLoiterDistance, heading + 180 + angleLoiterToTangent); + _finalApproachCoordinate.setAltitude(finalApproachAltitude()->rawValue().toDouble()); _ignoreRecalcSignals = true; - emit loiterCoordinateChanged(_loiterCoordinate); - emit coordinateChanged(_loiterCoordinate); + emit finalApproachCoordinateChanged(_finalApproachCoordinate); + emit coordinateChanged(_finalApproachCoordinate); _ignoreRecalcSignals = false; } } @@ -209,16 +291,16 @@ void LandingComplexItem::_recalcFromCoordinateChange(void) if (!_ignoreRecalcSignals && _landingCoordSet) { // These are our known values double radius = loiterRadius()->rawValue().toDouble(); - double landToLoiterDistance = _landingCoordinate.distanceTo(_loiterCoordinate); - double landToLoiterHeading = _landingCoordinate.azimuthTo(_loiterCoordinate); + double landToLoiterDistance = _landingCoordinate.distanceTo(_finalApproachCoordinate); + double landToLoiterHeading = _landingCoordinate.azimuthTo(_finalApproachCoordinate); double landToTangentDistance; if (landToLoiterDistance < radius) { // Degenerate case, set tangent to loiter coordinate - _loiterTangentCoordinate = _loiterCoordinate; + _loiterTangentCoordinate = _finalApproachCoordinate; landToTangentDistance = _landingCoordinate.distanceTo(_loiterTangentCoordinate); } else { - double loiterToTangentAngle = qRadiansToDegrees(qAsin(radius/landToLoiterDistance)) * (_loiterClockwise ? 1 : -1); + double loiterToTangentAngle = qRadiansToDegrees(qAsin(radius/landToLoiterDistance)) * (_loiterClockwise()->rawValue().toBool() ? 1 : -1); landToTangentDistance = qSqrt(qPow(landToLoiterDistance, 2) - qPow(radius, 2)); _loiterTangentCoordinate = _landingCoordinate.atDistanceAndAzimuth(landToTangentDistance, landToLoiterHeading + loiterToTangentAngle); @@ -236,4 +318,419 @@ void LandingComplexItem::_recalcFromCoordinateChange(void) } } +int LandingComplexItem::lastSequenceNumber(void) const +{ + // Fixed items are: + // land start, loiter, land + // Optional items are: + // stop photos/video + return _sequenceNumber + 2 + (stopTakingPhotos()->rawValue().toBool() ? 2 : 0) + (stopTakingVideo()->rawValue().toBool() ? 1 : 0); +} + +void LandingComplexItem::appendMissionItems(QList& items, QObject* missionItemParent) +{ + int seqNum = _sequenceNumber; + + // IMPORTANT NOTE: Any changes here must also be taken into account in scanForItem + + MissionItem* item = _createDoLandStartItem(seqNum++, missionItemParent); + items.append(item); + + + if (stopTakingPhotos()->rawValue().toBool()) { + CameraSection::appendStopTakingPhotos(items, seqNum, missionItemParent); + } + + if (stopTakingVideo()->rawValue().toBool()) { + CameraSection::appendStopTakingVideo(items, seqNum, missionItemParent); + } + + item = _createFinalApproachItem(seqNum++, missionItemParent); + items.append(item); + + item = _createLandItem(seqNum++, + _altitudesAreRelative, + _landingCoordinate.latitude(), _landingCoordinate.longitude(), landingAltitude()->rawValue().toDouble(), + missionItemParent); + items.append(item); +} + +MissionItem* LandingComplexItem::_createDoLandStartItem(int seqNum, QObject* parent) +{ + return new MissionItem(seqNum, // sequence number + MAV_CMD_DO_LAND_START, // MAV_CMD + MAV_FRAME_MISSION, // MAV_FRAME + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, // param 1-7 + true, // autoContinue + false, // isCurrentItem + parent); +} + +MissionItem* LandingComplexItem::_createFinalApproachItem(int seqNum, QObject* parent) +{ + if (useLoiterToAlt()->rawValue().toBool()) { + return new MissionItem(seqNum, + MAV_CMD_NAV_LOITER_TO_ALT, + _altitudesAreRelative ? MAV_FRAME_GLOBAL_RELATIVE_ALT : MAV_FRAME_GLOBAL, + 1.0, // Heading required = true + loiterRadius()->rawValue().toDouble() * (_loiterClockwise()->rawValue().toBool() ? 1.0 : -1.0), + 0.0, // param 3 - unused + 1.0, // Exit crosstrack - tangent of loiter to land point + _finalApproachCoordinate.latitude(), + _finalApproachCoordinate.longitude(), + _finalApproachAltitude()->rawValue().toFloat(), + true, // autoContinue + false, // isCurrentItem + parent); + } else { + return new MissionItem(seqNum, + MAV_CMD_NAV_WAYPOINT, + _altitudesAreRelative ? MAV_FRAME_GLOBAL_RELATIVE_ALT : MAV_FRAME_GLOBAL, + 0, // No hold time + 0, // Use default acceptance radius + 0, // Pass through waypoint + qQNaN(), // Yaw not specified + _finalApproachCoordinate.latitude(), + _finalApproachCoordinate.longitude(), + _finalApproachAltitude()->rawValue().toFloat(), + true, // autoContinue + false, // isCurrentItem + parent); + } +} + +bool LandingComplexItem::_scanForItem(QmlObjectListModel* visualItems, bool flyView, PlanMasterController* masterController, IsLandItemFunc isLandItemFunc, CreateItemFunc createItemFunc) +{ + qCDebug(LandingComplexItemLog) << "VTOLLandingComplexItem::scanForItem count" << visualItems->count(); + + if (visualItems->count() < 3) { + return false; + } + + // A valid landing pattern is comprised of the follow commands in this order at the end of the item list: + // MAV_CMD_DO_LAND_START - required + // Stop taking photos sequence - optional + // Stop taking video sequence - optional + // MAV_CMD_NAV_LOITER_TO_ALT or MAV_CMD_NAV_WAYPOINT + // MAV_CMD_NAV_LAND or MAV_CMS_NAV_VTOL_LAND + + // Start looking for the commands in reverse order from the end of the list + + int scanIndex = visualItems->count() - 1; + + if (scanIndex < 0 || scanIndex > visualItems->count() - 1) { + return false; + } + SimpleMissionItem* item = visualItems->value(scanIndex--); + if (!item) { + return false; + } + MissionItem& missionItemLand = item->missionItem(); + if (!isLandItemFunc(missionItemLand)) { + return false; + } + MAV_FRAME landPointFrame = missionItemLand.frame(); + + if (scanIndex < 0 || scanIndex > visualItems->count() - 1) { + return false; + } + item = visualItems->value(scanIndex); + if (!item) { + return false; + } + bool useLoiterToAlt = true; + MissionItem& missionItemFinalApproach = item->missionItem(); + if (missionItemFinalApproach.command() == MAV_CMD_NAV_LOITER_TO_ALT) { + if (missionItemFinalApproach.frame() != landPointFrame || + missionItemFinalApproach.param1() != 1.0 || missionItemFinalApproach.param3() != 0 || missionItemFinalApproach.param4() != 1.0) { + return false; + } + } else if (missionItemFinalApproach.command() == MAV_CMD_NAV_WAYPOINT) { + if (missionItemFinalApproach.frame() != landPointFrame || + missionItemFinalApproach.param1() != 0 || missionItemFinalApproach.param2() != 0 || missionItemFinalApproach.param3() != 0 || + !qIsNaN(missionItemFinalApproach.param4()) || + qIsNaN(missionItemFinalApproach.param5()) || qIsNaN(missionItemFinalApproach.param6()) || qIsNaN(missionItemFinalApproach.param6())) { + return false; + } + useLoiterToAlt = false; + } else { + return false; + } + + scanIndex -= CameraSection::stopTakingVideoCommandCount(); + bool stopTakingVideo = CameraSection::scanStopTakingVideo(visualItems, scanIndex, false /* removeScannedItems */); + if (!stopTakingVideo) { + scanIndex += CameraSection::stopTakingVideoCommandCount(); + } + + scanIndex -= CameraSection::stopTakingPhotosCommandCount(); + bool stopTakingPhotos = CameraSection::scanStopTakingPhotos(visualItems, scanIndex, false /* removeScannedItems */); + if (!stopTakingPhotos) { + scanIndex += CameraSection::stopTakingPhotosCommandCount(); + } + + scanIndex--; + if (scanIndex < 0 || scanIndex > visualItems->count() - 1) { + return false; + } + item = visualItems->value(scanIndex); + if (!item) { + return false; + } + MissionItem& missionItemDoLandStart = item->missionItem(); + if (missionItemDoLandStart.command() != MAV_CMD_DO_LAND_START || + missionItemDoLandStart.frame() != MAV_FRAME_MISSION || + missionItemDoLandStart.param1() != 0 || missionItemDoLandStart.param2() != 0 || missionItemDoLandStart.param3() != 0 || missionItemDoLandStart.param4() != 0 || + missionItemDoLandStart.param5() != 0 || missionItemDoLandStart.param6() != 0 || missionItemDoLandStart.param7() != 0) { + return false; + } + + // We made it this far so we do have a Fixed Wing Landing Pattern item at the end of the mission. + // Since we have scanned it we need to remove the items for it fromt the list + int deleteCount = 3; + if (stopTakingPhotos) { + deleteCount += CameraSection::stopTakingPhotosCommandCount(); + } + if (stopTakingVideo) { + deleteCount += CameraSection::stopTakingVideoCommandCount(); + } + int firstItem = visualItems->count() - deleteCount; + while (deleteCount--) { + visualItems->removeAt(firstItem)->deleteLater(); + } + + // Now stuff all the scanned information into the item + + LandingComplexItem* complexItem = createItemFunc(masterController, flyView, visualItems /* parent */); + + complexItem->_ignoreRecalcSignals = true; + + complexItem->_altitudesAreRelative = landPointFrame == MAV_FRAME_GLOBAL_RELATIVE_ALT; + complexItem->setFinalApproachCoordinate(QGeoCoordinate(missionItemFinalApproach.param5(), missionItemFinalApproach.param6())); + complexItem->finalApproachAltitude()->setRawValue(missionItemFinalApproach.param7()); + complexItem->useLoiterToAlt()->setRawValue(useLoiterToAlt); + + if (useLoiterToAlt) { + complexItem->loiterRadius()->setRawValue(qAbs(missionItemFinalApproach.param2())); + complexItem->loiterClockwise()->setRawValue(missionItemFinalApproach.param2() > 0); + } + + complexItem->_landingCoordinate.setLatitude(missionItemLand.param5()); + complexItem->_landingCoordinate.setLongitude(missionItemLand.param6()); + complexItem->landingAltitude()->setRawValue(missionItemLand.param7()); + + complexItem->stopTakingPhotos()->setRawValue(stopTakingPhotos); + complexItem->stopTakingVideo()->setRawValue(stopTakingVideo); + + complexItem->_landingCoordSet = true; + + complexItem->_ignoreRecalcSignals = false; + + complexItem->_recalcFromCoordinateChange(); + complexItem->setDirty(false); + + visualItems->append(complexItem); + + return true; +} + +void LandingComplexItem::applyNewAltitude(double newAltitude) +{ + finalApproachAltitude()->setRawValue(newAltitude); +} + +LandingComplexItem::ReadyForSaveState LandingComplexItem::readyForSaveState(void) const +{ + return _landingCoordSet && !_wizardMode ? ReadyForSave : NotReadyForSaveData; +} + +void LandingComplexItem::setDirty(bool dirty) +{ + if (_dirty != dirty) { + _dirty = dirty; + emit dirtyChanged(_dirty); + } +} + +void LandingComplexItem::_setDirty(void) +{ + setDirty(true); +} + +void LandingComplexItem::setSequenceNumber(int sequenceNumber) +{ + if (_sequenceNumber != sequenceNumber) { + _sequenceNumber = sequenceNumber; + emit sequenceNumberChanged(sequenceNumber); + emit lastSequenceNumberChanged(lastSequenceNumber()); + } +} + +double LandingComplexItem::amslEntryAlt(void) const +{ + return finalApproachAltitude()->rawValue().toDouble() + (_altitudesAreRelative ? _missionController->plannedHomePosition().altitude() : 0); +} + +double LandingComplexItem::amslExitAlt(void) const +{ + return landingAltitude()->rawValue().toDouble() + (_altitudesAreRelative ? _missionController->plannedHomePosition().altitude() : 0); + +} + +void LandingComplexItem::_signalLastSequenceNumberChanged(void) +{ + emit lastSequenceNumberChanged(lastSequenceNumber()); +} + +void LandingComplexItem::_updateFinalApproachCoodinateAltitudeFromFact(void) +{ + _finalApproachCoordinate.setAltitude(finalApproachAltitude()->rawValue().toDouble()); + emit finalApproachCoordinateChanged(_finalApproachCoordinate); + emit coordinateChanged(_finalApproachCoordinate); +} + +void LandingComplexItem::_updateLandingCoodinateAltitudeFromFact(void) +{ + _landingCoordinate.setAltitude(landingAltitude()->rawValue().toDouble()); + emit landingCoordinateChanged(_landingCoordinate); +} + +double LandingComplexItem::greatestDistanceTo(const QGeoCoordinate &other) const +{ + return qMax(_finalApproachCoordinate.distanceTo(other),_landingCoordinate.distanceTo(other)); +} + +QJsonObject LandingComplexItem::_save(void) +{ + QJsonObject saveObject; + + QGeoCoordinate coordinate; + QJsonValue jsonCoordinate; + + coordinate = _finalApproachCoordinate; + coordinate.setAltitude(finalApproachAltitude()->rawValue().toDouble()); + JsonHelper::saveGeoCoordinate(coordinate, true /* writeAltitude */, jsonCoordinate); + saveObject[_jsonFinalApproachCoordinateKey] = jsonCoordinate; + coordinate = _landingCoordinate; + coordinate.setAltitude(landingAltitude()->rawValue().toDouble()); + JsonHelper::saveGeoCoordinate(coordinate, true /* writeAltitude */, jsonCoordinate); + saveObject[_jsonLandingCoordinateKey] = jsonCoordinate; + + saveObject[_jsonLoiterRadiusKey] = loiterRadius()->rawValue().toDouble(); + saveObject[_jsonStopTakingPhotosKey] = stopTakingPhotos()->rawValue().toBool(); + saveObject[_jsonStopTakingVideoKey] = stopTakingVideo()->rawValue().toBool(); + saveObject[_jsonLoiterClockwiseKey] = loiterClockwise()->rawValue().toBool(); + saveObject[_jsonUseLoiterToAltKey] = useLoiterToAlt()->rawValue().toBool(); + saveObject[_jsonAltitudesAreRelativeKey] = _altitudesAreRelative; + + return saveObject; +} + +bool LandingComplexItem::_load(const QJsonObject& complexObject, int sequenceNumber, const QString& jsonComplexItemTypeValue, bool useDeprecatedRelAltKeys, QString& errorString) +{ + QList keyInfoList = { + { JsonHelper::jsonVersionKey, QJsonValue::Double, true }, + { VisualMissionItem::jsonTypeKey, QJsonValue::String, true }, + { ComplexMissionItem::jsonComplexItemTypeKey, QJsonValue::String, true }, + { _jsonDeprecatedLoiterCoordinateKey, QJsonValue::Array, false }, // Loiter changed to Final Approach + { _jsonFinalApproachCoordinateKey, QJsonValue::Array, false }, + { _jsonLoiterRadiusKey, QJsonValue::Double, true }, + { _jsonLoiterClockwiseKey, QJsonValue::Bool, true }, + { _jsonLandingCoordinateKey, QJsonValue::Array, true }, + { _jsonStopTakingPhotosKey, QJsonValue::Bool, false }, + { _jsonStopTakingVideoKey, QJsonValue::Bool, false }, + { _jsonUseLoiterToAltKey, QJsonValue::Bool, false }, + }; + if (!JsonHelper::validateKeys(complexObject, keyInfoList, errorString)) { + return false; + } + + if (!complexObject.contains(_jsonDeprecatedLoiterCoordinateKey) && !complexObject.contains(_jsonFinalApproachCoordinateKey)) { + QList keyInfoList = { + { _jsonFinalApproachCoordinateKey, QJsonValue::Array, true }, + }; + if (!JsonHelper::validateKeys(complexObject, keyInfoList, errorString)) { + return false; + } + } + + QString itemType = complexObject[VisualMissionItem::jsonTypeKey].toString(); + QString complexType = complexObject[ComplexMissionItem::jsonComplexItemTypeKey].toString(); + if (itemType != VisualMissionItem::jsonTypeComplexItemValue || complexType != jsonComplexItemTypeValue) { + errorString = tr("%1 does not support loading this complex mission item type: %2:%3").arg(qgcApp()->applicationName()).arg(itemType).arg(complexType); + return false; + } + + setSequenceNumber(sequenceNumber); + + _ignoreRecalcSignals = true; + + if (useDeprecatedRelAltKeys) { + QList v1KeyInfoList = { + { _jsonDeprecatedLoiterAltitudeRelativeKey, QJsonValue::Bool, true }, + { _jsonDeprecatedLandingAltitudeRelativeKey, QJsonValue::Bool, true }, + }; + if (!JsonHelper::validateKeys(complexObject, v1KeyInfoList, errorString)) { + return false; + } + + bool loiterAltitudeRelative = complexObject[_jsonDeprecatedLoiterAltitudeRelativeKey].toBool(); + bool landingAltitudeRelative = complexObject[_jsonDeprecatedLandingAltitudeRelativeKey].toBool(); + if (loiterAltitudeRelative != landingAltitudeRelative) { + qgcApp()->showAppMessage(tr("Fixed Wing Landing Pattern: " + "Setting the loiter and landing altitudes with different settings for altitude relative is no longer supported. " + "Both have been set to relative altitude. Be sure to adjust/check your plan prior to flight.")); + _altitudesAreRelative = true; + } else { + _altitudesAreRelative = loiterAltitudeRelative; + } + } else { + QList v2KeyInfoList = { + { _jsonAltitudesAreRelativeKey, QJsonValue::Bool, true }, + }; + if (!JsonHelper::validateKeys(complexObject, v2KeyInfoList, errorString)) { + _ignoreRecalcSignals = false; + return false; + } + _altitudesAreRelative = complexObject[_jsonAltitudesAreRelativeKey].toBool(); + } + + QGeoCoordinate coordinate; + QString finalApproachKey = complexObject.contains(_jsonFinalApproachCoordinateKey) ? _jsonFinalApproachCoordinateKey : _jsonDeprecatedLoiterCoordinateKey; + if (!JsonHelper::loadGeoCoordinate(complexObject[finalApproachKey], true /* altitudeRequired */, coordinate, errorString)) { + return false; + } + _finalApproachCoordinate = coordinate; + finalApproachAltitude()->setRawValue(coordinate.altitude()); + + if (!JsonHelper::loadGeoCoordinate(complexObject[_jsonLandingCoordinateKey], true /* altitudeRequired */, coordinate, errorString)) { + return false; + } + _landingCoordinate = coordinate; + landingAltitude()->setRawValue(coordinate.altitude()); + + loiterRadius()->setRawValue(complexObject[_jsonLoiterRadiusKey].toDouble()); + loiterClockwise()->setRawValue(complexObject[_jsonLoiterClockwiseKey].toBool()); + useLoiterToAlt()->setRawValue(complexObject[_jsonUseLoiterToAltKey].toBool(true)); + stopTakingPhotos()->setRawValue(complexObject[_jsonStopTakingPhotosKey].toBool(false)); + stopTakingVideo()->setRawValue(complexObject[_jsonStopTakingVideoKey].toBool(false)); + + _calcGlideSlope(); + + _landingCoordSet = true; + _ignoreRecalcSignals = false; + + _recalcFromCoordinateChange(); + emit coordinateChanged(this->coordinate()); // This will kick off terrain query + + return true; +} + +void LandingComplexItem::setAltitudesAreRelative(bool altitudesAreRelative) +{ + if (altitudesAreRelative != _altitudesAreRelative) { + _altitudesAreRelative = altitudesAreRelative; + emit altitudesAreRelativeChanged(_altitudesAreRelative); + } +} diff --git a/src/MissionManager/LandingComplexItem.h b/src/MissionManager/LandingComplexItem.h index 570fe11dc..2a6c534a1 100644 --- a/src/MissionManager/LandingComplexItem.h +++ b/src/MissionManager/LandingComplexItem.h @@ -17,6 +17,7 @@ Q_DECLARE_LOGGING_CATEGORY(LandingComplexItemLog) class PlanMasterController; +class LandingComplexItemTest; // Base class for landing patterns complex items. class LandingComplexItem : public ComplexMissionItem @@ -26,47 +27,103 @@ class LandingComplexItem : public ComplexMissionItem public: LandingComplexItem(PlanMasterController* masterController, bool flyView, QObject* parent); - Q_PROPERTY(Fact* loiterAltitude READ loiterAltitude CONSTANT) - Q_PROPERTY(Fact* loiterRadius READ loiterRadius CONSTANT) - Q_PROPERTY(Fact* landingAltitude READ landingAltitude CONSTANT) - Q_PROPERTY(Fact* landingHeading READ landingHeading CONSTANT) - Q_PROPERTY(Fact* landingDistance READ landingDistance CONSTANT) - Q_PROPERTY(bool loiterClockwise MEMBER _loiterClockwise NOTIFY loiterClockwiseChanged) - Q_PROPERTY(bool altitudesAreRelative MEMBER _altitudesAreRelative NOTIFY altitudesAreRelativeChanged) - Q_PROPERTY(Fact* stopTakingPhotos READ stopTakingPhotos CONSTANT) - Q_PROPERTY(Fact* stopTakingVideo READ stopTakingVideo CONSTANT) - Q_PROPERTY(QGeoCoordinate loiterCoordinate READ loiterCoordinate WRITE setLoiterCoordinate NOTIFY loiterCoordinateChanged) - Q_PROPERTY(QGeoCoordinate loiterTangentCoordinate READ loiterTangentCoordinate NOTIFY loiterTangentCoordinateChanged) - Q_PROPERTY(QGeoCoordinate landingCoordinate READ landingCoordinate WRITE setLandingCoordinate NOTIFY landingCoordinateChanged) - Q_PROPERTY(bool landingCoordSet MEMBER _landingCoordSet NOTIFY landingCoordSetChanged) + Q_PROPERTY(Fact* finalApproachAltitude READ finalApproachAltitude CONSTANT) + Q_PROPERTY(Fact* loiterRadius READ loiterRadius CONSTANT) + Q_PROPERTY(Fact* landingAltitude READ landingAltitude CONSTANT) + Q_PROPERTY(Fact* landingHeading READ landingHeading CONSTANT) + Q_PROPERTY(Fact* landingDistance READ landingDistance CONSTANT) + Q_PROPERTY(Fact* loiterClockwise READ loiterClockwise CONSTANT) + Q_PROPERTY(Fact* useLoiterToAlt READ useLoiterToAlt CONSTANT) + Q_PROPERTY(Fact* stopTakingPhotos READ stopTakingPhotos CONSTANT) + Q_PROPERTY(Fact* stopTakingVideo READ stopTakingVideo CONSTANT) + Q_PROPERTY(QGeoCoordinate finalApproachCoordinate READ finalApproachCoordinate WRITE setFinalApproachCoordinate NOTIFY finalApproachCoordinateChanged) + Q_PROPERTY(QGeoCoordinate loiterTangentCoordinate READ loiterTangentCoordinate NOTIFY loiterTangentCoordinateChanged) + Q_PROPERTY(QGeoCoordinate landingCoordinate READ landingCoordinate WRITE setLandingCoordinate NOTIFY landingCoordinateChanged) + Q_PROPERTY(bool altitudesAreRelative READ altitudesAreRelative WRITE setAltitudesAreRelative NOTIFY altitudesAreRelativeChanged) + Q_PROPERTY(bool landingCoordSet READ landingCoordSet NOTIFY landingCoordSetChanged) Q_INVOKABLE void setLandingHeadingToTakeoffHeading(); - // These are virtual so the Facts can be created with their own specific FactGroup metadata - virtual Fact* loiterAltitude (void) = 0; - virtual Fact* loiterRadius (void) = 0; - virtual Fact* landingAltitude (void) = 0; - virtual Fact* landingDistance (void) = 0; - virtual Fact* landingHeading (void) = 0; - virtual Fact* stopTakingPhotos (void) = 0; - virtual Fact* stopTakingVideo (void) = 0; - + const Fact* finalApproachAltitude (void) const { return _finalApproachAltitude(); } + const Fact* loiterRadius (void) const { return _loiterRadius(); } + const Fact* loiterClockwise (void) const { return _loiterClockwise(); } + const Fact* landingAltitude (void) const { return _landingAltitude(); } + const Fact* landingDistance (void) const { return _landingDistance(); } + const Fact* landingHeading (void) const { return _landingHeading(); } + const Fact* useLoiterToAlt (void) const { return _useLoiterToAlt(); } + const Fact* stopTakingPhotos (void) const { return _stopTakingPhotos(); } + const Fact* stopTakingVideo (void) const { return _stopTakingVideo(); } + + Fact* finalApproachAltitude (void) { return const_cast(const_cast(this)->_finalApproachAltitude()); }; + Fact* loiterRadius (void) { return const_cast(const_cast(this)->_loiterRadius()); }; + Fact* loiterClockwise (void) { return const_cast(const_cast(this)->_loiterClockwise()); }; + Fact* landingAltitude (void) { return const_cast(const_cast(this)->_landingAltitude()); }; + Fact* landingDistance (void) { return const_cast(const_cast(this)->_landingDistance()); }; + Fact* landingHeading (void) { return const_cast(const_cast(this)->_landingHeading()); }; + Fact* useLoiterToAlt (void) { return const_cast(const_cast(this)->_useLoiterToAlt()); }; + Fact* stopTakingPhotos (void) { return const_cast(const_cast(this)->_stopTakingPhotos()); }; + Fact* stopTakingVideo (void) { return const_cast(const_cast(this)->_stopTakingVideo()); }; + + bool altitudesAreRelative (void) const { return _altitudesAreRelative; } + bool landingCoordSet (void) const { return _landingCoordSet; } QGeoCoordinate landingCoordinate (void) const { return _landingCoordinate; } - QGeoCoordinate loiterCoordinate (void) const { return _loiterCoordinate; } + QGeoCoordinate finalApproachCoordinate (void) const { return _finalApproachCoordinate; } QGeoCoordinate loiterTangentCoordinate (void) const { return _loiterTangentCoordinate; } void setLandingCoordinate (const QGeoCoordinate& coordinate); - void setLoiterCoordinate (const QGeoCoordinate& coordinate); + void setFinalApproachCoordinate (const QGeoCoordinate& coordinate); + void setAltitudesAreRelative (bool altitudesAreRelative); // Overrides from ComplexMissionItem - double complexDistance(void) const final; + double complexDistance (void) const final; + double greatestDistanceTo (const QGeoCoordinate &other) const final; + int lastSequenceNumber (void) const final; + + // Overrides from VisualMissionItem + bool dirty (void) const final { return _dirty; } + bool isSimpleItem (void) const final { return false; } + bool isStandaloneCoordinate (void) const final { return false; } + bool specifiesCoordinate (void) const final { return true; } + bool specifiesAltitudeOnly (void) const final { return false; } + QString commandDescription (void) const final { return "Landing Pattern"; } + QString commandName (void) const final { return "Landing Pattern"; } + QString abbreviation (void) const final { return "L"; } + QGeoCoordinate coordinate (void) const final { return _finalApproachCoordinate; } + QGeoCoordinate exitCoordinate (void) const final { return _landingCoordinate; } + int sequenceNumber (void) const final { return _sequenceNumber; } + double specifiedFlightSpeed (void) final { return std::numeric_limits::quiet_NaN(); } + double specifiedGimbalYaw (void) final { return std::numeric_limits::quiet_NaN(); } + double specifiedGimbalPitch (void) final { return std::numeric_limits::quiet_NaN(); } + void appendMissionItems (QList& items, QObject* missionItemParent) final; + void applyNewAltitude (double newAltitude) final; + double additionalTimeDelay (void) const final { return 0; } + ReadyForSaveState readyForSaveState (void) const final; + bool exitCoordinateSameAsEntry (void) const final { return false; } + void setDirty (bool dirty) final; + void setCoordinate (const QGeoCoordinate& coordinate) final { setFinalApproachCoordinate(coordinate); } + void setSequenceNumber (int sequenceNumber) final; + double amslEntryAlt (void) const final; + double amslExitAlt (void) const final; + double minAMSLAltitude (void) const final { return amslExitAlt(); } + double maxAMSLAltitude (void) const final { return amslEntryAlt(); } + + static const char* finalApproachToLandDistanceName; + static const char* finalApproachAltitudeName; + static const char* loiterRadiusName; + static const char* loiterClockwiseName; + static const char* landingHeadingName; + static const char* landingAltitudeName; + static const char* useLoiterToAltName; + static const char* stopTakingPhotosName; + static const char* stopTakingVideoName; signals: - void loiterCoordinateChanged (QGeoCoordinate coordinate); + void finalApproachCoordinateChanged (QGeoCoordinate coordinate); void loiterTangentCoordinateChanged (QGeoCoordinate coordinate); void landingCoordinateChanged (QGeoCoordinate coordinate); void landingCoordSetChanged (bool landingCoordSet); void loiterClockwiseChanged (bool loiterClockwise); + void useLoiterToAltChanged (bool useLoiterToAlt); void altitudesAreRelativeChanged (bool altitudesAreRelative); void _updateFlightPathSegmentsSignal(void); @@ -74,23 +131,61 @@ protected slots: void _updateFlightPathSegmentsDontCallDirectly (void); void _recalcFromHeadingAndDistanceChange (void); void _recalcFromCoordinateChange (void); + void _setDirty (void); protected: - virtual void _calcGlideSlope(void) = 0; - - void _init (void); - QPointF _rotatePoint(const QPointF& point, const QPointF& origin, double angle); - - int _sequenceNumber = 0; - bool _dirty = false; - QGeoCoordinate _loiterCoordinate; + virtual const Fact* _finalApproachAltitude (void) const = 0; + virtual const Fact* _loiterRadius (void) const = 0; + virtual const Fact* _loiterClockwise (void) const = 0; + virtual const Fact* _landingAltitude (void) const = 0; + virtual const Fact* _landingDistance (void) const = 0; + virtual const Fact* _landingHeading (void) const = 0; + virtual const Fact* _useLoiterToAlt (void) const = 0; + virtual const Fact* _stopTakingPhotos (void) const = 0; + virtual const Fact* _stopTakingVideo (void) const = 0; + virtual void _calcGlideSlope (void) = 0; + virtual MissionItem* _createLandItem (int seqNum, bool altRel, double lat, double lon, double alt, QObject* parent) = 0; + + void _init (void); + QPointF _rotatePoint (const QPointF& point, const QPointF& origin, double angle); + MissionItem* _createDoLandStartItem (int seqNum, QObject* parent); + MissionItem* _createFinalApproachItem(int seqNum, QObject* parent); + QJsonObject _save (void); + bool _load (const QJsonObject& complexObject, int sequenceNumber, const QString& jsonComplexItemTypeValue, bool useDeprecatedRelAltKeys, QString& errorString); + + typedef bool (*IsLandItemFunc)(const MissionItem& missionItem); + typedef LandingComplexItem* (*CreateItemFunc)(PlanMasterController* masterController, bool flyView, QObject* parent); + + static bool _scanForItem(QmlObjectListModel* visualItems, bool flyView, PlanMasterController* masterController, IsLandItemFunc isLandItemFunc, CreateItemFunc createItemFunc); + + int _sequenceNumber = 0; + bool _dirty = false; + QGeoCoordinate _finalApproachCoordinate; QGeoCoordinate _loiterTangentCoordinate; QGeoCoordinate _landingCoordinate; - bool _landingCoordSet = false; - bool _ignoreRecalcSignals = false; - bool _loiterClockwise = true; - bool _altitudesAreRelative = true; + bool _landingCoordSet = false; + bool _ignoreRecalcSignals = false; + bool _altitudesAreRelative = true; + + static const char* _jsonFinalApproachCoordinateKey; + static const char* _jsonLoiterRadiusKey; + static const char* _jsonLoiterClockwiseKey; + static const char* _jsonLandingCoordinateKey; + static const char* _jsonAltitudesAreRelativeKey; + static const char* _jsonUseLoiterToAltKey; + static const char* _jsonStopTakingPhotosKey; + static const char* _jsonStopTakingVideoKey; + + // Only in older file formats + static const char* _jsonDeprecatedLandingAltitudeRelativeKey; + static const char* _jsonDeprecatedLoiterAltitudeRelativeKey; + static const char* _jsonDeprecatedLoiterCoordinateKey; private slots: - void _recalcFromRadiusChange(void); + void _recalcFromRadiusChange (void); + void _signalLastSequenceNumberChanged (void); + void _updateFinalApproachCoodinateAltitudeFromFact (void); + void _updateLandingCoodinateAltitudeFromFact (void); + + friend class LandingComplexItemTest; }; diff --git a/src/MissionManager/LandingComplexItemTest.cc b/src/MissionManager/LandingComplexItemTest.cc new file mode 100644 index 000000000..8acf414f7 --- /dev/null +++ b/src/MissionManager/LandingComplexItemTest.cc @@ -0,0 +1,378 @@ +/**************************************************************************** + * + * (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 "LandingComplexItemTest.h" +#include "QGCApplication.h" +#include "MissionCommandTree.h" +#include "MissionCommandUIInfo.h" +#include "CameraSectionTest.h" +#include "JsonHelper.h" + +const char* SimpleLandingComplexItem::settingsGroup = "SimpleLandingComplexItemUnitTest"; +const char* SimpleLandingComplexItem::jsonComplexItemTypeValue = "utSimpleLandingPattern"; + +LandingComplexItemTest::LandingComplexItemTest(void) +{ + +} + +void LandingComplexItemTest::init(void) +{ + VisualMissionItemTest::init(); + + _item = new SimpleLandingComplexItem(_masterController, false /* flyView */, this); + + // Start in a clean state + QVERIFY(!_item->dirty()); + _item->setLandingCoordinate(QGeoCoordinate(47, -122)); + _item->setDirty(false); + QVERIFY(!_item->dirty()); + + VisualMissionItemTest::_createSpy(_item, &_viMultiSpy); + + rgSignals[finalApproachCoordinateChangedIndex] = SIGNAL(finalApproachCoordinateChanged(QGeoCoordinate)); + rgSignals[loiterTangentCoordinateChangedIndex] = SIGNAL(loiterTangentCoordinateChanged(QGeoCoordinate)); + rgSignals[landingCoordinateChangedIndex] = SIGNAL(landingCoordinateChanged(QGeoCoordinate)); + rgSignals[landingCoordSetChangedIndex] = SIGNAL(landingCoordSetChanged(bool)); + rgSignals[loiterClockwiseChangedIndex] = SIGNAL(loiterClockwiseChanged(bool)); + rgSignals[useLoiterToAltChangedIndex] = SIGNAL(useLoiterToAltChanged(bool)); + rgSignals[altitudesAreRelativeChangedIndex] = SIGNAL(altitudesAreRelativeChanged(bool)); + rgSignals[_updateFlightPathSegmentsSignalIndex] = SIGNAL(_updateFlightPathSegmentsSignal()); + + _multiSpy = new MultiSignalSpy(); + QCOMPARE(_multiSpy->init(_item, rgSignals, cSignals), true); + + _validStopVideoItem = CameraSectionTest::createValidStopTimeItem(_masterController, this); + _validStopDistanceItem = CameraSectionTest::createValidStopTimeItem(_masterController, this); + _validStopTimeItem = CameraSectionTest::createValidStopTimeItem(_masterController, this); +} + +void LandingComplexItemTest::cleanup(void) +{ + delete _multiSpy; + delete _validStopVideoItem; + delete _validStopDistanceItem; + delete _validStopTimeItem; + VisualMissionItemTest::cleanup(); +} + +void LandingComplexItemTest::_testDirty(void) +{ + QVERIFY(!_item->dirty()); + _item->setDirty(true); + QVERIFY(_item->dirty()); + QVERIFY(_viMultiSpy->checkOnlySignalByMask(dirtyChangedMask)); + QVERIFY(_viMultiSpy->pullBoolFromSignalIndex(dirtyChangedIndex)); + _item->setDirty(false); + _viMultiSpy->clearAllSignals(); + + // These facts should set dirty when changed + QList rgFacts; + rgFacts << _item->finalApproachAltitude() + << _item->landingHeading() + << _item->loiterRadius() + << _item->loiterClockwise() + << _item->landingAltitude() + << _item->landingDistance() + << _item->useLoiterToAlt() + << _item->stopTakingPhotos() + << _item->stopTakingVideo(); + for(Fact* fact: rgFacts) { + qDebug() << fact->name(); + QVERIFY(!_item->dirty()); + changeFactValue(fact); + QVERIFY(_viMultiSpy->checkSignalByMask(dirtyChangedMask)); + QVERIFY(_viMultiSpy->pullBoolFromSignalIndex(dirtyChangedIndex)); + _item->setDirty(false); + _viMultiSpy->clearAllSignals(); + } + + // These bool properties should set dirty when changed + QList rgBoolNames; + rgBoolNames << "altitudesAreRelative"; + const QMetaObject* metaObject = _item->metaObject(); + for(const char* boolName: rgBoolNames) { + qDebug() << boolName; + QVERIFY(!_item->dirty()); + QMetaProperty boolProp = metaObject->property(metaObject->indexOfProperty(boolName)); + QVERIFY(boolProp.write(_item, !boolProp.read(_item).toBool())); + QVERIFY(_viMultiSpy->checkSignalByMask(dirtyChangedMask)); + QVERIFY(_viMultiSpy->pullBoolFromSignalIndex(dirtyChangedIndex)); + _item->setDirty(false); + _viMultiSpy->clearAllSignals(); + } + + // These coordinates should set dirty when changed + + QVERIFY(!_item->dirty()); + _item->setFinalApproachCoordinate(changeCoordinateValue(_item->finalApproachCoordinate())); + QVERIFY(_viMultiSpy->checkSignalByMask(dirtyChangedMask)); + QVERIFY(_viMultiSpy->pullBoolFromSignalIndex(dirtyChangedIndex)); + _item->setDirty(false); + _viMultiSpy->clearAllSignals(); + + QVERIFY(!_item->dirty()); + _item->setLandingCoordinate(changeCoordinateValue(_item->landingCoordinate())); + QVERIFY(_viMultiSpy->checkSignalByMask(dirtyChangedMask)); + QVERIFY(_viMultiSpy->pullBoolFromSignalIndex(dirtyChangedIndex)); + _item->setDirty(false); + _viMultiSpy->clearAllSignals(); +} + +void LandingComplexItemTest::_testItemCount(void) +{ + QList items; + + struct TestCase_s { + bool stopTakingPhotos; + bool stopTakingVideo; + } rgTestCases[] = { + { false, false }, + { false, true }, + { true, false }, + { true, true }, + }; + + for (size_t i=0; istopTakingPhotos()->setRawValue(testCase.stopTakingPhotos); + _item->stopTakingVideo()->setRawValue(testCase.stopTakingVideo); + _item->appendMissionItems(items, this); + QCOMPARE(items.count(), 3 + (testCase.stopTakingPhotos * CameraSection::stopTakingPhotosCommandCount()) + (testCase.stopTakingVideo * CameraSection::stopTakingVideoCommandCount())); + QCOMPARE(items.count() - 1, _item->lastSequenceNumber()); + items.clear(); + } +} + +void LandingComplexItemTest::_testAppendSectionItems(void) +{ + QList rgMissionItems; + + struct TestCase_s { + bool stopTakingPhotos; + bool stopTakingVideo; + bool useLoiterToAlt; + } rgTestCases[] = { + { false, false, false }, + { false, false, true }, + { false, true, false }, + { false, true, true }, + { true, false, false }, + { true, false, true }, + { true, true, false }, + { true, true, true }, + }; + + for (size_t i=0; istopTakingPhotos()->setRawValue(testCase.stopTakingPhotos); + _item->stopTakingVideo()->setRawValue(testCase.stopTakingVideo); + _item->useLoiterToAlt()->setRawValue(testCase.useLoiterToAlt); + _item->appendMissionItems(rgMissionItems, this); + + // First item should be DO_LAND_START always + QCOMPARE(rgMissionItems[0]->command(), MAV_CMD_DO_LAND_START); + + // Next come stop photo/video + + // Convert to simple visual items for verification + QmlObjectListModel* simpleItems = new QmlObjectListModel(this); + for (MissionItem* item: rgMissionItems) { + SimpleMissionItem* simpleItem = new SimpleMissionItem(_masterController, false /* flyView */, false /* forLoad */, simpleItems); + simpleItem->missionItem() = *item; + simpleItems->append(simpleItem); + } + + // Validate stop commands + QCOMPARE(CameraSection::scanStopTakingPhotos(simpleItems, 1, false /* removeScannedItems */), testCase.stopTakingPhotos); + QCOMPARE(CameraSection::scanStopTakingVideo(simpleItems, 1 + (testCase.stopTakingPhotos ? CameraSection::stopTakingPhotosCommandCount() : 0), false /* removeScannedItems */), testCase.stopTakingVideo); + + // Lastly is final approach item and land + int finalApproachIndex = 1 + (testCase.stopTakingPhotos ? CameraSection::stopTakingPhotosCommandCount() : 0) + (testCase.stopTakingVideo ? CameraSection::stopTakingVideoCommandCount() : 0); + QCOMPARE(rgMissionItems[finalApproachIndex]->command(), testCase.useLoiterToAlt ? MAV_CMD_NAV_LOITER_TO_ALT : MAV_CMD_NAV_WAYPOINT); + qDebug() << rgMissionItems[finalApproachIndex+1]->command(); + QCOMPARE(rgMissionItems[finalApproachIndex+1]->command(), MAV_CMD_NAV_LAND); + + simpleItems->deleteLater(); + rgMissionItems.clear(); + } +} + +void LandingComplexItemTest::_testScanForItems(void) +{ + QList rgMissionItems; + + struct TestCase_s { + bool stopTakingPhotos; + bool stopTakingVideo; + bool useLoiterToAlt; + } rgTestCases[] = { + { false, false, false }, + { false, false, true }, + { false, true, false }, + { false, true, true }, + { true, false, false }, + { true, false, true }, + { true, true, false }, + { true, true, true }, + }; + + for (size_t i=0; istopTakingPhotos()->setRawValue(testCase.stopTakingPhotos); + _item->stopTakingVideo()->setRawValue(testCase.stopTakingVideo); + _item->useLoiterToAlt()->setRawValue(testCase.useLoiterToAlt); + _item->appendMissionItems(rgMissionItems, this); + + // Convert to simple visual items for _scan + QmlObjectListModel* visualItems = new QmlObjectListModel(this); + for (MissionItem* item: rgMissionItems) { + SimpleMissionItem* simpleItem = new SimpleMissionItem(_masterController, false /* flyView */, false /* forLoad */, visualItems); + simpleItem->missionItem() = *item; + visualItems->append(simpleItem); + } + + QVERIFY(LandingComplexItem::_scanForItem(visualItems, false /* flyView */, _masterController, &SimpleLandingComplexItem::_isValidLandItem, &SimpleLandingComplexItem::_createItem)); + QCOMPARE(visualItems->count(), 1); + SimpleLandingComplexItem* scannedItem = visualItems->value(0); + QVERIFY(scannedItem); + _validateItem(scannedItem, _item); + + visualItems->deleteLater(); + rgMissionItems.clear(); + } +} + +void LandingComplexItemTest::_testSaveLoad(void) +{ + QString errorString; + + QJsonObject saveObject = _item->_save(); + + saveObject[JsonHelper::jsonVersionKey] = 1; + saveObject[VisualMissionItem::jsonTypeKey] = VisualMissionItem::jsonTypeComplexItemValue; + saveObject[ComplexMissionItem::jsonComplexItemTypeKey] = SimpleLandingComplexItem::jsonComplexItemTypeValue; + + // Test useDeprecatedRelAltKeys = false + SimpleLandingComplexItem* newItem = new SimpleLandingComplexItem(_masterController, false /* flyView */, this /* parent */); + bool loadSuccess = newItem->_load(saveObject, 0 /* sequenceNumber */, SimpleLandingComplexItem::jsonComplexItemTypeValue, false /* useDeprecatedRelAltKeys */, errorString); + if (!loadSuccess) { + qDebug() << "_load failed" << errorString; + } + QVERIFY(loadSuccess); + QVERIFY(errorString.isEmpty()); + _validateItem(newItem, _item); + newItem->deleteLater(); + + // Test useDeprecatedRelAltKeys = true + bool relAlt = saveObject[LandingComplexItem::_jsonAltitudesAreRelativeKey].toBool(); + saveObject[LandingComplexItem::_jsonDeprecatedLoiterAltitudeRelativeKey] = relAlt; + saveObject[LandingComplexItem::_jsonDeprecatedLandingAltitudeRelativeKey] = relAlt; + saveObject.remove(LandingComplexItem::_jsonAltitudesAreRelativeKey); + newItem = new SimpleLandingComplexItem(_masterController, false /* flyView */, this /* parent */); + loadSuccess = newItem->_load(saveObject, 0 /* sequenceNumber */, SimpleLandingComplexItem::jsonComplexItemTypeValue, true /* useDeprecatedRelAltKeys */, errorString); + if (!loadSuccess) { + qDebug() << "_load failed" << errorString; + } + QVERIFY(loadSuccess); + QVERIFY(errorString.isEmpty()); + _validateItem(newItem, _item); + newItem->deleteLater(); + + // Test for _jsonDeprecatedLoiterCoordinateKey support + saveObject = _item->_save(); + saveObject[JsonHelper::jsonVersionKey] = 1; + saveObject[VisualMissionItem::jsonTypeKey] = VisualMissionItem::jsonTypeComplexItemValue; + saveObject[ComplexMissionItem::jsonComplexItemTypeKey] = SimpleLandingComplexItem::jsonComplexItemTypeValue; + saveObject[LandingComplexItem::_jsonDeprecatedLoiterCoordinateKey] = saveObject[LandingComplexItem::_jsonFinalApproachCoordinateKey]; + saveObject.remove(LandingComplexItem::_jsonFinalApproachCoordinateKey); + newItem = new SimpleLandingComplexItem(_masterController, false /* flyView */, this /* parent */); + loadSuccess = newItem->_load(saveObject, 0 /* sequenceNumber */, SimpleLandingComplexItem::jsonComplexItemTypeValue, false /* useDeprecatedRelAltKeys */, errorString); + if (!loadSuccess) { + qDebug() << "_load failed" << errorString; + } + QVERIFY(loadSuccess); + QVERIFY(errorString.isEmpty()); + _validateItem(newItem, _item); + newItem->deleteLater(); +} + +void LandingComplexItemTest::_validateItem(LandingComplexItem* actualItem, LandingComplexItem* expectedItem) +{ + QVERIFY(actualItem); + + QCOMPARE(actualItem->stopTakingPhotos()->rawValue().toBool(), expectedItem->stopTakingPhotos()->rawValue().toBool()); + QCOMPARE(actualItem->stopTakingVideo()->rawValue().toBool(), expectedItem->stopTakingVideo()->rawValue().toBool()); + QCOMPARE(actualItem->useLoiterToAlt()->rawValue().toBool(), expectedItem->useLoiterToAlt()->rawValue().toBool()); + QCOMPARE(actualItem->finalApproachAltitude()->rawValue().toInt(), expectedItem->finalApproachAltitude()->rawValue().toInt()); + QCOMPARE(actualItem->landingAltitude()->rawValue().toInt(), expectedItem->landingAltitude()->rawValue().toInt()); + QCOMPARE(actualItem->landingHeading()->rawValue().toInt(), expectedItem->landingHeading()->rawValue().toInt()); + QCOMPARE(actualItem->landingDistance()->rawValue().toInt(), expectedItem->landingDistance()->rawValue().toInt()); + QCOMPARE(actualItem->altitudesAreRelative(), expectedItem->altitudesAreRelative()); + QCOMPARE(actualItem->landingCoordSet(), expectedItem->landingCoordSet()); + + QVERIFY(fuzzyCompareLatLon(actualItem->finalApproachCoordinate(), expectedItem->finalApproachCoordinate())); + QVERIFY(fuzzyCompareLatLon(actualItem->landingCoordinate(), expectedItem->landingCoordinate())); + if (actualItem->useLoiterToAlt()->rawValue().toBool()) { + QVERIFY(fuzzyCompareLatLon(actualItem->loiterTangentCoordinate(), expectedItem->loiterTangentCoordinate())); + QCOMPARE(actualItem->loiterRadius()->rawValue().toInt(), expectedItem->loiterRadius()->rawValue().toInt()); + QCOMPARE(actualItem->loiterClockwise()->rawValue().toBool(), expectedItem->loiterClockwise()->rawValue().toBool()); + } +} + +SimpleLandingComplexItem::SimpleLandingComplexItem(PlanMasterController* masterController, bool flyView, QObject* parent) + : LandingComplexItem (masterController, flyView, parent) + , _metaDataMap (FactMetaData::createMapFromJsonFile(QStringLiteral(":/json/VTOLLandingPattern.FactMetaData.json"), this)) + , _landingDistanceFact (settingsGroup, _metaDataMap[finalApproachToLandDistanceName]) + , _finalApproachAltitudeFact(settingsGroup, _metaDataMap[finalApproachAltitudeName]) + , _loiterRadiusFact (settingsGroup, _metaDataMap[loiterRadiusName]) + , _loiterClockwiseFact (settingsGroup, _metaDataMap[loiterClockwiseName]) + , _landingHeadingFact (settingsGroup, _metaDataMap[landingHeadingName]) + , _landingAltitudeFact (settingsGroup, _metaDataMap[landingAltitudeName]) + , _useLoiterToAltFact (settingsGroup, _metaDataMap[useLoiterToAltName]) + , _stopTakingPhotosFact (settingsGroup, _metaDataMap[stopTakingPhotosName]) + , _stopTakingVideoFact (settingsGroup, _metaDataMap[stopTakingVideoName]) +{ + _isIncomplete = false; + + _init(); + _recalcFromHeadingAndDistanceChange(); + + setDirty(false); +} + +MissionItem* SimpleLandingComplexItem::_createLandItem(int seqNum, bool altRel, double lat, double lon, double alt, QObject* parent) +{ + return new MissionItem(seqNum, + MAV_CMD_NAV_LAND, + altRel ? MAV_FRAME_GLOBAL_RELATIVE_ALT : MAV_FRAME_GLOBAL, + 0.0, 0.0, 0.0, 0.0, + lat, lon, alt, + true, // autoContinue + false, // isCurrentItem + parent); + +} + +bool SimpleLandingComplexItem::_isValidLandItem(const MissionItem& missionItem) +{ + if (missionItem.command() != MAV_CMD_NAV_LAND || + !(missionItem.frame() == MAV_FRAME_GLOBAL_RELATIVE_ALT || missionItem.frame() == MAV_FRAME_GLOBAL) || + missionItem.param1() != 0 || missionItem.param2() != 0 || missionItem.param3() != 0 || missionItem.param4() != 0 || + qIsNaN(missionItem.param5()) || qIsNaN(missionItem.param6()) || qIsNaN(missionItem.param7())) { + return false; + } else { + return true; + } +} diff --git a/src/MissionManager/LandingComplexItemTest.h b/src/MissionManager/LandingComplexItemTest.h new file mode 100644 index 000000000..cf32f8230 --- /dev/null +++ b/src/MissionManager/LandingComplexItemTest.h @@ -0,0 +1,123 @@ +/**************************************************************************** + * + * (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 "VisualMissionItemTest.h" +#include "LandingComplexItem.h" +#include "MultiSignalSpy.h" +#include "PlanMasterController.h" + +class LandingComplexItemTest : public VisualMissionItemTest +{ + Q_OBJECT + +public: + LandingComplexItemTest(void); + + void cleanup(void) override; + void init (void) override; + +private slots: + void _testDirty (void); + void _testItemCount (void); + void _testAppendSectionItems (void); + void _testScanForItems (void); + void _testSaveLoad (void); + +private: + void _validateItem(LandingComplexItem* actualItem, LandingComplexItem* expectedItem); + + enum { + finalApproachCoordinateChangedIndex = 0, + loiterTangentCoordinateChangedIndex, + landingCoordinateChangedIndex, + landingCoordSetChangedIndex, + loiterClockwiseChangedIndex, + useLoiterToAltChangedIndex, + altitudesAreRelativeChangedIndex, + _updateFlightPathSegmentsSignalIndex, + maxSignalIndex, + }; + + enum { + finalApproachCoordinateChangedMask = 1 << finalApproachCoordinateChangedIndex, + loiterTangentCoordinateChangedMask = 1 << loiterTangentCoordinateChangedIndex, + landingCoordinateChangedMask = 1 << landingCoordinateChangedIndex, + landingCoordSetChangedMask = 1 << landingCoordSetChangedIndex, + loiterClockwiseChangedMask = 1 << loiterClockwiseChangedIndex, + useLoiterToAltChangedMask = 1 << useLoiterToAltChangedIndex, + altitudesAreRelativeChangedMask = 1 << altitudesAreRelativeChangedIndex, + _updateFlightPathSegmentsSignalMask = 1 << _updateFlightPathSegmentsSignalIndex, + }; + + static const size_t cSignals = maxSignalIndex; + const char* rgSignals[cSignals]; + + LandingComplexItem* _item = nullptr; + MultiSignalSpy* _multiSpy = nullptr; + MultiSignalSpy* _viMultiSpy = nullptr; + SimpleMissionItem* _validStopVideoItem = nullptr; + SimpleMissionItem* _validStopDistanceItem = nullptr; + SimpleMissionItem* _validStopTimeItem = nullptr; +}; + +// Simple class used to test LandingComplexItem base class +class SimpleLandingComplexItem : public LandingComplexItem +{ + Q_OBJECT + +public: + SimpleLandingComplexItem(PlanMasterController* masterController, bool flyView, QObject* parent); + + // Overrides from ComplexMissionItem + QString patternName (void) const final { return QString(); } + bool load (const QJsonObject& /*complexObject*/, int /*sequenceNumber*/, QString& /*errorString*/) final { return false; }; + QString mapVisualQML(void) const final { return QStringLiteral("FWLandingPatternMapVisual.qml"); } + + // Overrides from VisualMissionItem + void save (QJsonArray& /*missionItems*/) { }; + + static const QString name; + + static const char* jsonComplexItemTypeValue; + + static const char* settingsGroup; + +private: + static LandingComplexItem* _createItem (PlanMasterController* masterController, bool flyView, QObject* parent) { return new SimpleLandingComplexItem(masterController, flyView, parent); } + static bool _isValidLandItem(const MissionItem& missionItem); + + // Overrides from LandingComplexItem + const Fact* _finalApproachAltitude (void) const final { return &_finalApproachAltitudeFact; } + const Fact* _loiterRadius (void) const final { return &_loiterRadiusFact; } + const Fact* _loiterClockwise (void) const final { return &_loiterClockwiseFact; } + const Fact* _landingAltitude (void) const final { return &_landingAltitudeFact; } + const Fact* _landingDistance (void) const final { return &_landingDistanceFact; } + const Fact* _landingHeading (void) const final { return &_landingHeadingFact; } + const Fact* _useLoiterToAlt (void) const final { return &_useLoiterToAltFact; } + const Fact* _stopTakingPhotos (void) const final { return &_stopTakingPhotosFact; } + const Fact* _stopTakingVideo (void) const final { return &_stopTakingVideoFact; } + void _calcGlideSlope (void) final { }; + MissionItem* _createLandItem (int seqNum, bool altRel, double lat, double lon, double alt, QObject* parent) final; + + QMap _metaDataMap; + + Fact _landingDistanceFact; + Fact _finalApproachAltitudeFact; + Fact _loiterRadiusFact; + Fact _loiterClockwiseFact; + Fact _landingHeadingFact; + Fact _landingAltitudeFact; + Fact _useLoiterToAltFact; + Fact _stopTakingPhotosFact; + Fact _stopTakingVideoFact; + + friend class LandingComplexItemTest; +}; diff --git a/src/MissionManager/MissionController.cc b/src/MissionManager/MissionController.cc index a920d34a3..ed36ff6ab 100644 --- a/src/MissionManager/MissionController.cc +++ b/src/MissionManager/MissionController.cc @@ -2153,8 +2153,10 @@ void MissionController::setDirty(bool dirty) 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, masterController); + // First we look for a Landing Patterns which are at the end + if (!FixedWingLandingComplexItem::scanForItem(visualItems, _flyView, masterController)) { + VTOLLandingComplexItem::scanForItem(visualItems, _flyView, masterController); + } int scanIndex = 0; while (scanIndex < visualItems->count()) { diff --git a/src/MissionManager/VTOLLandingComplexItem.cc b/src/MissionManager/VTOLLandingComplexItem.cc index 8985d1c06..808ccec96 100644 --- a/src/MissionManager/VTOLLandingComplexItem.cc +++ b/src/MissionManager/VTOLLandingComplexItem.cc @@ -25,35 +25,21 @@ const QString VTOLLandingComplexItem::name(tr("VTOL Landing")); const char* VTOLLandingComplexItem::settingsGroup = "VTOLLanding"; const char* VTOLLandingComplexItem::jsonComplexItemTypeValue = "vtolLandingPattern"; -const char* VTOLLandingComplexItem::loiterToLandDistanceName = "LandingDistance"; -const char* VTOLLandingComplexItem::landingHeadingName = "LandingHeading"; -const char* VTOLLandingComplexItem::loiterAltitudeName = "LoiterAltitude"; -const char* VTOLLandingComplexItem::loiterRadiusName = "LoiterRadius"; -const char* VTOLLandingComplexItem::landingAltitudeName = "LandingAltitude"; -const char* VTOLLandingComplexItem::stopTakingPhotosName = "StopTakingPhotos"; -const char* VTOLLandingComplexItem::stopTakingVideoName = "StopTakingVideo"; - -const char* VTOLLandingComplexItem::_jsonLoiterCoordinateKey = "loiterCoordinate"; -const char* VTOLLandingComplexItem::_jsonLoiterRadiusKey = "loiterRadius"; -const char* VTOLLandingComplexItem::_jsonLoiterClockwiseKey = "loiterClockwise"; -const char* VTOLLandingComplexItem::_jsonLandingCoordinateKey = "landCoordinate"; -const char* VTOLLandingComplexItem::_jsonAltitudesAreRelativeKey = "altitudesAreRelative"; -const char* VTOLLandingComplexItem::_jsonStopTakingPhotosKey = "stopTakingPhotos"; -const char* VTOLLandingComplexItem::_jsonStopTakingVideoKey = "stopVideoPhotos"; - VTOLLandingComplexItem::VTOLLandingComplexItem(PlanMasterController* masterController, bool flyView, QObject* parent) : LandingComplexItem (masterController, flyView, parent) , _metaDataMap (FactMetaData::createMapFromJsonFile(QStringLiteral(":/json/VTOLLandingPattern.FactMetaData.json"), this)) - , _landingDistanceFact (settingsGroup, _metaDataMap[loiterToLandDistanceName]) - , _loiterAltitudeFact (settingsGroup, _metaDataMap[loiterAltitudeName]) + , _landingDistanceFact (settingsGroup, _metaDataMap[finalApproachToLandDistanceName]) + , _finalApproachAltitudeFact(settingsGroup, _metaDataMap[finalApproachAltitudeName]) , _loiterRadiusFact (settingsGroup, _metaDataMap[loiterRadiusName]) + , _loiterClockwiseFact (settingsGroup, _metaDataMap[loiterClockwiseName]) , _landingHeadingFact (settingsGroup, _metaDataMap[landingHeadingName]) , _landingAltitudeFact (settingsGroup, _metaDataMap[landingAltitudeName]) + , _useLoiterToAltFact (settingsGroup, _metaDataMap[useLoiterToAltName]) , _stopTakingPhotosFact (settingsGroup, _metaDataMap[stopTakingPhotosName]) , _stopTakingVideoFact (settingsGroup, _metaDataMap[stopTakingVideoName]) { - _editorQml = "qrc:/qml/VTOLLandingPatternEditor.qml"; - _isIncomplete = false; + _editorQml = "qrc:/qml/VTOLLandingPatternEditor.qml"; + _isIncomplete = false; _init(); @@ -66,237 +52,42 @@ VTOLLandingComplexItem::VTOLLandingComplexItem(PlanMasterController* masterContr _landingDistanceFact.metaData()->setRawMin(vtolTransitionDistanceFact->metaData()->rawMin()); } - connect(&_loiterAltitudeFact, &Fact::valueChanged, this, &VTOLLandingComplexItem::_updateLoiterCoodinateAltitudeFromFact); - connect(&_landingAltitudeFact, &Fact::valueChanged, this, &VTOLLandingComplexItem::_updateLandingCoodinateAltitudeFromFact); - - connect(this, &VTOLLandingComplexItem::altitudesAreRelativeChanged, this, &VTOLLandingComplexItem::_amslEntryAltChanged); - connect(this, &VTOLLandingComplexItem::altitudesAreRelativeChanged, this, &VTOLLandingComplexItem::_amslExitAltChanged); - connect(_missionController, &MissionController::plannedHomePositionChanged, this, &VTOLLandingComplexItem::_amslEntryAltChanged); - connect(_missionController, &MissionController::plannedHomePositionChanged, this, &VTOLLandingComplexItem::_amslExitAltChanged); - connect(&_loiterAltitudeFact, &Fact::valueChanged, this, &VTOLLandingComplexItem::_amslEntryAltChanged); - connect(&_landingAltitudeFact, &Fact::valueChanged, this, &VTOLLandingComplexItem::_amslExitAltChanged); - connect(this, &VTOLLandingComplexItem::amslEntryAltChanged, this, &VTOLLandingComplexItem::maxAMSLAltitudeChanged); - connect(this, &VTOLLandingComplexItem::amslExitAltChanged, this, &VTOLLandingComplexItem::minAMSLAltitudeChanged); - - connect(&_stopTakingPhotosFact, &Fact::valueChanged, this, &VTOLLandingComplexItem::_signalLastSequenceNumberChanged); - connect(&_stopTakingVideoFact, &Fact::valueChanged, this, &VTOLLandingComplexItem::_signalLastSequenceNumberChanged); - - connect(&_loiterAltitudeFact, &Fact::valueChanged, this, &VTOLLandingComplexItem::_setDirty); - connect(&_landingAltitudeFact, &Fact::valueChanged, this, &VTOLLandingComplexItem::_setDirty); - connect(&_landingDistanceFact, &Fact::valueChanged, this, &VTOLLandingComplexItem::_setDirty); - connect(&_landingHeadingFact, &Fact::valueChanged, this, &VTOLLandingComplexItem::_setDirty); - connect(&_loiterRadiusFact, &Fact::valueChanged, this, &VTOLLandingComplexItem::_setDirty); - connect(&_stopTakingPhotosFact, &Fact::valueChanged, this, &VTOLLandingComplexItem::_setDirty); - connect(&_stopTakingVideoFact, &Fact::valueChanged, this, &VTOLLandingComplexItem::_setDirty); - connect(this, &VTOLLandingComplexItem::loiterCoordinateChanged, this, &VTOLLandingComplexItem::_setDirty); - connect(this, &VTOLLandingComplexItem::landingCoordinateChanged, this, &VTOLLandingComplexItem::_setDirty); - connect(this, &VTOLLandingComplexItem::loiterClockwiseChanged, this, &VTOLLandingComplexItem::_setDirty); - connect(this, &VTOLLandingComplexItem::altitudesAreRelativeChanged, this, &VTOLLandingComplexItem::_setDirty); - - connect(this, &VTOLLandingComplexItem::landingCoordSetChanged, this, &VTOLLandingComplexItem::readyForSaveStateChanged); - connect(this, &VTOLLandingComplexItem::wizardModeChanged, this, &VTOLLandingComplexItem::readyForSaveStateChanged); - - connect(this, &VTOLLandingComplexItem::loiterCoordinateChanged, this, &VTOLLandingComplexItem::complexDistanceChanged); - connect(this, &VTOLLandingComplexItem::loiterTangentCoordinateChanged,this, &VTOLLandingComplexItem::complexDistanceChanged); - connect(this, &VTOLLandingComplexItem::landingCoordinateChanged, this, &VTOLLandingComplexItem::complexDistanceChanged); - - connect(this, &VTOLLandingComplexItem::loiterTangentCoordinateChanged,this, &VTOLLandingComplexItem::_updateFlightPathSegmentsSignal); - connect(this, &VTOLLandingComplexItem::landingCoordinateChanged, this, &VTOLLandingComplexItem::_updateFlightPathSegmentsSignal); - connect(&_loiterAltitudeFact, &Fact::valueChanged, this, &VTOLLandingComplexItem::_updateFlightPathSegmentsSignal); - connect(&_landingAltitudeFact, &Fact::valueChanged, this, &VTOLLandingComplexItem::_updateFlightPathSegmentsSignal); - connect(this, &VTOLLandingComplexItem::altitudesAreRelativeChanged, this, &VTOLLandingComplexItem::_updateFlightPathSegmentsSignal); - connect(_missionController, &MissionController::plannedHomePositionChanged, this, &VTOLLandingComplexItem::_updateFlightPathSegmentsSignal); - - // The follow is used to compress multiple recalc calls in a row to into a single call. - connect(this, &VTOLLandingComplexItem::_updateFlightPathSegmentsSignal, this, &VTOLLandingComplexItem::_updateFlightPathSegmentsDontCallDirectly, Qt::QueuedConnection); - qgcApp()->addCompressedSignal(QMetaMethod::fromSignal(&VTOLLandingComplexItem::_updateFlightPathSegmentsSignal)); - - if (_masterController->controllerVehicle()->apmFirmware()) { - // ArduPilot does not support camera commands - _stopTakingVideoFact.setRawValue(false); - _stopTakingPhotosFact.setRawValue(false); - } - _recalcFromHeadingAndDistanceChange(); setDirty(false); } -int VTOLLandingComplexItem::lastSequenceNumber(void) const -{ - // Fixed items are: - // land start, loiter, land - // Optional items are: - // stop photos/video - return _sequenceNumber + 2 + (_stopTakingPhotosFact.rawValue().toBool() ? 2 : 0) + (_stopTakingVideoFact.rawValue().toBool() ? 1 : 0); -} - -void VTOLLandingComplexItem::setDirty(bool dirty) -{ - if (_dirty != dirty) { - _dirty = dirty; - emit dirtyChanged(_dirty); - } -} - void VTOLLandingComplexItem::save(QJsonArray& missionItems) { - QJsonObject saveObject; + QJsonObject saveObject = _save(); saveObject[JsonHelper::jsonVersionKey] = 1; saveObject[VisualMissionItem::jsonTypeKey] = VisualMissionItem::jsonTypeComplexItemValue; saveObject[ComplexMissionItem::jsonComplexItemTypeKey] = jsonComplexItemTypeValue; - QGeoCoordinate coordinate; - QJsonValue jsonCoordinate; - - coordinate = _loiterCoordinate; - coordinate.setAltitude(_loiterAltitudeFact.rawValue().toDouble()); - JsonHelper::saveGeoCoordinate(coordinate, true /* writeAltitude */, jsonCoordinate); - saveObject[_jsonLoiterCoordinateKey] = jsonCoordinate; - - coordinate = _landingCoordinate; - coordinate.setAltitude(_landingAltitudeFact.rawValue().toDouble()); - JsonHelper::saveGeoCoordinate(coordinate, true /* writeAltitude */, jsonCoordinate); - saveObject[_jsonLandingCoordinateKey] = jsonCoordinate; - - saveObject[_jsonLoiterRadiusKey] = _loiterRadiusFact.rawValue().toDouble(); - saveObject[_jsonStopTakingPhotosKey] = _stopTakingPhotosFact.rawValue().toBool(); - saveObject[_jsonStopTakingVideoKey] = _stopTakingVideoFact.rawValue().toBool(); - saveObject[_jsonLoiterClockwiseKey] = _loiterClockwise; - saveObject[_jsonAltitudesAreRelativeKey] = _altitudesAreRelative; - missionItems.append(saveObject); } -void VTOLLandingComplexItem::setSequenceNumber(int sequenceNumber) -{ - if (_sequenceNumber != sequenceNumber) { - _sequenceNumber = sequenceNumber; - emit sequenceNumberChanged(sequenceNumber); - emit lastSequenceNumberChanged(lastSequenceNumber()); - } -} - bool VTOLLandingComplexItem::load(const QJsonObject& complexObject, int sequenceNumber, QString& errorString) { QList keyInfoList = { - { JsonHelper::jsonVersionKey, QJsonValue::Double, true }, - { VisualMissionItem::jsonTypeKey, QJsonValue::String, true }, - { ComplexMissionItem::jsonComplexItemTypeKey, QJsonValue::String, true }, - { _jsonLoiterCoordinateKey, QJsonValue::Array, true }, - { _jsonLoiterRadiusKey, QJsonValue::Double, true }, - { _jsonLoiterClockwiseKey, QJsonValue::Bool, true }, - { _jsonLandingCoordinateKey, QJsonValue::Array, true }, - { _jsonStopTakingPhotosKey, QJsonValue::Bool, false }, - { _jsonStopTakingVideoKey, QJsonValue::Bool, false }, + { JsonHelper::jsonVersionKey, QJsonValue::Double, true }, }; if (!JsonHelper::validateKeys(complexObject, keyInfoList, errorString)) { return false; } - QString itemType = complexObject[VisualMissionItem::jsonTypeKey].toString(); - QString complexType = complexObject[ComplexMissionItem::jsonComplexItemTypeKey].toString(); - if (itemType != VisualMissionItem::jsonTypeComplexItemValue || complexType != jsonComplexItemTypeValue) { - errorString = tr("%1 does not support loading this complex mission item type: %2:%3").arg(qgcApp()->applicationName()).arg(itemType).arg(complexType); - return false; - } - - setSequenceNumber(sequenceNumber); - - _ignoreRecalcSignals = true; - int version = complexObject[JsonHelper::jsonVersionKey].toInt(); - if (version == 1) { - QList v2KeyInfoList = { - { _jsonAltitudesAreRelativeKey, QJsonValue::Bool, true }, - }; - if (!JsonHelper::validateKeys(complexObject, v2KeyInfoList, errorString)) { - _ignoreRecalcSignals = false; - return false; - } - - _altitudesAreRelative = complexObject[_jsonAltitudesAreRelativeKey].toBool(); - } else { + if (version != 1) { errorString = tr("%1 complex item version %2 not supported").arg(jsonComplexItemTypeValue).arg(version); _ignoreRecalcSignals = false; return false; } - QGeoCoordinate coordinate; - if (!JsonHelper::loadGeoCoordinate(complexObject[_jsonLoiterCoordinateKey], true /* altitudeRequired */, coordinate, errorString)) { - return false; - } - _loiterCoordinate = coordinate; - _loiterAltitudeFact.setRawValue(coordinate.altitude()); - - if (!JsonHelper::loadGeoCoordinate(complexObject[_jsonLandingCoordinateKey], true /* altitudeRequired */, coordinate, errorString)) { - return false; - } - _landingCoordinate = coordinate; - _landingAltitudeFact.setRawValue(coordinate.altitude()); - - _loiterRadiusFact.setRawValue(complexObject[_jsonLoiterRadiusKey].toDouble()); - _loiterClockwise = complexObject[_jsonLoiterClockwiseKey].toBool(); - - if (complexObject.contains(_jsonStopTakingPhotosKey)) { - _stopTakingPhotosFact.setRawValue(complexObject[_jsonStopTakingPhotosKey].toBool()); - } else { - _stopTakingPhotosFact.setRawValue(false); - } - if (complexObject.contains(_jsonStopTakingVideoKey)) { - _stopTakingVideoFact.setRawValue(complexObject[_jsonStopTakingVideoKey].toBool()); - } else { - _stopTakingVideoFact.setRawValue(false); - } - - _landingCoordSet = true; - - _ignoreRecalcSignals = false; - - _recalcFromCoordinateChange(); - emit coordinateChanged(this->coordinate()); // This will kick off terrain query - - return true; -} - -double VTOLLandingComplexItem::greatestDistanceTo(const QGeoCoordinate &other) const -{ - return qMax(_loiterCoordinate.distanceTo(other),_landingCoordinate.distanceTo(other)); -} - -bool VTOLLandingComplexItem::specifiesCoordinate(void) const -{ - return true; -} - -MissionItem* VTOLLandingComplexItem::createDoLandStartItem(int seqNum, QObject* parent) -{ - return new MissionItem(seqNum, // sequence number - MAV_CMD_DO_LAND_START, // MAV_CMD - MAV_FRAME_MISSION, // MAV_FRAME - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, // param 1-7 - true, // autoContinue - false, // isCurrentItem - parent); -} - -MissionItem* VTOLLandingComplexItem::createLoiterToAltItem(int seqNum, bool altRel, double loiterRadius, double lat, double lon, double alt, QObject* parent) -{ - return new MissionItem(seqNum, - MAV_CMD_NAV_LOITER_TO_ALT, - altRel ? MAV_FRAME_GLOBAL_RELATIVE_ALT : MAV_FRAME_GLOBAL, - 1.0, // Heading required = true - loiterRadius, // Loiter radius - 0.0, // param 3 - unused - 1.0, // Exit crosstrack - tangent of loiter to land point - lat, lon, alt, - true, // autoContinue - false, // isCurrentItem - parent); + return _load(complexObject, sequenceNumber, jsonComplexItemTypeValue, false /* useDeprecatedRelAltKeys */, errorString); } -MissionItem* VTOLLandingComplexItem::createLandItem(int seqNum, bool altRel, double lat, double lon, double alt, QObject* parent) +MissionItem* VTOLLandingComplexItem::_createLandItem(int seqNum, bool altRel, double lat, double lon, double alt, QObject* parent) { return new MissionItem(seqNum, MAV_CMD_NAV_VTOL_LAND, @@ -309,219 +100,23 @@ MissionItem* VTOLLandingComplexItem::createLandItem(int seqNum, bool altRel, dou } -void VTOLLandingComplexItem::appendMissionItems(QList& items, QObject* missionItemParent) +void VTOLLandingComplexItem::_calcGlideSlope(void) { - int seqNum = _sequenceNumber; - - // IMPORTANT NOTE: Any changes here must also be taken into account in scanForItem - - MissionItem* item = createDoLandStartItem(seqNum++, missionItemParent); - items.append(item); - - - if (_stopTakingPhotosFact.rawValue().toBool()) { - CameraSection::appendStopTakingPhotos(items, seqNum, missionItemParent); - } - - if (_stopTakingVideoFact.rawValue().toBool()) { - CameraSection::appendStopTakingVideo(items, seqNum, missionItemParent); - } - - double loiterRadius = _loiterRadiusFact.rawValue().toDouble() * (_loiterClockwise ? 1.0 : -1.0); - item = createLoiterToAltItem(seqNum++, - _altitudesAreRelative, - loiterRadius, - _loiterCoordinate.latitude(), _loiterCoordinate.longitude(), _loiterAltitudeFact.rawValue().toDouble(), - missionItemParent); - items.append(item); - - item = createLandItem(seqNum++, - _altitudesAreRelative, - _landingCoordinate.latitude(), _landingCoordinate.longitude(), _landingAltitudeFact.rawValue().toDouble(), - missionItemParent); - items.append(item); + // No glide slope calc for VTOL } -bool VTOLLandingComplexItem::scanForItem(QmlObjectListModel* visualItems, bool flyView, PlanMasterController* masterController) +bool VTOLLandingComplexItem::_isValidLandItem(const MissionItem& missionItem) { - qCDebug(VTOLLandingComplexItemLog) << "VTOLLandingComplexItem::scanForItem count" << visualItems->count(); - - if (visualItems->count() < 3) { + if (missionItem.command() != MAV_CMD_NAV_LAND || + !(missionItem.frame() == MAV_FRAME_GLOBAL_RELATIVE_ALT || missionItem.frame() == MAV_FRAME_GLOBAL) || + missionItem.param1() != 0 || missionItem.param2() != 0 || missionItem.param3() != 0 || missionItem.param4() != 0) { return false; + } else { + return true; } - - // A valid fixed wing landing pattern is comprised of the follow commands in this order at the end of the item list: - // MAV_CMD_DO_LAND_START - required - // Stop taking photos sequence - optional - // Stop taking video sequence - optional - // MAV_CMD_NAV_LOITER_TO_ALT - required - // MAV_CMD_NAV_LAND - required - - // Start looking for the commands in reverse order from the end of the list - - int scanIndex = visualItems->count() - 1; - - if (scanIndex < 0 || scanIndex > visualItems->count() - 1) { - return false; - } - SimpleMissionItem* item = visualItems->value(scanIndex--); - if (!item) { - return false; - } - MissionItem& missionItemLand = item->missionItem(); - if (missionItemLand.command() != MAV_CMD_NAV_LAND || - !(missionItemLand.frame() == MAV_FRAME_GLOBAL_RELATIVE_ALT || missionItemLand.frame() == MAV_FRAME_GLOBAL) || - missionItemLand.param1() != 0 || missionItemLand.param2() != 0 || missionItemLand.param3() != 0 || missionItemLand.param4() != 0) { - return false; - } - MAV_FRAME landPointFrame = missionItemLand.frame(); - - if (scanIndex < 0 || scanIndex > visualItems->count() - 1) { - return false; - } - item = visualItems->value(scanIndex); - if (!item) { - return false; - } - MissionItem& missionItemLoiter = item->missionItem(); - if (missionItemLoiter.command() != MAV_CMD_NAV_LOITER_TO_ALT || - missionItemLoiter.frame() != landPointFrame || - missionItemLoiter.param1() != 1.0 || missionItemLoiter.param3() != 0 || missionItemLoiter.param4() != 1.0) { - return false; - } - - scanIndex -= CameraSection::stopTakingVideoCommandCount(); - bool stopTakingVideo = CameraSection::scanStopTakingVideo(visualItems, scanIndex, false /* removeScannedItems */); - if (!stopTakingVideo) { - scanIndex += CameraSection::stopTakingVideoCommandCount(); - } - - scanIndex -= CameraSection::stopTakingPhotosCommandCount(); - bool stopTakingPhotos = CameraSection::scanStopTakingPhotos(visualItems, scanIndex, false /* removeScannedItems */); - if (!stopTakingPhotos) { - scanIndex += CameraSection::stopTakingPhotosCommandCount(); - } - - scanIndex--; - if (scanIndex < 0 || scanIndex > visualItems->count() - 1) { - return false; - } - item = visualItems->value(scanIndex); - if (!item) { - return false; - } - MissionItem& missionItemDoLandStart = item->missionItem(); - if (missionItemDoLandStart.command() != MAV_CMD_DO_LAND_START || - missionItemDoLandStart.frame() != MAV_FRAME_MISSION || - missionItemDoLandStart.param1() != 0 || missionItemDoLandStart.param2() != 0 || missionItemDoLandStart.param3() != 0 || missionItemDoLandStart.param4() != 0|| missionItemDoLandStart.param5() != 0|| missionItemDoLandStart.param6() != 0) { - return false; - } - - // We made it this far so we do have a Fixed Wing Landing Pattern item at the end of the mission. - // Since we have scanned it we need to remove the items for it fromt the list - int deleteCount = 3; - if (stopTakingPhotos) { - deleteCount += CameraSection::stopTakingPhotosCommandCount(); - } - if (stopTakingVideo) { - deleteCount += CameraSection::stopTakingVideoCommandCount(); - } - int firstItem = visualItems->count() - deleteCount; - while (deleteCount--) { - visualItems->removeAt(firstItem)->deleteLater(); - } - - // Now stuff all the scanned information into the item - - VTOLLandingComplexItem* complexItem = new VTOLLandingComplexItem(masterController, flyView, visualItems); - - complexItem->_ignoreRecalcSignals = true; - - complexItem->_altitudesAreRelative = landPointFrame == MAV_FRAME_GLOBAL_RELATIVE_ALT; - complexItem->_loiterRadiusFact.setRawValue(qAbs(missionItemLoiter.param2())); - complexItem->_loiterClockwise = missionItemLoiter.param2() > 0; - complexItem->setLoiterCoordinate(QGeoCoordinate(missionItemLoiter.param5(), missionItemLoiter.param6())); - complexItem->_loiterAltitudeFact.setRawValue(missionItemLoiter.param7()); - - complexItem->_landingCoordinate.setLatitude(missionItemLand.param5()); - complexItem->_landingCoordinate.setLongitude(missionItemLand.param6()); - complexItem->_landingAltitudeFact.setRawValue(missionItemLand.param7()); - - complexItem->_stopTakingPhotosFact.setRawValue(stopTakingPhotos); - complexItem->_stopTakingVideoFact.setRawValue(stopTakingVideo); - - complexItem->_landingCoordSet = true; - - complexItem->_ignoreRecalcSignals = false; - complexItem->_recalcFromCoordinateChange(); - complexItem->setDirty(false); - - visualItems->append(complexItem); - - return true; -} - -double VTOLLandingComplexItem::_mathematicAngleToHeading(double angle) -{ - double heading = (angle - 90) * -1; - if (heading < 0) { - heading += 360; - } - - return heading; -} - -double VTOLLandingComplexItem::_headingToMathematicAngle(double heading) -{ - return heading - 90 * -1; } -void VTOLLandingComplexItem::_updateLoiterCoodinateAltitudeFromFact(void) -{ - _loiterCoordinate.setAltitude(_loiterAltitudeFact.rawValue().toDouble()); - emit loiterCoordinateChanged(_loiterCoordinate); - emit coordinateChanged(_loiterCoordinate); -} - -void VTOLLandingComplexItem::_updateLandingCoodinateAltitudeFromFact(void) -{ - _landingCoordinate.setAltitude(_landingAltitudeFact.rawValue().toDouble()); - emit landingCoordinateChanged(_landingCoordinate); -} - -void VTOLLandingComplexItem::_setDirty(void) -{ - setDirty(true); -} - -void VTOLLandingComplexItem::applyNewAltitude(double newAltitude) -{ - _loiterAltitudeFact.setRawValue(newAltitude); -} - -void VTOLLandingComplexItem::_signalLastSequenceNumberChanged(void) -{ - emit lastSequenceNumberChanged(lastSequenceNumber()); -} - -VTOLLandingComplexItem::ReadyForSaveState VTOLLandingComplexItem::readyForSaveState(void) const -{ - return _landingCoordSet && !_wizardMode ? ReadyForSave : NotReadyForSaveData; -} - - -double VTOLLandingComplexItem::amslEntryAlt(void) const -{ - return _loiterAltitudeFact.rawValue().toDouble() + (_altitudesAreRelative ? _missionController->plannedHomePosition().altitude() : 0); -} - -double VTOLLandingComplexItem::amslExitAlt(void) const -{ - return _landingAltitudeFact.rawValue().toDouble() + (_altitudesAreRelative ? _missionController->plannedHomePosition().altitude() : 0); - -} - -void VTOLLandingComplexItem::_calcGlideSlope(void) +bool VTOLLandingComplexItem::scanForItem(QmlObjectListModel* visualItems, bool flyView, PlanMasterController* masterController) { - // No glide slope calc for VTOL + return _scanForItem(visualItems, flyView, masterController, _isValidLandItem, _createItem); } diff --git a/src/MissionManager/VTOLLandingComplexItem.h b/src/MissionManager/VTOLLandingComplexItem.h index 79f82fdb2..f5a8afc8f 100644 --- a/src/MissionManager/VTOLLandingComplexItem.h +++ b/src/MissionManager/VTOLLandingComplexItem.h @@ -26,98 +26,51 @@ class VTOLLandingComplexItem : public LandingComplexItem public: VTOLLandingComplexItem(PlanMasterController* masterController, bool flyView, QObject* parent); - Fact* loiterAltitude (void) final { return &_loiterAltitudeFact; } - Fact* loiterRadius (void) final { return &_loiterRadiusFact; } - Fact* landingAltitude (void) final { return &_landingAltitudeFact; } - Fact* landingDistance (void) final { return &_landingDistanceFact; } - Fact* landingHeading (void) final { return &_landingHeadingFact; } - Fact* stopTakingPhotos (void) final { return &_stopTakingPhotosFact; } - Fact* stopTakingVideo (void) final { return &_stopTakingVideoFact; } - /// Scans the loaded items for a landing pattern complex item 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); - static MissionItem* createLandItem (int seqNum, bool altRel, double lat, double lon, double alt, QObject* parent); - // Overrides from ComplexMissionItem QString patternName (void) const final { return name; } - int lastSequenceNumber (void) const final; bool load (const QJsonObject& complexObject, int sequenceNumber, QString& errorString) final; - double greatestDistanceTo (const QGeoCoordinate &other) const final; QString mapVisualQML (void) const final { return QStringLiteral("VTOLLandingPatternMapVisual.qml"); } // Overrides from VisualMissionItem - bool dirty (void) const final { return _dirty; } - bool isSimpleItem (void) const final { return false; } - bool isStandaloneCoordinate (void) const final { return false; } - bool specifiesCoordinate (void) const final; - bool specifiesAltitudeOnly (void) const final { return false; } - QString commandDescription (void) const final { return "Landing Pattern"; } - QString commandName (void) const final { return "Landing Pattern"; } - QString abbreviation (void) const final { return "L"; } - QGeoCoordinate coordinate (void) const final { return _loiterCoordinate; } - QGeoCoordinate exitCoordinate (void) const final { return _landingCoordinate; } - int sequenceNumber (void) const final { return _sequenceNumber; } - double specifiedFlightSpeed (void) final { return std::numeric_limits::quiet_NaN(); } - double specifiedGimbalYaw (void) final { return std::numeric_limits::quiet_NaN(); } - double specifiedGimbalPitch (void) final { return std::numeric_limits::quiet_NaN(); } - void appendMissionItems (QList& items, QObject* missionItemParent) final; - void applyNewAltitude (double newAltitude) final; - double additionalTimeDelay (void) const final { return 0; } - ReadyForSaveState readyForSaveState (void) const final; - bool exitCoordinateSameAsEntry (void) const final { return false; } - void setDirty (bool dirty) final; - void setCoordinate (const QGeoCoordinate& coordinate) final { setLoiterCoordinate(coordinate); } - void setSequenceNumber (int sequenceNumber) final; void save (QJsonArray& missionItems) final; - double amslEntryAlt (void) const final; - double amslExitAlt (void) const final; - double minAMSLAltitude (void) const final { return amslExitAlt(); } - double maxAMSLAltitude (void) const final { return amslEntryAlt(); } static const QString name; static const char* jsonComplexItemTypeValue; static const char* settingsGroup; - static const char* loiterToLandDistanceName; - static const char* loiterAltitudeName; - static const char* loiterRadiusName; - static const char* landingHeadingName; - static const char* landingAltitudeName; - static const char* stopTakingPhotosName; - static const char* stopTakingVideoName; - -private slots: - void _updateLoiterCoodinateAltitudeFromFact (void); - void _updateLandingCoodinateAltitudeFromFact (void); - double _mathematicAngleToHeading (double angle); - double _headingToMathematicAngle (double heading); - void _setDirty (void); - void _signalLastSequenceNumberChanged (void); private: - void _calcGlideSlope(void) final; + static LandingComplexItem* _createItem (PlanMasterController* masterController, bool flyView, QObject* parent) { return new VTOLLandingComplexItem(masterController, flyView, parent); } + static bool _isValidLandItem(const MissionItem& missionItem); + + // Overrides from LandingComplexItem + const Fact* _finalApproachAltitude (void) const final { return &_finalApproachAltitudeFact; } + const Fact* _loiterRadius (void) const final { return &_loiterRadiusFact; } + const Fact* _loiterClockwise (void) const final { return &_loiterClockwiseFact; } + const Fact* _landingAltitude (void) const final { return &_landingAltitudeFact; } + const Fact* _landingDistance (void) const final { return &_landingDistanceFact; } + const Fact* _landingHeading (void) const final { return &_landingHeadingFact; } + const Fact* _useLoiterToAlt (void) const final { return &_useLoiterToAltFact; } + const Fact* _stopTakingPhotos (void) const final { return &_stopTakingPhotosFact; } + const Fact* _stopTakingVideo (void) const final { return &_stopTakingVideoFact; } + void _calcGlideSlope (void) final; + MissionItem* _createLandItem (int seqNum, bool altRel, double lat, double lon, double alt, QObject* parent) final; QMap _metaDataMap; Fact _landingDistanceFact; - Fact _loiterAltitudeFact; + Fact _finalApproachAltitudeFact; Fact _loiterRadiusFact; + Fact _loiterClockwiseFact; Fact _landingHeadingFact; Fact _landingAltitudeFact; + Fact _useLoiterToAltFact; Fact _stopTakingPhotosFact; Fact _stopTakingVideoFact; - static const char* _jsonLoiterCoordinateKey; - static const char* _jsonLoiterRadiusKey; - static const char* _jsonLoiterClockwiseKey; - static const char* _jsonLandingCoordinateKey; - static const char* _jsonAltitudesAreRelativeKey; - static const char* _jsonStopTakingPhotosKey; - static const char* _jsonStopTakingVideoKey; - friend VTOLLandingPatternTest; }; diff --git a/src/MissionManager/VTOLLandingPattern.FactMetaData.json b/src/MissionManager/VTOLLandingPattern.FactMetaData.json index f17902902..57fb430ae 100644 --- a/src/MissionManager/VTOLLandingPattern.FactMetaData.json +++ b/src/MissionManager/VTOLLandingPattern.FactMetaData.json @@ -5,7 +5,7 @@ [ { "name": "LandingDistance", - "shortDesc": "Distance between landing and loiter points.", + "shortDesc": "Distance between landing and approach points.", "type": "double", "units": "m", "min": 10, @@ -14,7 +14,7 @@ }, { "name": "LandingHeading", - "shortDesc": "Heading from loiter point to land point.", + "shortDesc": "Heading from approach to land point.", "type": "double", "units": "deg", "min": 0.0, @@ -23,8 +23,8 @@ "default": 270.0 }, { - "name": "LoiterAltitude", - "shortDesc": "Aircraft will proceed to the loiter point and loiter downwards until it reaches this approach altitude. Once altitude is reached the aircraft will fly to land point at current altitude.", + "name": "FinalApproachAltitude", + "shortDesc": "Altitude to begin landing approach from.", "type": "double", "units": "m", "decimalPlaces": 1, @@ -39,6 +39,12 @@ "units": "m", "default": 75.0 }, +{ + "name": "LoiterClockwise", + "shortDesc": "Loiter clockwise around the final approach point.", + "type": "bool", + "default": true +}, { "name": "LandingAltitude", "shortDesc": "Altitude for landing point on ground.", @@ -47,6 +53,12 @@ "decimalPlaces": 1, "default": 0.0 }, +{ + "name": "UseLoiterToAlt", + "shortDesc": "Use a loiter to altitude item for final appoach. Otherwise use a regular waypoint.", + "type": "bool", + "default": true +}, { "name": "StopTakingPhotos", "shortDesc": "Stop taking photos", diff --git a/src/MissionManager/VisualMissionItemTest.cc b/src/MissionManager/VisualMissionItemTest.cc index 9b5b749a9..f08cda32d 100644 --- a/src/MissionManager/VisualMissionItemTest.cc +++ b/src/MissionManager/VisualMissionItemTest.cc @@ -55,10 +55,10 @@ void VisualMissionItemTest::cleanup(void) _masterController->deleteLater(); } -void VisualMissionItemTest::_createSpy(SimpleMissionItem* simpleItem, MultiSignalSpy** visualSpy) +void VisualMissionItemTest::_createSpy(VisualMissionItem* visualItem, MultiSignalSpy** visualSpy) { *visualSpy = nullptr; MultiSignalSpy* spy = new MultiSignalSpy(); - QCOMPARE(spy->init(simpleItem, rgVisualItemSignals, cVisualItemSignals), true); + QCOMPARE(spy->init(visualItem, rgVisualItemSignals, cVisualItemSignals), true); *visualSpy = spy; } diff --git a/src/MissionManager/VisualMissionItemTest.h b/src/MissionManager/VisualMissionItemTest.h index 9f14f299d..4e89ead5d 100644 --- a/src/MissionManager/VisualMissionItemTest.h +++ b/src/MissionManager/VisualMissionItemTest.h @@ -30,7 +30,7 @@ public: void cleanup(void) override; protected: - void _createSpy(SimpleMissionItem* simpleItem, MultiSignalSpy** visualSpy); + void _createSpy(VisualMissionItem* visualItem, MultiSignalSpy** visualSpy); enum { altDifferenceChangedIndex = 0, diff --git a/src/PlanView/FWLandingPatternEditor.qml b/src/PlanView/FWLandingPatternEditor.qml index 6701183d4..0745fb690 100644 --- a/src/PlanView/FWLandingPatternEditor.qml +++ b/src/PlanView/FWLandingPatternEditor.qml @@ -52,20 +52,26 @@ Rectangle { visible: !editorColumnNeedLandingPoint.visible SectionHeader { - id: loiterPointSection + id: finalApproachSection anchors.left: parent.left anchors.right: parent.right - text: qsTr("Loiter point") + text: qsTr("Final approach") } Column { anchors.left: parent.left anchors.right: parent.right spacing: _margin - visible: loiterPointSection.checked + visible: finalApproachSection.checked Item { width: 1; height: _spacer } + FactCheckBox { + text: qsTr("Use loiter to altitude") + fact: missionItem.useLoiterToAlt + visible: missionItem.useLoiterToAlt.visible + } + GridLayout { anchors.left: parent.left anchors.right: parent.right @@ -75,24 +81,28 @@ Rectangle { AltitudeFactTextField { Layout.fillWidth: true - fact: missionItem.loiterAltitude + fact: missionItem.finalApproachAltitude altitudeMode: _altitudeMode } - QGCLabel { text: qsTr("Radius") } + QGCLabel { + text: qsTr("Radius") + visible: missionItem.useLoiterToAlt.rawValue + } FactTextField { Layout.fillWidth: true fact: missionItem.loiterRadius + visible: missionItem.useLoiterToAlt.rawValue } } Item { width: 1; height: _spacer } - QGCCheckBox { - text: qsTr("Loiter clockwise") - checked: missionItem.loiterClockwise - onClicked: missionItem.loiterClockwise = checked + FactCheckBox { + text: qsTr("Loiter clockwise") + fact: missionItem.loiterClockwise + visible: missionItem.useLoiterToAlt.rawValue } QGCButton { @@ -139,7 +149,7 @@ Rectangle { QGCRadioButton { id: specifyLandingDistance - text: qsTr("Landing Dist") + text: qsTr("Distance") checked: missionItem.valueSetIsDistance.rawValue onClicked: missionItem.valueSetIsDistance.rawValue = checked Layout.fillWidth: true diff --git a/src/PlanView/FWLandingPatternMapVisual.qml b/src/PlanView/FWLandingPatternMapVisual.qml index 7e89f04dd..6c38469c5 100644 --- a/src/PlanView/FWLandingPatternMapVisual.qml +++ b/src/PlanView/FWLandingPatternMapVisual.qml @@ -31,21 +31,27 @@ Item { readonly property real _landingWidthMeters: 15 readonly property real _landingLengthMeters: 100 - property var _missionItem: object + property var _missionItem: object property var _mouseArea - property var _dragAreas: [ ] + property var _dragAreas: [ ] property var _flightPath - property real _landingAreaBearing: _missionItem.landingCoordinate.azimuthTo(_missionItem.loiterTangentCoordinate) property var _loiterPointObject property var _landingPointObject property real _transitionAltitudeMeters property real _midSlopeAltitudeMeters - property real _landingAltitudeMeters: _missionItem.landingAltitude.rawValue - property real _loiterAltitudeMeters: _missionItem.loiterAltitude.rawValue + property real _landingAltitudeMeters: _missionItem.landingAltitude.rawValue + property real _finalApproachAltitudeMeters: _missionItem.finalApproachAltitude.rawValue + property bool _useLoiterToAlt: _missionItem.useLoiterToAlt.rawValue + property real _landingAreaBearing: _missionItem.landingCoordinate.azimuthTo(_useLoiterToAlt ? _missionItem.loiterTangentCoordinate : _missionItem.finalApproachCoordinate) function _calcGlideSlopeHeights() { - var adjacent = _missionItem.landingCoordinate.distanceTo(_missionItem.loiterTangentCoordinate) - var opposite = _loiterAltitudeMeters - _landingAltitudeMeters + var adjacent + if (_useLoiterToAlt) { + adjacent = _missionItem.landingCoordinate.distanceTo(_missionItem.loiterTangentCoordinate) + } else { + adjacent = _missionItem.landingCoordinate.distanceTo(_missionItem.finalApproachCoordinate) + } + var opposite = _finalApproachAltitudeMeters - _landingAltitudeMeters var angleRadians = Math.atan(opposite / adjacent) var transitionDistance = _landingLengthMeters / 2 var glideSlopeDistance = adjacent - transitionDistance @@ -60,7 +66,7 @@ Item { function showItemVisuals() { if (objMgr.rgDynamicObjects.length === 0) { - _loiterPointObject = objMgr.createObject(loiterPointComponent, map, true /* parentObjectIsMap */) + _loiterPointObject = objMgr.createObject(finalApproachComponent, map, true /* parentObjectIsMap */) _landingPointObject = objMgr.createObject(landingPointComponent, map, true /* parentObjectIsMap */) var rgComponents = [ flightPathComponent, loiterRadiusComponent, landingAreaComponent, landingAreaLabelComponent, @@ -93,13 +99,17 @@ Item { function showDragAreas() { if (_dragAreas.length === 0) { - _dragAreas.push(loiterDragAreaComponent.createObject(map)) + _dragAreas.push(finalApproachDragAreaComponent.createObject(map)) _dragAreas.push(landDragAreaComponent.createObject(map)) } } function _setFlightPath() { - _flightPath = [ _missionItem.loiterTangentCoordinate, _missionItem.landingCoordinate ] + if (_useLoiterToAlt) { + _flightPath = [ _missionItem.loiterTangentCoordinate, _missionItem.landingCoordinate ] + } else { + _flightPath = [ _missionItem.finalApproachCoordinate, _missionItem.landingCoordinate ] + } } QGCDynamicObjectManager { @@ -124,8 +134,9 @@ Item { hideItemVisuals() } - on_LandingAltitudeMetersChanged: _calcGlideSlopeHeights() - on_LoiterAltitudeMetersChanged: _calcGlideSlopeHeights() + on_LandingAltitudeMetersChanged: _calcGlideSlopeHeights() + on_FinalApproachAltitudeMetersChanged: _calcGlideSlopeHeights() + on_UseLoiterToAltChanged: { _calcGlideSlopeHeights(); _setFlightPath() } Connections { target: _missionItem @@ -170,6 +181,11 @@ Item { _calcGlideSlopeHeights() _setFlightPath() } + + onFinalApproachCoordinateChanged: { + _calcGlideSlopeHeights() + _setFlightPath() + } } // Mouse area to capture landing point coordindate @@ -194,14 +210,14 @@ Item { } } - // Control which is used to drag the loiter point + // Control which is used to drag the final approach point Component { - id: loiterDragAreaComponent + id: finalApproachDragAreaComponent MissionItemIndicatorDrag { mapControl: _root.map itemIndicator: _loiterPointObject - itemCoordinate: _missionItem.loiterCoordinate + itemCoordinate: _missionItem.finalApproachCoordinate visible: _root.interactive property bool _preventReentrancy: false @@ -211,8 +227,8 @@ Item { if (Drag.active) { _preventReentrancy = true var angle = _missionItem.landingCoordinate.azimuthTo(itemCoordinate) - var distance = _missionItem.landingCoordinate.distanceTo(_missionItem.loiterCoordinate) - _missionItem.loiterCoordinate = _missionItem.landingCoordinate.atDistanceAndAzimuth(distance, angle) + var distance = _missionItem.landingCoordinate.distanceTo(_missionItem.finalApproachCoordinate) + _missionItem.finalApproachCoordinate = _missionItem.landingCoordinate.atDistanceAndAzimuth(distance, angle) _preventReentrancy = false } } @@ -246,20 +262,20 @@ Item { } } - // Loiter point + // Final approach point Component { - id: loiterPointComponent + id: finalApproachComponent MapQuickItem { anchorPoint.x: sourceItem.anchorPointX anchorPoint.y: sourceItem.anchorPointY z: QGroundControl.zOrderMapItems - coordinate: _missionItem.loiterCoordinate + coordinate: _missionItem.finalApproachCoordinate sourceItem: MissionItemIndexLabel { index: _missionItem.sequenceNumber - label: qsTr("Loiter") + label: _useLoiterToAlt ? qsTr("Loiter") : qsTr("Approach") checked: _missionItem.isCurrentItem onClicked: _root.clicked(_missionItem.sequenceNumber) @@ -292,11 +308,12 @@ Item { MapCircle { z: QGroundControl.zOrderMapItems - center: _missionItem.loiterCoordinate + center: _missionItem.finalApproachCoordinate radius: _missionItem.loiterRadius.rawValue border.width: 2 border.color: "green" color: "transparent" + visible: _useLoiterToAlt } } @@ -347,6 +364,7 @@ Item { z: QGroundControl.zOrderMapItems visible: _missionItem.isCurrentItem + sourceItem: QGCLabel { id: glideSlopeLabel text: qsTr("Glide Slope") @@ -383,6 +401,7 @@ Item { target: _missionItem onLandingCoordinateChanged: recalc() onLoiterTangentCoordinateChanged: recalc() + onFinalApproachCoordinateChanged: recalc() } } } @@ -415,6 +434,7 @@ Item { target: _missionItem onLandingCoordinateChanged: recalc() onLoiterTangentCoordinateChanged: recalc() + onFinalApproachCoordinateChanged: recalc() } } } @@ -437,7 +457,7 @@ Item { path = [ ] addCoordinate(_missionItem.landingCoordinate.atDistanceAndAzimuth(hypotenuse, _landingAreaBearing - angleDegrees)) addCoordinate(_missionItem.landingCoordinate.atDistanceAndAzimuth(hypotenuse, _landingAreaBearing + angleDegrees)) - addCoordinate(_missionItem.loiterTangentCoordinate) + addCoordinate(_useLoiterToAlt ? _missionItem.loiterTangentCoordinate : _missionItem.finalApproachCoordinate) } Component.onCompleted: recalc() @@ -446,6 +466,12 @@ Item { target: _missionItem onLandingCoordinateChanged: recalc() onLoiterTangentCoordinateChanged: recalc() + onFinalApproachCoordinateChanged: recalc() + } + + Connections { + target: _missionItem.useLoiterToAlt + onRawValueChanged: recalc() } } } @@ -477,8 +503,8 @@ Item { target: _missionItem onLandingCoordinateChanged: recalc() onLoiterTangentCoordinateChanged: recalc() + onFinalApproachCoordinateChanged: recalc() } - } } @@ -499,7 +525,7 @@ Item { function recalc() { var transitionCoordinate = _missionItem.landingCoordinate.atDistanceAndAzimuth(_landingLengthMeters / 2, _landingAreaBearing) - var halfDistance = transitionCoordinate.distanceTo(_missionItem.loiterTangentCoordinate) / 2 + var halfDistance = transitionCoordinate.distanceTo(_useLoiterToAlt ? _missionItem.loiterTangentCoordinate : _missionItem.finalApproachCoordinate) / 2 var centeredCoordinate = transitionCoordinate.atDistanceAndAzimuth(halfDistance, _landingAreaBearing) var angleIncrement = _landingAreaBearing > 180 ? -90 : 90 coordinate = centeredCoordinate.atDistanceAndAzimuth(_landingWidthMeters / 2, _landingAreaBearing + angleIncrement) @@ -511,8 +537,13 @@ Item { target: _missionItem onLandingCoordinateChanged: recalc() onLoiterTangentCoordinateChanged: recalc() + onFinalApproachCoordinateChanged: recalc() } + Connections { + target: _missionItem.useLoiterToAlt + onRawValueChanged: recalc() + } } } @@ -524,11 +555,11 @@ Item { anchorPoint.y: 0 z: QGroundControl.zOrderMapItems visible: _missionItem.isCurrentItem - coordinate: _missionItem.loiterTangentCoordinate + coordinate: _useLoiterToAlt ? _missionItem.loiterTangentCoordinate : _missionItem.finalApproachCoordinate sourceItem: HeightIndicator { map: _root.map - heightText: _missionItem.loiterAltitude.value.toFixed(1) + QGroundControl.unitsConversion.appSettingsHorizontalDistanceUnitsString + heightText: _missionItem.finalApproachAltitude.value.toFixed(1) + QGroundControl.unitsConversion.appSettingsHorizontalDistanceUnitsString } } } diff --git a/src/PlanView/VTOLLandingPatternEditor.qml b/src/PlanView/VTOLLandingPatternEditor.qml index 0d0927f4b..264278496 100644 --- a/src/PlanView/VTOLLandingPatternEditor.qml +++ b/src/PlanView/VTOLLandingPatternEditor.qml @@ -51,29 +51,27 @@ Rectangle { spacing: _margin visible: !editorColumnNeedLandingPoint.visible - QGCLabel { - anchors.left: parent.left - anchors.right: parent.right - wrapMode: Text.WordWrap - font.pointSize: ScreenTools.smallFontPointSize - text: qsTr("Loiter down to specified altitude. Fly to land point while transitioning. Hover straight down to land.") - } - SectionHeader { - id: loiterPointSection + id: finalApproachSection anchors.left: parent.left anchors.right: parent.right - text: qsTr("Loiter point") + text: qsTr("Final approach") } Column { anchors.left: parent.left anchors.right: parent.right spacing: _margin - visible: loiterPointSection.checked + visible: finalApproachSection.checked Item { width: 1; height: _spacer } + FactCheckBox { + text: qsTr("Use loiter to altitude") + fact: missionItem.useLoiterToAlt + visible: missionItem.useLoiterToAlt.visible + } + GridLayout { anchors.left: parent.left anchors.right: parent.right @@ -83,24 +81,28 @@ Rectangle { AltitudeFactTextField { Layout.fillWidth: true - fact: missionItem.loiterAltitude + fact: missionItem.finalApproachAltitude altitudeMode: _altitudeMode } - QGCLabel { text: qsTr("Radius") } + QGCLabel { + text: qsTr("Radius") + visible: missionItem.useLoiterToAlt.rawValue + } FactTextField { Layout.fillWidth: true fact: missionItem.loiterRadius + visible: missionItem.useLoiterToAlt.rawValue } } Item { width: 1; height: _spacer } - QGCCheckBox { - text: qsTr("Loiter clockwise") - checked: missionItem.loiterClockwise - onClicked: missionItem.loiterClockwise = checked + FactCheckBox { + text: qsTr("Loiter clockwise") + fact: missionItem.loiterClockwise + visible: missionItem.useLoiterToAlt.rawValue } QGCButton { @@ -222,7 +224,7 @@ Rectangle { wrapMode: Text.WordWrap color: qgcPal.warningText font.pointSize: ScreenTools.smallFontPointSize - text: qsTr("* Avoid tailwind from loiter to land.") + text: qsTr("* Avoid tailwind on approach to land.") } QGCLabel { diff --git a/src/PlanView/VTOLLandingPatternMapVisual.qml b/src/PlanView/VTOLLandingPatternMapVisual.qml index 424511efe..109bf0e76 100644 --- a/src/PlanView/VTOLLandingPatternMapVisual.qml +++ b/src/PlanView/VTOLLandingPatternMapVisual.qml @@ -33,9 +33,10 @@ Item { property var _mouseArea property var _dragAreas: [ ] property var _flightPath - property real _landingAreaBearing: _missionItem.landingCoordinate.azimuthTo(_missionItem.loiterTangentCoordinate) property var _loiterPointObject property var _landingPointObject + property bool _useLoiterToAlt: _missionItem.useLoiterToAlt.rawValue + property real _landingAreaBearing: _missionItem.landingCoordinate.azimuthTo(_useLoiterToAlt ? _missionItem.loiterTangentCoordinate : _missionItem.finalApproachCoordinate) function hideItemVisuals() { objMgr.destroyObjects() @@ -43,7 +44,7 @@ Item { function showItemVisuals() { if (objMgr.rgDynamicObjects.length === 0) { - _loiterPointObject = objMgr.createObject(loiterPointComponent, map, true /* parentObjectIsMap */) + _loiterPointObject = objMgr.createObject(finalApproachPointComponent, map, true /* parentObjectIsMap */) _landingPointObject = objMgr.createObject(landingPointComponent, map, true /* parentObjectIsMap */) var rgComponents = [ flightPathComponent, loiterRadiusComponent ] @@ -80,7 +81,11 @@ Item { } function _setFlightPath() { - _flightPath = [ _missionItem.loiterTangentCoordinate, _missionItem.landingCoordinate ] + if (_useLoiterToAlt) { + _flightPath = [ _missionItem.loiterTangentCoordinate, _missionItem.landingCoordinate ] + } else { + _flightPath = [ _missionItem.finalApproachCoordinate, _missionItem.landingCoordinate ] + } } QGCDynamicObjectManager { @@ -105,6 +110,8 @@ Item { hideItemVisuals() } + on_UseLoiterToAltChanged: _setFlightPath() + Connections { target: _missionItem @@ -141,6 +148,7 @@ Item { onLandingCoordinateChanged: _setFlightPath() onLoiterTangentCoordinateChanged: _setFlightPath() + onFinalApproachCoordinateChanged: _setFlightPath() } // Mouse area to capture landing point coordindate @@ -164,16 +172,28 @@ Item { } } - // Control which is used to drag the loiter point + // Control which is used to drag the final approach point Component { id: loiterDragAreaComponent MissionItemIndicatorDrag { mapControl: _root.map itemIndicator: _loiterPointObject - itemCoordinate: _missionItem.loiterCoordinate - - onItemCoordinateChanged: _missionItem.loiterCoordinate = itemCoordinate + itemCoordinate: _missionItem.finalApproachCoordinate + + property bool _preventReentrancy: false + + onItemCoordinateChanged: { + if (!_preventReentrancy) { + if (Drag.active) { + _preventReentrancy = true + var angle = _missionItem.landingCoordinate.azimuthTo(itemCoordinate) + var distance = _missionItem.landingCoordinate.distanceTo(_missionItem.finalApproachCoordinate) + _missionItem.finalApproachCoordinate = _missionItem.landingCoordinate.atDistanceAndAzimuth(distance, angle) + _preventReentrancy = false + } + } + } } } @@ -202,20 +222,20 @@ Item { } } - // Loiter point + // Final approach point Component { - id: loiterPointComponent + id: finalApproachPointComponent MapQuickItem { anchorPoint.x: sourceItem.anchorPointX anchorPoint.y: sourceItem.anchorPointY z: QGroundControl.zOrderMapItems - coordinate: _missionItem.loiterCoordinate + coordinate: _missionItem.finalApproachCoordinate sourceItem: MissionItemIndexLabel { index: _missionItem.sequenceNumber - label: qsTr("Loiter") + label: _useLoiterToAlt ? qsTr("Loiter") : qsTr("Approach") checked: _missionItem.isCurrentItem onClicked: _root.clicked(_missionItem.sequenceNumber) @@ -249,11 +269,12 @@ Item { MapCircle { z: QGroundControl.zOrderMapItems - center: _missionItem.loiterCoordinate + center: _missionItem.finalApproachCoordinate radius: _missionItem.loiterRadius.rawValue border.width: 2 border.color: "green" color: "transparent" + visible: _useLoiterToAlt } } } diff --git a/src/qgcunittest/MultiSignalSpy.cc b/src/qgcunittest/MultiSignalSpy.cc index bb3b26133..d091772aa 100644 --- a/src/qgcunittest/MultiSignalSpy.cc +++ b/src/qgcunittest/MultiSignalSpy.cc @@ -59,7 +59,7 @@ bool MultiSignalSpy::init(QObject* signalEmitter, ///< [in] object whi for (size_t i=0; i<_cSignals; i++) { _rgSpys[i] = new QSignalSpy(_signalEmitter, _rgSignals[i]); if (!_rgSpys[i]->isValid()) { - qDebug() << "Invalid signal"; + qDebug() << "Invalid signal: index" << i; return false; } } diff --git a/src/qgcunittest/UnitTest.cc b/src/qgcunittest/UnitTest.cc index e7a1f9d4d..417941cbe 100644 --- a/src/qgcunittest/UnitTest.cc +++ b/src/qgcunittest/UnitTest.cc @@ -525,3 +525,8 @@ bool UnitTest::fuzzyCompareLatLon(const QGeoCoordinate& coord1, const QGeoCoordi { return coord1.distanceTo(coord2) < 1.0; } + +QGeoCoordinate UnitTest::changeCoordinateValue(const QGeoCoordinate& coordinate) +{ + return coordinate.atDistanceAndAzimuth(1, 0); +} diff --git a/src/qgcunittest/UnitTest.h b/src/qgcunittest/UnitTest.h index ef46ed331..6833d2972 100644 --- a/src/qgcunittest/UnitTest.h +++ b/src/qgcunittest/UnitTest.h @@ -100,6 +100,8 @@ public: /// @param increment 0 use standard increment, other increment by specified amount if double value void changeFactValue(Fact* fact, double increment = 0); + QGeoCoordinate changeCoordinateValue(const QGeoCoordinate& coordinate); + /// Returns true is the position of the two coordinates is less then a meter from each other. /// Does not check altitude. static bool fuzzyCompareLatLon(const QGeoCoordinate& coord1, const QGeoCoordinate& coord2); diff --git a/src/qgcunittest/UnitTestList.cc b/src/qgcunittest/UnitTestList.cc index 308630892..e8077b659 100644 --- a/src/qgcunittest/UnitTestList.cc +++ b/src/qgcunittest/UnitTestList.cc @@ -49,6 +49,7 @@ #include "InitialConnectTest.h" #include "FTPManagerTest.h" #include "MissionCommandTreeEditorTest.h" +#include "LandingComplexItemTest.h" UT_REGISTER_TEST(FactSystemTestGeneric) UT_REGISTER_TEST(FactSystemTestPX4) @@ -84,6 +85,7 @@ UT_REGISTER_TEST(TransectStyleComplexItemTest) UT_REGISTER_TEST(QGCMapPolylineTest) UT_REGISTER_TEST(CameraCalcTest) UT_REGISTER_TEST(FWLandingPatternTest) +UT_REGISTER_TEST(LandingComplexItemTest) UT_REGISTER_TEST_STANDALONE(MissionCommandTreeEditorTest) // List of unit test which are currently disabled. -- 2.22.0