diff --git a/src/FirmwarePlugin/APM/ArduCopterFirmwarePlugin.cc b/src/FirmwarePlugin/APM/ArduCopterFirmwarePlugin.cc index ca33b2e5dde4c3d553b28252198abc91a048a4d9..57094a7bd5c01ca1a53623e9d1f7a688067be82a 100644 --- a/src/FirmwarePlugin/APM/ArduCopterFirmwarePlugin.cc +++ b/src/FirmwarePlugin/APM/ArduCopterFirmwarePlugin.cc @@ -237,11 +237,11 @@ QString ArduCopterFirmwarePlugin::geoFenceRadiusParam(Vehicle* vehicle) bool ArduCopterFirmwarePlugin::vehicleYawsToNextWaypointInMission(const Vehicle* vehicle) const { - if (vehicle->parameterManager()->parameterExists(FactSystem::defaultComponentId, QStringLiteral("WP_YAW_BEHAVIOR"))) { + if (!vehicle->isOfflineEditingVehicle() && vehicle->parameterManager()->parameterExists(FactSystem::defaultComponentId, QStringLiteral("WP_YAW_BEHAVIOR"))) { Fact* yawMode = vehicle->parameterManager()->getParameter(FactSystem::defaultComponentId, QStringLiteral("WP_YAW_BEHAVIOR")); return yawMode && yawMode->rawValue().toInt() != 0; } - return false; + return true; } void ArduCopterFirmwarePlugin::missionFlightSpeedInfo(Vehicle* vehicle, double& hoverSpeed, double& cruiseSpeed) diff --git a/src/FirmwarePlugin/PX4/PX4FirmwarePlugin.cc b/src/FirmwarePlugin/PX4/PX4FirmwarePlugin.cc index 669fc01203c8f7cd1f8df64fe3be8185b67db1ba..ce622359f3d2bf86e039efd72d9370a93700c9da 100644 --- a/src/FirmwarePlugin/PX4/PX4FirmwarePlugin.cc +++ b/src/FirmwarePlugin/PX4/PX4FirmwarePlugin.cc @@ -518,11 +518,11 @@ void PX4FirmwarePlugin::_handleAutopilotVersion(Vehicle* vehicle, mavlink_messag bool PX4FirmwarePlugin::vehicleYawsToNextWaypointInMission(const Vehicle* vehicle) const { - if (vehicle->parameterManager()->parameterExists(FactSystem::defaultComponentId, QStringLiteral("MIS_YAWMODE"))) { + if (!vehicle->isOfflineEditingVehicle() && vehicle->parameterManager()->parameterExists(FactSystem::defaultComponentId, QStringLiteral("MIS_YAWMODE"))) { Fact* yawMode = vehicle->parameterManager()->getParameter(FactSystem::defaultComponentId, QStringLiteral("MIS_YAWMODE")); return yawMode && yawMode->rawValue().toInt() == 1; } - return false; + return true; } void PX4FirmwarePlugin::missionFlightSpeedInfo(Vehicle* vehicle, double& hoverSpeed, double& cruiseSpeed) diff --git a/src/FlightMap/MapItems/MissionItemIndicator.qml b/src/FlightMap/MapItems/MissionItemIndicator.qml index 440f2a077c70323e2f057a64325f9a10dd9bd787..d4f3f70d47d4885821e206031427fdea8893ead7 100644 --- a/src/FlightMap/MapItems/MissionItemIndicator.qml +++ b/src/FlightMap/MapItems/MissionItemIndicator.qml @@ -35,7 +35,7 @@ MapQuickItem { index: missionItem ? missionItem.sequenceNumber : 0 gimbalYaw: missionItem.missionGimbalYaw vehicleYaw: missionItem.missionVehicleYaw - showGimbalYaw: missionItem.showMissionGimbalYaw + showGimbalYaw: !isNaN(missionItem.missionGimbalYaw) onClicked: _item.clicked() property bool _isCurrentItem: missionItem ? missionItem.isCurrentItem : false diff --git a/src/MissionManager/MissionControllerTest.cc b/src/MissionManager/MissionControllerTest.cc index 0955a9d08feb346bdcfc3fdd60726e59b72c56c4..d417708c1979df14ed3fdfd2f100621c4b2ce4b0 100644 --- a/src/MissionManager/MissionControllerTest.cc +++ b/src/MissionManager/MissionControllerTest.cc @@ -200,3 +200,27 @@ void MissionControllerTest::_setupVisualItemSignals(VisualMissionItem* visualIte Q_CHECK_PTR(_multiSpyMissionItem); QCOMPARE(_multiSpyMissionItem->init(visualItem, _rgVisualItemSignals, _cVisualItemSignals), true); } + +void MissionControllerTest::_testGimbalRecalc(void) +{ + _initForFirmwareType(MAV_AUTOPILOT_PX4); + _missionController->insertSimpleMissionItem(QGeoCoordinate(0, 0), 1); + _missionController->insertSimpleMissionItem(QGeoCoordinate(0, 0), 2); + _missionController->insertSimpleMissionItem(QGeoCoordinate(0, 0), 3); + _missionController->insertSimpleMissionItem(QGeoCoordinate(0, 0), 4); + + // No specific gimbal yaw set yet + for (int i=1; i<_missionController->visualItems()->count(); i++) { + VisualMissionItem* visualItem = _missionController->visualItems()->value(i); + QVERIFY(qIsNaN(visualItem->missionGimbalYaw())); + } + + // Specify gimbal yaw on settings item should generate yaw on all items + MissionSettingsItem* settingsItem = _missionController->visualItems()->value(0); + settingsItem->cameraSection()->setSpecifyGimbal(true); + settingsItem->cameraSection()->gimbalYaw()->setRawValue(0.0); + for (int i=1; i<_missionController->visualItems()->count(); i++) { + VisualMissionItem* visualItem = _missionController->visualItems()->value(i); + QCOMPARE(visualItem->missionGimbalYaw(), 0.0); + } +} diff --git a/src/MissionManager/MissionControllerTest.h b/src/MissionManager/MissionControllerTest.h index 9e469a90a5e14a05d2706f1f18065dc5fb350700..2cc2559ad118c2504a00a385ea7436de29bd8c85 100644 --- a/src/MissionManager/MissionControllerTest.h +++ b/src/MissionManager/MissionControllerTest.h @@ -31,6 +31,9 @@ public: private slots: void cleanup(void); + void _testGimbalRecalc(void); + +private: void _testEmptyVehicleAPM(void); void _testEmptyVehiclePX4(void); void _testAddWayppointAPM(void); diff --git a/src/MissionManager/MissionSettingsItem.h b/src/MissionManager/MissionSettingsItem.h index 39e9e16f54b7f86e959b8cdb4161ebf8eea10865..8679f4925d9dc651aa1815243f6ace97584dcea3 100644 --- a/src/MissionManager/MissionSettingsItem.h +++ b/src/MissionManager/MissionSettingsItem.h @@ -33,8 +33,8 @@ public: Fact* plannedHomePositionAltitude (void) { return &_plannedHomePositionAltitudeFact; } - QObject* cameraSection(void) { return &_cameraSection; } - QObject* speedSection(void) { return &_speedSection; } + CameraSection* cameraSection(void) { return &_cameraSection; } + SpeedSection* speedSection(void) { return &_speedSection; } /// Scans the loaded items for settings items bool scanForMissionSettings(QmlObjectListModel* visualItems, int scanIndex); diff --git a/src/MissionManager/SimpleMissionItem.cc b/src/MissionManager/SimpleMissionItem.cc index c90b06f4bc047353ba57c6f7f64e24e005d5f9fe..bc982faa94277826f66f9f957fff06fa028b6b7e 100644 --- a/src/MissionManager/SimpleMissionItem.cc +++ b/src/MissionManager/SimpleMissionItem.cc @@ -80,9 +80,9 @@ SimpleMissionItem::SimpleMissionItem(Vehicle* vehicle, QObject* parent) connect(&_missionItem, &MissionItem::specifiedFlightSpeedChanged, this, &SimpleMissionItem::specifiedFlightSpeedChanged); - connect(this, &SimpleMissionItem::sequenceNumberChanged, this, &SimpleMissionItem::lastSequenceNumberChanged); - connect(this, &SimpleMissionItem::cameraSectionChanged, this, &SimpleMissionItem::_setDirtyFromSignal); - connect(this, &SimpleMissionItem::cameraSectionChanged, this, &SimpleMissionItem::_updateLastSequenceNumber); + connect(this, &SimpleMissionItem::sequenceNumberChanged, this, &SimpleMissionItem::lastSequenceNumberChanged); + connect(this, &SimpleMissionItem::cameraSectionChanged, this, &SimpleMissionItem::_setDirtyFromSignal); + connect(this, &SimpleMissionItem::cameraSectionChanged, this, &SimpleMissionItem::_updateLastSequenceNumber); } SimpleMissionItem::SimpleMissionItem(Vehicle* vehicle, const MissionItem& missionItem, QObject* parent) @@ -736,8 +736,10 @@ void SimpleMissionItem::_updateOptionalSections(void) connect(_cameraSection, &CameraSection::specifyGimbalChanged, this, &SimpleMissionItem::specifiedGimbalYawChanged); connect(_cameraSection, &CameraSection::specifiedGimbalYawChanged, this, &SimpleMissionItem::specifiedGimbalYawChanged); - connect(_speedSection, &CameraSection::dirtyChanged, this, &SimpleMissionItem::_sectionDirtyChanged); - connect(_speedSection, &CameraSection::itemCountChanged, this, &SimpleMissionItem::_updateLastSequenceNumber); + connect(_speedSection, &SpeedSection::dirtyChanged, this, &SimpleMissionItem::_sectionDirtyChanged); + connect(_speedSection, &SpeedSection::itemCountChanged, this, &SimpleMissionItem::_updateLastSequenceNumber); + connect(_speedSection, &SpeedSection::specifyFlightSpeedChanged, this, &SimpleMissionItem::specifiedFlightSpeedChanged); + connect(_speedSection->flightSpeed(), &Fact::rawValueChanged, this, &SimpleMissionItem::specifiedFlightSpeedChanged); emit cameraSectionChanged(_cameraSection); emit speedSectionChanged(_speedSection); diff --git a/src/MissionManager/SimpleMissionItemTest.cc b/src/MissionManager/SimpleMissionItemTest.cc index e36795caae771992c8e681a9081d09461a4f741b..6d2f9bbfd0c911ccc457825c1ffd730e96f3ed59 100644 --- a/src/MissionManager/SimpleMissionItemTest.cc +++ b/src/MissionManager/SimpleMissionItemTest.cc @@ -107,8 +107,7 @@ void SimpleMissionItemTest::init(void) _spySimpleItem = new MultiSignalSpy(); QCOMPARE(_spySimpleItem->init(_simpleItem, rgSimpleItemSignals, cSimpleItemSignals), true); - _spyVisualItem = new MultiSignalSpy(); - QCOMPARE(_spyVisualItem->init(_simpleItem, rgVisualItemSignals, cVisualItemSignals), true); + VisualMissionItemTest::_createSpy(_simpleItem, &_spyVisualItem); } void SimpleMissionItemTest::cleanup(void) @@ -257,7 +256,33 @@ void SimpleMissionItemTest::_testSignals(void) QVERIFY(_spyVisualItem->checkSignalsByMask(commandNameChangedMask | dirtyChangedMask | coordinateChangedMask)); } -void SimpleMissionItemTest::_testCameraSectionSignals(void) +void SimpleMissionItemTest::_testCameraSection(void) { + // No gimbal yaw to start with + QVERIFY(qIsNaN(_simpleItem->specifiedGimbalYaw())); + QVERIFY(qIsNaN(_simpleItem->missionGimbalYaw())); + QCOMPARE(_simpleItem->dirty(), false); + + double gimbalYaw = 10.1234; + _simpleItem->cameraSection()->setSpecifyGimbal(true); + _simpleItem->cameraSection()->gimbalYaw()->setRawValue(gimbalYaw); + QCOMPARE(_simpleItem->specifiedGimbalYaw(), gimbalYaw); + QVERIFY(qIsNaN(_simpleItem->missionGimbalYaw())); + QCOMPARE(_spyVisualItem->checkSignalsByMask(specifiedGimbalYawChangedMask), true); + QCOMPARE(_simpleItem->dirty(), true); +} + +void SimpleMissionItemTest::_testSpeedSection(void) +{ + // No flight speed + QVERIFY(qIsNaN(_simpleItem->specifiedFlightSpeed())); + QCOMPARE(_simpleItem->dirty(), false); + + double flightSpeed = 10.1234; + _simpleItem->speedSection()->setSpecifyFlightSpeed(true); + _simpleItem->speedSection()->flightSpeed()->setRawValue(flightSpeed); + QCOMPARE(_simpleItem->specifiedFlightSpeed(), flightSpeed); + QCOMPARE(_spyVisualItem->checkSignalsByMask(specifiedFlightSpeedChangedMask), true); + QCOMPARE(_simpleItem->dirty(), true); } diff --git a/src/MissionManager/SimpleMissionItemTest.h b/src/MissionManager/SimpleMissionItemTest.h index e28178a6cfcea9ef8fb4c38b2ff49235aa249263..1572f2bece3c7af85b3ad199b877b12ad1bca0b6 100644 --- a/src/MissionManager/SimpleMissionItemTest.h +++ b/src/MissionManager/SimpleMissionItemTest.h @@ -25,10 +25,11 @@ public: private slots: void _testSignals(void); - void _testCameraSectionSignals(void); void _testEditorFacts(void); void _testDefaultValues(void); - + void _testCameraSection(void); + void _testSpeedSection(void); + private: enum { commandChangedIndex = 0, diff --git a/src/MissionManager/VisualMissionItem.h b/src/MissionManager/VisualMissionItem.h index b2cdba88b4c6e5aa83b983476d6c7eb8995f2a6c..cb63d4a0c4aaf46700c5752afe5aad1e31d584fb 100644 --- a/src/MissionManager/VisualMissionItem.h +++ b/src/MissionManager/VisualMissionItem.h @@ -67,7 +67,6 @@ public: Q_PROPERTY(double specifiedGimbalYaw READ specifiedGimbalYaw NOTIFY specifiedGimbalYawChanged) ///< NaN if this item goes not specify gimbal yaw Q_PROPERTY(double missionGimbalYaw READ missionGimbalYaw NOTIFY missionGimbalYawChanged) ///< Current gimbal yaw state at this point in mission Q_PROPERTY(double missionVehicleYaw READ missionVehicleYaw NOTIFY missionVehicleYawChanged) ///< Expected vehicle yaw at this point in mission - Q_PROPERTY(double showMissionGimbalYaw READ showMissionGimbalYaw NOTIFY missionGimbalYawChanged) ///< true: Show gimbal yaw position on map indicators // The following properties are calculated/set by the MissionController recalc methods @@ -143,7 +142,6 @@ public: double missionGimbalYaw (void) const { return _missionGimbalYaw; } double missionVehicleYaw (void) const { return _missionVehicleYaw; } - bool showMissionGimbalYaw(void) const { return !qIsNaN(_missionGimbalYaw); } void setMissionVehicleYaw(double vehicleYaw); static const char* jsonTypeKey; ///< Json file attribute which specifies the item type diff --git a/src/MissionManager/VisualMissionItemTest.cc b/src/MissionManager/VisualMissionItemTest.cc index eee77ca30c223f9780990f15eb30d0d3cc19ef45..43a027887cad634c2e9749e9aa34952ca956f3bd 100644 --- a/src/MissionManager/VisualMissionItemTest.cc +++ b/src/MissionManager/VisualMissionItemTest.cc @@ -56,3 +56,11 @@ void VisualMissionItemTest::cleanup(void) delete _offlineVehicle; UnitTest::cleanup(); } + +void VisualMissionItemTest::_createSpy(SimpleMissionItem* simpleItem, MultiSignalSpy** visualSpy) +{ + *visualSpy = NULL; + MultiSignalSpy* spy = new MultiSignalSpy(); + QCOMPARE(spy->init(simpleItem, rgVisualItemSignals, cVisualItemSignals), true); + *visualSpy = spy; +} diff --git a/src/MissionManager/VisualMissionItemTest.h b/src/MissionManager/VisualMissionItemTest.h index 30d4c196a8f33f3f360b29e4237b759e447255ff..3728bfe5339760e2e5cfd90049fba901862b1bb8 100644 --- a/src/MissionManager/VisualMissionItemTest.h +++ b/src/MissionManager/VisualMissionItemTest.h @@ -12,6 +12,7 @@ #include "UnitTest.h" #include "TCPLink.h" #include "MultiSignalSpy.h" +#include "SimpleMissionItem.h" #include @@ -27,6 +28,8 @@ public: void cleanup(void) override; protected: + void _createSpy(SimpleMissionItem* simpleItem, MultiSignalSpy** visualSpy); + enum { altDifferenceChangedIndex = 0, altPercentChangedIndex,