diff --git a/src/MissionManager/CameraSection.cc b/src/MissionManager/CameraSection.cc index b6a2a4d341e8d28777767983b92f341464bd526e..f16d54aaf1a111023475cb317f1b2bfe0732dd9c 100644 --- a/src/MissionManager/CameraSection.cc +++ b/src/MissionManager/CameraSection.cc @@ -59,8 +59,8 @@ CameraSection::CameraSection(Vehicle* vehicle, QObject* parent) connect(&_cameraActionFact, &Fact::valueChanged, this, &CameraSection::_cameraActionChanged); - connect(&_gimbalPitchFact, &Fact::valueChanged, this, &CameraSection::_setDirty); - connect(&_gimbalYawFact, &Fact::valueChanged, this, &CameraSection::_setDirty); + connect(&_gimbalPitchFact, &Fact::valueChanged, this, &CameraSection::_dirtyIfSpecified); + connect(&_gimbalYawFact, &Fact::valueChanged, this, &CameraSection::_dirtyIfSpecified); connect(&_cameraPhotoIntervalDistanceFact, &Fact::valueChanged, this, &CameraSection::_setDirty); connect(&_cameraPhotoIntervalTimeFact, &Fact::valueChanged, this, &CameraSection::_setDirty); connect(&_cameraModeFact, &Fact::valueChanged, this, &CameraSection::_setDirty); @@ -68,7 +68,9 @@ CameraSection::CameraSection(Vehicle* vehicle, QObject* parent) connect(this, &CameraSection::specifyCameraModeChanged, this, &CameraSection::_setDirty); connect(this, &CameraSection::specifyGimbalChanged, this, &CameraSection::_updateSpecifiedGimbalYaw); + connect(this, &CameraSection::specifyGimbalChanged, this, &CameraSection::_updateSpecifiedGimbalPitch); connect(&_gimbalYawFact, &Fact::valueChanged, this, &CameraSection::_updateSpecifiedGimbalYaw); + connect(&_gimbalPitchFact, &Fact::valueChanged, this, &CameraSection::_updateSpecifiedGimbalPitch); } void CameraSection::setSpecifyGimbal(bool specifyGimbal) @@ -491,9 +493,23 @@ double CameraSection::specifiedGimbalYaw(void) const return _specifyGimbal ? _gimbalYawFact.rawValue().toDouble() : std::numeric_limits::quiet_NaN(); } +double CameraSection::specifiedGimbalPitch(void) const +{ + return _specifyGimbal ? _gimbalPitchFact.rawValue().toDouble() : std::numeric_limits::quiet_NaN(); +} + void CameraSection::_updateSpecifiedGimbalYaw(void) { - emit specifiedGimbalYawChanged(specifiedGimbalYaw()); + if (_specifyGimbal) { + emit specifiedGimbalYawChanged(specifiedGimbalYaw()); + } +} + +void CameraSection::_updateSpecifiedGimbalPitch(void) +{ + if (_specifyGimbal) { + emit specifiedGimbalPitchChanged(specifiedGimbalPitch()); + } } void CameraSection::_updateSettingsSpecified(void) @@ -521,3 +537,11 @@ bool CameraSection::cameraModeSupported(void) const { return _vehicle->firmwarePlugin()->supportedMissionCommands().contains(MAV_CMD_SET_CAMERA_MODE); } + +void CameraSection::_dirtyIfSpecified(void) +{ + // We only set the dirty bit if specify gimbal it set. This allows us to change defaults without affecting dirty. + if (_specifyGimbal) { + setDirty(true); + } +} diff --git a/src/MissionManager/CameraSection.h b/src/MissionManager/CameraSection.h index 862354ae9b4f2b576e28e091ce46a9e903990bb6..32d019e5efeb414cee187747aae6811571c09a57 100644 --- a/src/MissionManager/CameraSection.h +++ b/src/MissionManager/CameraSection.h @@ -61,6 +61,10 @@ public: ///< @return The gimbal yaw specified by this item, NaN if not specified double specifiedGimbalYaw(void) const; + ///< Signals specifiedGimbalPitchChanged + ///< @return The gimbal pitch specified by this item, NaN if not specified + double specifiedGimbalPitch(void) const; + // Overrides from Section bool available (void) const override { return _available; } bool dirty (void) const override { return _dirty; } @@ -75,14 +79,17 @@ signals: bool specifyGimbalChanged (bool specifyGimbal); bool specifyCameraModeChanged (bool specifyCameraMode); void specifiedGimbalYawChanged (double gimbalYaw); + void specifiedGimbalPitchChanged(double gimbalPitch); private slots: void _setDirty(void); void _setDirtyAndUpdateItemCount(void); void _updateSpecifiedGimbalYaw(void); + void _updateSpecifiedGimbalPitch(void); void _specifyChanged(void); void _updateSettingsSpecified(void); void _cameraActionChanged(void); + void _dirtyIfSpecified(void); private: bool _scanGimbal(QmlObjectListModel* visualItems, int scanIndex); diff --git a/src/MissionManager/CameraSectionTest.cc b/src/MissionManager/CameraSectionTest.cc index 3408a23784f2555c8055eaddd7d2190c5271fc55..b93aae5c1c5850f146977b01c0c1eadc1515e56f 100644 --- a/src/MissionManager/CameraSectionTest.cc +++ b/src/MissionManager/CameraSectionTest.cc @@ -36,6 +36,7 @@ void CameraSectionTest::init(void) rgCameraSignals[specifyGimbalChangedIndex] = SIGNAL(specifyGimbalChanged(bool)); rgCameraSignals[specifiedGimbalYawChangedIndex] = SIGNAL(specifiedGimbalYawChanged(double)); + rgCameraSignals[specifiedGimbalPitchChangedIndex] = SIGNAL(specifiedGimbalPitchChanged(double)); rgCameraSignals[specifyCameraModeChangedIndex] = SIGNAL(specifyCameraModeChanged(bool)); _cameraSection = _simpleItem->cameraSection(); @@ -206,22 +207,32 @@ void CameraSectionTest::_testDirty(void) _cameraSection->setDirty(false); _spySection->clearAllSignals(); - // Check the remaining items that should set dirty bit + // dirty SHOULD NOT change if pitch or yaw is changed while specifyGimbal IS NOT set + _cameraSection->setSpecifyGimbal(false); + _cameraSection->setDirty(false); + _spySection->clearAllSignals(); + _cameraSection->gimbalPitch()->setRawValue(_cameraSection->gimbalPitch()->rawValue().toDouble() + 1); + _cameraSection->gimbalYaw()->setRawValue(_cameraSection->gimbalYaw()->rawValue().toDouble() + 1); + QVERIFY(_spySection->checkNoSignalByMask(dirtyChangedMask)); + QCOMPARE(_cameraSection->dirty(), false); + // dirty SHOULD change if pitch or yaw is changed while specifyGimbal IS set + _cameraSection->setSpecifyGimbal(true); + _cameraSection->setDirty(false); + _spySection->clearAllSignals(); _cameraSection->gimbalPitch()->setRawValue(_cameraSection->gimbalPitch()->rawValue().toDouble() + 1); QVERIFY(_spySection->checkSignalByMask(dirtyChangedMask)); - QCOMPARE(_spySection->pullBoolFromSignalIndex(dirtyChangedIndex), true); QCOMPARE(_cameraSection->dirty(), true); _cameraSection->setDirty(false); _spySection->clearAllSignals(); - - _cameraSection->gimbalYaw()->setRawValue(_cameraSection->gimbalPitch()->rawValue().toDouble() + 1); + _cameraSection->gimbalYaw()->setRawValue(_cameraSection->gimbalYaw()->rawValue().toDouble() + 1); QVERIFY(_spySection->checkSignalByMask(dirtyChangedMask)); - QCOMPARE(_spySection->pullBoolFromSignalIndex(dirtyChangedIndex), true); QCOMPARE(_cameraSection->dirty(), true); _cameraSection->setDirty(false); _spySection->clearAllSignals(); + // Check the remaining items that should set dirty bit + _cameraSection->cameraAction()->setRawValue(_cameraSection->cameraAction()->rawValue().toInt() + 1); QVERIFY(_spySection->checkSignalByMask(dirtyChangedMask)); QCOMPARE(_spySection->pullBoolFromSignalIndex(dirtyChangedIndex), true); @@ -1081,3 +1092,23 @@ void CameraSectionTest::_testScanForMultipleItems(void) } } } + +void CameraSectionTest::_testSpecifiedGimbalValuesChanged(void) +{ + // specifiedGimbal[Yaw|Pitch]Changed SHOULD NOT signal if values are changed when specifyGimbal IS NOT set + _cameraSection->setSpecifyGimbal(false); + _spyCamera->clearAllSignals(); + _cameraSection->gimbalYaw()->setRawValue(_cameraSection->gimbalYaw()->rawValue().toDouble() + 1); + QVERIFY(_spyCamera->checkNoSignalByMask(specifiedGimbalYawChangedMask)); + _cameraSection->gimbalPitch()->setRawValue(_cameraSection->gimbalPitch()->rawValue().toDouble() + 1); + QVERIFY(_spyCamera->checkNoSignalByMask(specifiedGimbalPitchChangedMask)); + + // specifiedGimbal[Yaw|Pitch]Changed SHOULD signal if values are changed when specifyGimbal IS set + _cameraSection->setSpecifyGimbal(true); + _spyCamera->clearAllSignals(); + _cameraSection->gimbalYaw()->setRawValue(_cameraSection->gimbalYaw()->rawValue().toDouble() + 1); + QVERIFY(_spyCamera->checkSignalByMask(specifiedGimbalYawChangedMask)); + _spyCamera->clearAllSignals(); + _cameraSection->gimbalPitch()->setRawValue(_cameraSection->gimbalPitch()->rawValue().toDouble() + 1); + QVERIFY(_spyCamera->checkSignalByMask(specifiedGimbalPitchChangedMask)); +} diff --git a/src/MissionManager/CameraSectionTest.h b/src/MissionManager/CameraSectionTest.h index 0ca4ffdd03f55f240b2bbf6265ae4f48cd9b5e7d..5e824a8fb720cd5b4cf9de32a1e36cd93f58c343 100644 --- a/src/MissionManager/CameraSectionTest.h +++ b/src/MissionManager/CameraSectionTest.h @@ -24,20 +24,21 @@ public: void cleanup(void) override; private slots: - void _testDirty(void); - void _testSettingsAvailable(void); - void _checkAvailable(void); - void _testItemCount(void); - void _testAppendSectionItems(void); - void _testScanForGimbalSection(void); - void _testScanForPhotoIntervalTimeSection(void); - void _testScanForPhotoIntervalDistanceSection(void); - void _testScanForStartVideoSection(void); - void _testScanForStopVideoSection(void); - void _testScanForStopImageSection(void); - void _testScanForCameraModeSection(void); - void _testScanForTakePhotoSection(void); - void _testScanForMultipleItems(void); + void _testDirty (void); + void _testSettingsAvailable (void); + void _checkAvailable (void); + void _testItemCount (void); + void _testAppendSectionItems (void); + void _testScanForGimbalSection (void); + void _testScanForPhotoIntervalTimeSection (void); + void _testScanForPhotoIntervalDistanceSection (void); + void _testScanForStartVideoSection (void); + void _testScanForStopVideoSection (void); + void _testScanForStopImageSection (void); + void _testScanForCameraModeSection (void); + void _testScanForTakePhotoSection (void); + void _testScanForMultipleItems (void); + void _testSpecifiedGimbalValuesChanged (void); private: void _createSpy(CameraSection* cameraSection, MultiSignalSpy** cameraSpy); @@ -47,14 +48,16 @@ private: enum { specifyGimbalChangedIndex = 0, specifiedGimbalYawChangedIndex, + specifiedGimbalPitchChangedIndex, specifyCameraModeChangedIndex, maxSignalIndex, }; enum { - specifyGimbalChangedMask = 1 << specifyGimbalChangedIndex, - specifiedGimbalYawChangedMask = 1 << specifiedGimbalYawChangedIndex, - specifyCameraModeChangedMask = 1 << specifyCameraModeChangedIndex, + specifyGimbalChangedMask = 1 << specifyGimbalChangedIndex, + specifiedGimbalYawChangedMask = 1 << specifiedGimbalYawChangedIndex, + specifiedGimbalPitchChangedMask = 1 << specifiedGimbalPitchChangedIndex, + specifyCameraModeChangedMask = 1 << specifyCameraModeChangedIndex, }; static const size_t cCameraSignals = maxSignalIndex; diff --git a/src/MissionManager/FixedWingLandingComplexItem.h b/src/MissionManager/FixedWingLandingComplexItem.h index 0c838076cb40793b2dfb77ee595c525b1c829614..296fa70f6bc7ed5d2f0702d2611bcdaf1b6d0bd7 100644 --- a/src/MissionManager/FixedWingLandingComplexItem.h +++ b/src/MissionManager/FixedWingLandingComplexItem.h @@ -77,6 +77,7 @@ public: 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; diff --git a/src/MissionManager/MissionController.cc b/src/MissionManager/MissionController.cc index 8d923f608cb9ee7c5e434177c5a88e13d306ced9..88d8faae8878e8f1c4871403b78f2c5003f299de 100644 --- a/src/MissionManager/MissionController.cc +++ b/src/MissionManager/MissionController.cc @@ -91,6 +91,7 @@ void MissionController::_resetMissionFlightStatus(void) _missionFlightStatus.vehicleSpeed = _controllerVehicle->multiRotor() || _managerVehicle->vtol() ? _missionFlightStatus.hoverSpeed : _missionFlightStatus.cruiseSpeed; _missionFlightStatus.vehicleYaw = 0.0; _missionFlightStatus.gimbalYaw = std::numeric_limits::quiet_NaN(); + _missionFlightStatus.gimbalPitch = std::numeric_limits::quiet_NaN(); // Battery information @@ -355,6 +356,7 @@ int MissionController::insertSimpleMissionItem(QGeoCoordinate coordinate, int i) newItem->missionItem().setParam7(prevAltitude); } } + newItem->setMissionFlightStatus(_missionFlightStatus); _visualItems->insert(i, newItem); _recalcAll(); @@ -1227,12 +1229,13 @@ void MissionController::_recalcMissionFlightStatus() } // Look for gimbal change - if (_managerVehicle->vehicleYawsToNextWaypointInMission()) { - // We current only support gimbal display in this mode - double gimbalYaw = item->specifiedGimbalYaw(); - if (!qIsNaN(gimbalYaw)) { - _missionFlightStatus.gimbalYaw = gimbalYaw; - } + double gimbalYaw = item->specifiedGimbalYaw(); + if (!qIsNaN(gimbalYaw)) { + _missionFlightStatus.gimbalYaw = gimbalYaw; + } + double gimbalPitch = item->specifiedGimbalPitch(); + if (!qIsNaN(gimbalPitch)) { + _missionFlightStatus.gimbalPitch = gimbalPitch; } if (i == 0) { @@ -1511,6 +1514,7 @@ void MissionController::_initVisualItem(VisualMissionItem* visualItem) connect(visualItem, &VisualMissionItem::exitCoordinateHasRelativeAltitudeChanged, this, &MissionController::_recalcWaypointLines); connect(visualItem, &VisualMissionItem::specifiedFlightSpeedChanged, this, &MissionController::_recalcMissionFlightStatus); connect(visualItem, &VisualMissionItem::specifiedGimbalYawChanged, this, &MissionController::_recalcMissionFlightStatus); + connect(visualItem, &VisualMissionItem::specifiedGimbalPitchChanged, this, &MissionController::_recalcMissionFlightStatus); connect(visualItem, &VisualMissionItem::terrainAltitudeChanged, this, &MissionController::_recalcMissionFlightStatus); connect(visualItem, &VisualMissionItem::lastSequenceNumberChanged, this, &MissionController::_recalcSequence); diff --git a/src/MissionManager/MissionController.h b/src/MissionManager/MissionController.h index f004aca05839cdfa3762735bee6711c58b657811..de2825f503a16650667a18586def21ae3e90ac55 100644 --- a/src/MissionManager/MissionController.h +++ b/src/MissionManager/MissionController.h @@ -54,6 +54,7 @@ public: double vehicleSpeed; ///< Either cruise or hover speed based on vehicle type and vtol state double vehicleYaw; double gimbalYaw; ///< NaN signals yaw was never changed + double gimbalPitch; ///< NaN signals pitch was never changed int mAhBattery; ///< 0 for not available double hoverAmps; ///< Amp consumption during hover double cruiseAmps; ///< Amp consumption during cruise diff --git a/src/MissionManager/MissionItem.cc b/src/MissionManager/MissionItem.cc index 7696e0439d22ba9f2fa5b2c652ae5af516cb4a06..9b3e26d11daa7558f27eef1e39592e46e05b5710 100644 --- a/src/MissionManager/MissionItem.cc +++ b/src/MissionManager/MissionItem.cc @@ -52,6 +52,7 @@ MissionItem::MissionItem(QObject* parent) setAutoContinue(true); + connect(&_param1Fact, &Fact::rawValueChanged, this, &MissionItem::_param1Changed); connect(&_param2Fact, &Fact::rawValueChanged, this, &MissionItem::_param2Changed); connect(&_param3Fact, &Fact::rawValueChanged, this, &MissionItem::_param3Changed); } @@ -442,6 +443,27 @@ double MissionItem::specifiedGimbalYaw(void) const return gimbalYaw; } +double MissionItem::specifiedGimbalPitch(void) const +{ + double gimbalPitch = std::numeric_limits::quiet_NaN(); + + if (_commandFact.rawValue().toInt() == MAV_CMD_DO_MOUNT_CONTROL && _param7Fact.rawValue().toInt() == MAV_MOUNT_MODE_MAVLINK_TARGETING) { + gimbalPitch = _param1Fact.rawValue().toDouble(); + } + + return gimbalPitch; +} + +void MissionItem::_param1Changed(QVariant value) +{ + Q_UNUSED(value); + + double gimbalPitch = specifiedGimbalPitch(); + if (!qIsNaN(gimbalPitch)) { + emit specifiedGimbalPitchChanged(gimbalPitch); + } +} + void MissionItem::_param2Changed(QVariant value) { Q_UNUSED(value); diff --git a/src/MissionManager/MissionItem.h b/src/MissionManager/MissionItem.h index 78e07593aac0604214d072b890350fb6a60a2f52..8202a8480535ffdd41aaef21af3edb43fadfbc16 100644 --- a/src/MissionManager/MissionItem.h +++ b/src/MissionManager/MissionItem.h @@ -82,6 +82,9 @@ public: /// @return Flight gimbal yaw change value if this item supports it. If not it returns NaN. double specifiedGimbalYaw(void) const; + /// @return Flight gimbal pitch change value if this item supports it. If not it returns NaN. + double specifiedGimbalPitch(void) const; + void setCommand (MAV_CMD command); void setSequenceNumber (int sequenceNumber); void setIsCurrentItem (bool isCurrentItem); @@ -107,10 +110,12 @@ signals: void sequenceNumberChanged (int sequenceNumber); void specifiedFlightSpeedChanged(double flightSpeed); void specifiedGimbalYawChanged (double gimbalYaw); + void specifiedGimbalPitchChanged(double gimbalPitch); private slots: - void _param2Changed (QVariant value); - void _param3Changed (QVariant value); + void _param1Changed(QVariant value); + void _param2Changed(QVariant value); + void _param3Changed(QVariant value); private: bool _convertJsonV1ToV2(const QJsonObject& json, QJsonObject& v2Json, QString& errorString); diff --git a/src/MissionManager/MissionSettingsItem.cc b/src/MissionManager/MissionSettingsItem.cc index 8fa2708044c3aa5d0cd22cb9a375821aa99b94d4..808b4448d2351ada31d25bfa6a5c5ad8f3f987fd 100644 --- a/src/MissionManager/MissionSettingsItem.cc +++ b/src/MissionManager/MissionSettingsItem.cc @@ -63,8 +63,9 @@ MissionSettingsItem::MissionSettingsItem(Vehicle* vehicle, QObject* parent) connect(&_cameraSection, &CameraSection::dirtyChanged, this, &MissionSettingsItem::_sectionDirtyChanged); connect(&_speedSection, &SpeedSection::dirtyChanged, this, &MissionSettingsItem::_sectionDirtyChanged); - connect(&_cameraSection, &CameraSection::specifiedGimbalYawChanged, this, &MissionSettingsItem::specifiedGimbalYawChanged); - connect(&_speedSection, &SpeedSection::specifiedFlightSpeedChanged, this, &MissionSettingsItem::specifiedFlightSpeedChanged); + connect(&_cameraSection, &CameraSection::specifiedGimbalYawChanged, this, &MissionSettingsItem::specifiedGimbalYawChanged); + connect(&_cameraSection, &CameraSection::specifiedGimbalPitchChanged, this, &MissionSettingsItem::specifiedGimbalPitchChanged); + connect(&_speedSection, &SpeedSection::specifiedFlightSpeedChanged, this, &MissionSettingsItem::specifiedFlightSpeedChanged); } int MissionSettingsItem::lastSequenceNumber(void) const @@ -259,6 +260,11 @@ double MissionSettingsItem::specifiedGimbalYaw(void) return _cameraSection.specifyGimbal() ? _cameraSection.gimbalYaw()->rawValue().toDouble() : std::numeric_limits::quiet_NaN(); } +double MissionSettingsItem::specifiedGimbalPitch(void) +{ + return _cameraSection.specifyGimbal() ? _cameraSection.gimbalPitch()->rawValue().toDouble() : std::numeric_limits::quiet_NaN(); +} + void MissionSettingsItem::_updateAltitudeInCoordinate(QVariant value) { double newAltitude = value.toDouble(); diff --git a/src/MissionManager/MissionSettingsItem.h b/src/MissionManager/MissionSettingsItem.h index c61a6d7e7ad5b11e642adc8052c37fbc98a2789f..d20410917ddcd245265d2acd330b22d86a3ce5ce 100644 --- a/src/MissionManager/MissionSettingsItem.h +++ b/src/MissionManager/MissionSettingsItem.h @@ -73,6 +73,7 @@ public: QGeoCoordinate exitCoordinate (void) const final { return _plannedHomePositionCoordinate; } int sequenceNumber (void) const final { return _sequenceNumber; } double specifiedGimbalYaw (void) final; + double specifiedGimbalPitch (void) final; void appendMissionItems (QList& items, QObject* missionItemParent) final; void applyNewAltitude (double newAltitude) final { Q_UNUSED(newAltitude); /* no action */ } double specifiedFlightSpeed (void) final; diff --git a/src/MissionManager/SimpleMissionItem.cc b/src/MissionManager/SimpleMissionItem.cc index 4cded559f6574e04f28335e988fea654600fa503..4676fd4625de71827536f986f9f017f2c82be683 100644 --- a/src/MissionManager/SimpleMissionItem.cc +++ b/src/MissionManager/SimpleMissionItem.cc @@ -717,6 +717,11 @@ double SimpleMissionItem::specifiedGimbalYaw(void) return _cameraSection->available() ? _cameraSection->specifiedGimbalYaw() : missionItem().specifiedGimbalYaw(); } +double SimpleMissionItem::specifiedGimbalPitch(void) +{ + return _cameraSection->available() ? _cameraSection->specifiedGimbalPitch() : missionItem().specifiedGimbalPitch(); +} + bool SimpleMissionItem::scanForSections(QmlObjectListModel* visualItems, int scanIndex, Vehicle* vehicle) { bool sectionFound = false; @@ -754,10 +759,12 @@ void SimpleMissionItem::_updateOptionalSections(void) _speedSection->setAvailable(true); } - connect(_cameraSection, &CameraSection::dirtyChanged, this, &SimpleMissionItem::_sectionDirtyChanged); - connect(_cameraSection, &CameraSection::itemCountChanged, this, &SimpleMissionItem::_updateLastSequenceNumber); - connect(_cameraSection, &CameraSection::availableChanged, this, &SimpleMissionItem::specifiedGimbalYawChanged); - connect(_cameraSection, &CameraSection::specifiedGimbalYawChanged, this, &SimpleMissionItem::specifiedGimbalYawChanged); + connect(_cameraSection, &CameraSection::dirtyChanged, this, &SimpleMissionItem::_sectionDirtyChanged); + connect(_cameraSection, &CameraSection::itemCountChanged, this, &SimpleMissionItem::_updateLastSequenceNumber); + connect(_cameraSection, &CameraSection::availableChanged, this, &SimpleMissionItem::specifiedGimbalYawChanged); + connect(_cameraSection, &CameraSection::availableChanged, this, &SimpleMissionItem::specifiedGimbalPitchChanged); + connect(_cameraSection, &CameraSection::specifiedGimbalPitchChanged, this, &SimpleMissionItem::specifiedGimbalPitchChanged); + connect(_cameraSection, &CameraSection::specifiedGimbalYawChanged, this, &SimpleMissionItem::specifiedGimbalYawChanged); connect(_speedSection, &SpeedSection::dirtyChanged, this, &SimpleMissionItem::_sectionDirtyChanged); connect(_speedSection, &SpeedSection::itemCountChanged, this, &SimpleMissionItem::_updateLastSequenceNumber); @@ -813,3 +820,20 @@ void SimpleMissionItem::applyNewAltitude(double newAltitude) } } } + +void SimpleMissionItem::setMissionFlightStatus(MissionController::MissionFlightStatus_t& missionFlightStatus) +{ + // If user has not already set speed/gimbal, set defaults from previous items. + VisualMissionItem::setMissionFlightStatus(missionFlightStatus); + if (_speedSection->available() && !_speedSection->specifyFlightSpeed() && !qFuzzyCompare(_speedSection->flightSpeed()->rawValue().toDouble(), missionFlightStatus.vehicleSpeed)) { + _speedSection->flightSpeed()->setRawValue(missionFlightStatus.vehicleSpeed); + } + if (_cameraSection->available() && !_cameraSection->specifyGimbal()) { + if (!qIsNaN(missionFlightStatus.gimbalYaw) && !qFuzzyCompare(_cameraSection->gimbalYaw()->rawValue().toDouble(), missionFlightStatus.gimbalYaw)) { + _cameraSection->gimbalYaw()->setRawValue(missionFlightStatus.gimbalYaw); + } + if (!qIsNaN(missionFlightStatus.gimbalPitch) && !qFuzzyCompare(_cameraSection->gimbalPitch()->rawValue().toDouble(), missionFlightStatus.gimbalPitch)) { + _cameraSection->gimbalPitch()->setRawValue(missionFlightStatus.gimbalPitch); + } + } +} diff --git a/src/MissionManager/SimpleMissionItem.h b/src/MissionManager/SimpleMissionItem.h index 2e0e0cafe66dcfc82e731daa25c8873d36cf4c43..0479658293c37b652c2947d909234f729844c828 100644 --- a/src/MissionManager/SimpleMissionItem.h +++ b/src/MissionManager/SimpleMissionItem.h @@ -102,9 +102,11 @@ public: int sequenceNumber (void) const final { return _missionItem.sequenceNumber(); } double specifiedFlightSpeed (void) final; double specifiedGimbalYaw (void) final; + double specifiedGimbalPitch (void) final; QString mapVisualQML (void) const final { return QStringLiteral("SimpleItemMapVisual.qml"); } void appendMissionItems (QList& items, QObject* missionItemParent) final; void applyNewAltitude (double newAltitude) final; + void setMissionFlightStatus (MissionController::MissionFlightStatus_t& missionFlightStatus) final; bool coordinateHasRelativeAltitude (void) const final { return _missionItem.relativeAltitude(); } bool exitCoordinateHasRelativeAltitude (void) const final { return coordinateHasRelativeAltitude(); } diff --git a/src/MissionManager/SpeedSection.cc b/src/MissionManager/SpeedSection.cc index 6f465a3dca0ab9a9b5afccf64a72f988d353480d..90e49b5b2368442ecbf5b47d3f305114dd9667ea 100644 --- a/src/MissionManager/SpeedSection.cc +++ b/src/MissionManager/SpeedSection.cc @@ -39,7 +39,7 @@ SpeedSection::SpeedSection(Vehicle* vehicle, QObject* parent) _flightSpeedFact.setRawValue(flightSpeed); connect(this, &SpeedSection::specifyFlightSpeedChanged, this, &SpeedSection::settingsSpecifiedChanged); - connect(&_flightSpeedFact, &Fact::valueChanged, this, &SpeedSection::_setDirty); + connect(&_flightSpeedFact, &Fact::valueChanged, this, &SpeedSection::_flightSpeedChanged); connect(this, &SpeedSection::specifyFlightSpeedChanged, this, &SpeedSection::_updateSpecifiedFlightSpeed); connect(&_flightSpeedFact, &Fact::valueChanged, this, &SpeedSection::_updateSpecifiedFlightSpeed); @@ -60,11 +60,6 @@ void SpeedSection::setAvailable(bool available) } } -void SpeedSection::_setDirty(void) -{ - setDirty(true); -} - void SpeedSection::setDirty(bool dirty) { if (_dirty != dirty) { @@ -146,6 +141,16 @@ double SpeedSection::specifiedFlightSpeed(void) const void SpeedSection::_updateSpecifiedFlightSpeed(void) { - emit specifiedFlightSpeedChanged(specifiedFlightSpeed()); + if (_specifyFlightSpeed) { + emit specifiedFlightSpeedChanged(specifiedFlightSpeed()); + } } +void SpeedSection::_flightSpeedChanged(void) +{ + // We only set the dirty bit if specify flight speed it set. This allows us to change defaults for flight speed + // without affecting dirty. + if (_specifyFlightSpeed) { + setDirty(true); + } +} diff --git a/src/MissionManager/SpeedSection.h b/src/MissionManager/SpeedSection.h index 1b37b8f9f21facd47d15497cb03489639c44901b..ad0c66fde1763ec118ddaa6d52633a8e0ce4722d 100644 --- a/src/MissionManager/SpeedSection.h +++ b/src/MissionManager/SpeedSection.h @@ -46,8 +46,8 @@ signals: void specifiedFlightSpeedChanged (double flightSpeed); private slots: - void _setDirty (void); void _updateSpecifiedFlightSpeed(void); + void _flightSpeedChanged (void); private: bool _available; diff --git a/src/MissionManager/SpeedSectionTest.cc b/src/MissionManager/SpeedSectionTest.cc index eeed8ee2e3b40da65be30d65ab4d13c119ccb7ca..37a89776e2cec0b2ba54d67b3975d1a97c546412 100644 --- a/src/MissionManager/SpeedSectionTest.cc +++ b/src/MissionManager/SpeedSectionTest.cc @@ -73,7 +73,18 @@ void SpeedSectionTest::_testDirty(void) QCOMPARE(_speedSection->dirty(), false); _spySection->clearAllSignals(); - // Check the remaining items that should set dirty bit + // Flight speed change should only signal if specifyFlightSpeed is set + + _speedSection->setSpecifyFlightSpeed(false); + _speedSection->setDirty(false); + _spySection->clearAllSignals(); + _speedSection->flightSpeed()->setRawValue(_speedSection->flightSpeed()->rawValue().toDouble() + 1); + QVERIFY(_spySection->checkNoSignalByMask(dirtyChangedMask)); + QCOMPARE(_speedSection->dirty(), false); + + _speedSection->setSpecifyFlightSpeed(true); + _speedSection->setDirty(false); + _spySection->clearAllSignals(); _speedSection->flightSpeed()->setRawValue(_speedSection->flightSpeed()->rawValue().toDouble() + 1); QVERIFY(_spySection->checkSignalByMask(dirtyChangedMask)); QCOMPARE(_spySection->pullBoolFromSignalIndex(dirtyChangedIndex), true); @@ -264,3 +275,18 @@ void SpeedSectionTest::_testScanForSection(void) visualItems.clear(); scanIndex = 0; } + +void SpeedSectionTest::_testSpecifiedFlightSpeedChanged(void) +{ + // specifiedFlightSpeedChanged SHOULD NOT signal if flight speed is changed when specifyFlightSpeed IS NOT set + _speedSection->setSpecifyFlightSpeed(false); + _spySpeed->clearAllSignals(); + _speedSection->flightSpeed()->setRawValue(_speedSection->flightSpeed()->rawValue().toDouble() + 1); + QVERIFY(_spySpeed->checkNoSignalByMask(specifiedFlightSpeedChangedMask)); + + // specifiedFlightSpeedChanged SHOULD signal if flight speed is changed when specifyFlightSpeed IS set + _speedSection->setSpecifyFlightSpeed(true); + _spySpeed->clearAllSignals(); + _speedSection->flightSpeed()->setRawValue(_speedSection->flightSpeed()->rawValue().toDouble() + 1); + QVERIFY(_spySpeed->checkSignalByMask(specifiedFlightSpeedChangedMask)); +} diff --git a/src/MissionManager/SpeedSectionTest.h b/src/MissionManager/SpeedSectionTest.h index 406a86d672be805775fadc989a874328c1cad6fc..fe61a6ec72b4450dd7191f5566fb46a4112f7890 100644 --- a/src/MissionManager/SpeedSectionTest.h +++ b/src/MissionManager/SpeedSectionTest.h @@ -24,12 +24,13 @@ public: void cleanup(void) override; private slots: - void _testDirty(void); - void _testSettingsAvailable(void); - void _checkAvailable(void); - void _testItemCount(void); - void _testAppendSectionItems(void); - void _testScanForSection(void); + void _testDirty (void); + void _testSettingsAvailable (void); + void _checkAvailable (void); + void _testItemCount (void); + void _testAppendSectionItems (void); + void _testScanForSection (void); + void _testSpecifiedFlightSpeedChanged (void); private: void _createSpy(SpeedSection* speedSection, MultiSignalSpy** speedSpy); diff --git a/src/MissionManager/StructureScanComplexItem.h b/src/MissionManager/StructureScanComplexItem.h index 1843edbbee17fd19dc397209f748805f5901df8e..cebfa9a403824419faafaadf047d600d333c1513 100644 --- a/src/MissionManager/StructureScanComplexItem.h +++ b/src/MissionManager/StructureScanComplexItem.h @@ -81,6 +81,7 @@ public: 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 setMissionFlightStatus (MissionController::MissionFlightStatus_t& missionFlightStatus) final; void applyNewAltitude (double newAltitude) final; diff --git a/src/MissionManager/SurveyMissionItem.h b/src/MissionManager/SurveyMissionItem.h index 78e2ef41b1ec107331288a98b0c942b197290f4b..315008a932d495c4fd78ba477f7bce75bf264e72 100644 --- a/src/MissionManager/SurveyMissionItem.h +++ b/src/MissionManager/SurveyMissionItem.h @@ -117,6 +117,7 @@ public: 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 setMissionFlightStatus (MissionController::MissionFlightStatus_t& missionFlightStatus) final; void applyNewAltitude (double newAltitude) final; diff --git a/src/MissionManager/VisualMissionItem.h b/src/MissionManager/VisualMissionItem.h index fbd3d34eba51bdb482a4f947557ffef4ece09f74..a5a9d2296e12514df58d5ab86b78f873c9dbfdfe 100644 --- a/src/MissionManager/VisualMissionItem.h +++ b/src/MissionManager/VisualMissionItem.h @@ -65,7 +65,8 @@ public: Q_PROPERTY(QString mapVisualQML READ mapVisualQML CONSTANT) ///< QMl code for map visuals Q_PROPERTY(QmlObjectListModel* childItems READ childItems CONSTANT) Q_PROPERTY(double specifiedFlightSpeed READ specifiedFlightSpeed NOTIFY specifiedFlightSpeedChanged) ///< NaN if this item does not specify flight speed - Q_PROPERTY(double specifiedGimbalYaw READ specifiedGimbalYaw NOTIFY specifiedGimbalYawChanged) ///< NaN if this item goes not specify gimbal yaw + Q_PROPERTY(double specifiedGimbalYaw READ specifiedGimbalYaw NOTIFY specifiedGimbalYawChanged) ///< Gimbal yaw, NaN for not specified + Q_PROPERTY(double specifiedGimbalPitch READ specifiedGimbalPitch NOTIFY specifiedGimbalPitchChanged) ///< Gimbal pitch, NaN for not specified 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 @@ -116,6 +117,7 @@ public: virtual int sequenceNumber (void) const = 0; virtual double specifiedFlightSpeed (void) = 0; virtual double specifiedGimbalYaw (void) = 0; + virtual double specifiedGimbalPitch (void) = 0; /// Update item to mission flight status at point where this item appears in mission. /// IMPORTANT: Overrides must call base class implementation @@ -173,6 +175,7 @@ signals: void specifiesAltitudeOnlyChanged (void); void specifiedFlightSpeedChanged (void); void specifiedGimbalYawChanged (void); + void specifiedGimbalPitchChanged (void); void lastSequenceNumberChanged (int sequenceNumber); void missionGimbalYawChanged (double missionGimbalYaw); void missionVehicleYawChanged (double missionVehicleYaw); diff --git a/src/MissionManager/VisualMissionItemTest.cc b/src/MissionManager/VisualMissionItemTest.cc index 43a027887cad634c2e9749e9aa34952ca956f3bd..8a6e235ceaeacb6499c174b5b05895ae938aff3a 100644 --- a/src/MissionManager/VisualMissionItemTest.cc +++ b/src/MissionManager/VisualMissionItemTest.cc @@ -43,6 +43,7 @@ void VisualMissionItemTest::init(void) rgVisualItemSignals[specifiesAltitudeOnlyChangedIndex] = SIGNAL(specifiesAltitudeOnlyChanged()); rgVisualItemSignals[specifiedFlightSpeedChangedIndex] = SIGNAL(specifiedFlightSpeedChanged()); rgVisualItemSignals[specifiedGimbalYawChangedIndex] = SIGNAL(specifiedGimbalYawChanged()); + rgVisualItemSignals[specifiedGimbalPitchChangedIndex] = SIGNAL(specifiedGimbalPitchChanged()); rgVisualItemSignals[lastSequenceNumberChangedIndex] = SIGNAL(lastSequenceNumberChanged(int)); rgVisualItemSignals[missionGimbalYawChangedIndex] = SIGNAL(missionGimbalYawChanged(double)); rgVisualItemSignals[missionVehicleYawChangedIndex] = SIGNAL(missionVehicleYawChanged(double)); diff --git a/src/MissionManager/VisualMissionItemTest.h b/src/MissionManager/VisualMissionItemTest.h index 20ed2b267975c0af3e435f6e6dcba79d3e3ac681..5428bf5d4764d47bd45e1c154d25e586539eaad2 100644 --- a/src/MissionManager/VisualMissionItemTest.h +++ b/src/MissionManager/VisualMissionItemTest.h @@ -49,6 +49,7 @@ protected: specifiesAltitudeOnlyChangedIndex, specifiedFlightSpeedChangedIndex, specifiedGimbalYawChangedIndex, + specifiedGimbalPitchChangedIndex, lastSequenceNumberChangedIndex, missionGimbalYawChangedIndex, missionVehicleYawChangedIndex, @@ -77,6 +78,7 @@ protected: specifiesAltitudeOnlyChangedMask = 1 << specifiesAltitudeOnlyChangedIndex, specifiedFlightSpeedChangedMask = 1 << specifiedFlightSpeedChangedIndex, specifiedGimbalYawChangedMask = 1 << specifiedGimbalYawChangedIndex, + specifiedGimbalPitchChangedMask = 1 << specifiedGimbalPitchChangedIndex, lastSequenceNumberChangedMask = 1 << lastSequenceNumberChangedIndex, missionGimbalYawChangedMask = 1 << missionGimbalYawChangedIndex, missionVehicleYawChangedMask = 1 << missionVehicleYawChangedIndex,