From 80a651e2e760fc305887f80538833d23084c6f6a Mon Sep 17 00:00:00 2001 From: DonLakeFlyer Date: Sun, 12 Jul 2020 10:51:08 -0700 Subject: [PATCH] Fix global alt mode support --- src/MissionManager/MissionController.cc | 5 +++- src/MissionManager/MissionControllerTest.cc | 36 +++++++++++++++++++++++++++++ src/MissionManager/MissionControllerTest.h | 9 ++++---- src/MissionManager/SimpleMissionItem.cc | 8 ++++--- 4 files changed, 50 insertions(+), 8 deletions(-) diff --git a/src/MissionManager/MissionController.cc b/src/MissionManager/MissionController.cc index b94aece..278655d 100644 --- a/src/MissionManager/MissionController.cc +++ b/src/MissionManager/MissionController.cc @@ -327,7 +327,10 @@ VisualMissionItem* MissionController::_insertSimpleMissionItemWorker(QGeoCoordin if (_findPreviousAltitude(visualItemIndex, &prevAltitude, &prevAltitudeMode)) { newItem->altitude()->setRawValue(prevAltitude); - newItem->setAltitudeMode(static_cast(prevAltitudeMode)); + if (globalAltitudeMode() == QGroundControlQmlGlobal::AltitudeModeNone) { + // We are in mixed altitude modes, so copy from previous. Otherwise alt mode will be set from global setting. + newItem->setAltitudeMode(static_cast(prevAltitudeMode)); + } } } } diff --git a/src/MissionManager/MissionControllerTest.cc b/src/MissionManager/MissionControllerTest.cc index 857017f..b0de3e1 100644 --- a/src/MissionManager/MissionControllerTest.cc +++ b/src/MissionManager/MissionControllerTest.cc @@ -203,3 +203,39 @@ void MissionControllerTest::_testLoadJsonSectionAvailable(void) } } + +void MissionControllerTest::_testGlobalAltMode(void) +{ + _initForFirmwareType(MAV_AUTOPILOT_PX4); + + struct _globalAltMode_s { + QGroundControlQmlGlobal::AltitudeMode altMode; + MAV_FRAME expectedMavFrame; + } altModeTestCases[] = { + { QGroundControlQmlGlobal::AltitudeModeRelative, MAV_FRAME_GLOBAL_RELATIVE_ALT }, + { QGroundControlQmlGlobal::AltitudeModeAbsolute, MAV_FRAME_GLOBAL }, + { QGroundControlQmlGlobal::AltitudeModeAboveTerrain, MAV_FRAME_GLOBAL }, + { QGroundControlQmlGlobal::AltitudeModeTerrainFrame, MAV_FRAME_GLOBAL_TERRAIN_ALT }, + }; + + for (const _globalAltMode_s& testCase: altModeTestCases) { + _missionController->removeAll(); + _missionController->setGlobalAltitudeMode(testCase.altMode); + + _missionController->insertTakeoffItem(QGeoCoordinate(0, 0), 1); + _missionController->insertSimpleMissionItem(QGeoCoordinate(0, 0), 2); + _missionController->insertSimpleMissionItem(QGeoCoordinate(0, 0), 3); + _missionController->insertSimpleMissionItem(QGeoCoordinate(0, 0), 4); + + SimpleMissionItem* si = qobject_cast(_missionController->visualItems()->value(1)); + QCOMPARE(si->altitudeMode(), QGroundControlQmlGlobal::AltitudeModeRelative); + QCOMPARE(si->missionItem().frame(), MAV_FRAME_GLOBAL_RELATIVE_ALT); + + for (int i=2; i<_missionController->visualItems()->count(); i++) { + qDebug() << i; + SimpleMissionItem* si = qobject_cast(_missionController->visualItems()->value(i)); + QCOMPARE(si->altitudeMode(), testCase.altMode); + QCOMPARE(si->missionItem().frame(), testCase.expectedMavFrame); + } + } +} diff --git a/src/MissionManager/MissionControllerTest.h b/src/MissionManager/MissionControllerTest.h index c01764b..902f775 100644 --- a/src/MissionManager/MissionControllerTest.h +++ b/src/MissionManager/MissionControllerTest.h @@ -32,10 +32,11 @@ public: private slots: void cleanup(void); - void _testGimbalRecalc(void); - void _testLoadJsonSectionAvailable(void); - void _testEmptyVehicleAPM(void); - void _testEmptyVehiclePX4(void); + void _testGimbalRecalc (void); + void _testLoadJsonSectionAvailable (void); + void _testEmptyVehicleAPM (void); + void _testEmptyVehiclePX4 (void); + void _testGlobalAltMode (void); private: #if 0 diff --git a/src/MissionManager/SimpleMissionItem.cc b/src/MissionManager/SimpleMissionItem.cc index 9221091..3034dfd 100644 --- a/src/MissionManager/SimpleMissionItem.cc +++ b/src/MissionManager/SimpleMissionItem.cc @@ -726,14 +726,17 @@ void SimpleMissionItem::_setDefaultsForCommand(void) _altitudeMode = QGroundControlQmlGlobal::AltitudeModeRelative; emit altitudeModeChanged(); _amslAltAboveTerrainFact.setRawValue(qQNaN()); - if (specifiesAltitude() || isStandaloneCoordinate()) { + if (specifiesAltitude()) { double defaultAlt = qgcApp()->toolbox()->settingsManager()->appSettings()->defaultMissionItemAltitude()->rawValue().toDouble(); _altitudeFact.setRawValue(defaultAlt); _missionItem._param7Fact.setRawValue(defaultAlt); - setAltitudeMode(_missionController->globalAltitudeModeDefault()); + // Note that setAltitudeMode will also set MAV_FRAME correctly through signalling + // Takeoff items always use relative alt since that is the highest quality data to base altitude from + setAltitudeMode(isTakeoffItem() ? QGroundControlQmlGlobal::AltitudeModeRelative : _missionController->globalAltitudeModeDefault()); } else { _altitudeFact.setRawValue(0); _missionItem._param7Fact.setRawValue(0); + _missionItem.setFrame(MAV_FRAME_MISSION); } MAV_CMD command = static_cast(this->command()); @@ -759,7 +762,6 @@ void SimpleMissionItem::_setDefaultsForCommand(void) } _missionItem.setAutoContinue(true); - _missionItem.setFrame(specifiesAltitude() ? MAV_FRAME_GLOBAL_RELATIVE_ALT : MAV_FRAME_MISSION); setRawEdit(false); }