From 11df383eab23b00ace796acaa780429a3269a3fd Mon Sep 17 00:00:00 2001 From: DonLakeFlyer Date: Tue, 6 Feb 2018 15:12:44 -0800 Subject: [PATCH] Switch to using ROI_WPNEXT_OFFSET Also remove custom gimbal support --- .../StructureScanComplexItem.cc | 48 +++++++------------ src/MissionManager/StructureScanComplexItem.h | 9 ---- .../StructureScanComplexItemTest.cc | 17 +------ .../StructureScanComplexItemTest.h | 1 - src/PlanView/StructureScanEditor.qml | 27 ----------- 5 files changed, 17 insertions(+), 85 deletions(-) diff --git a/src/MissionManager/StructureScanComplexItem.cc b/src/MissionManager/StructureScanComplexItem.cc index a32b22848..a9ab76c04 100644 --- a/src/MissionManager/StructureScanComplexItem.cc +++ b/src/MissionManager/StructureScanComplexItem.cc @@ -24,8 +24,6 @@ QGC_LOGGING_CATEGORY(StructureScanComplexItemLog, "StructureScanComplexItemLog") const char* StructureScanComplexItem::_altitudeFactName = "Altitude"; const char* StructureScanComplexItem::_structureHeightFactName = "StructureHeight"; const char* StructureScanComplexItem::_layersFactName = "Layers"; -const char* StructureScanComplexItem::_gimbalPitchFactName = "GimbalPitch"; -const char* StructureScanComplexItem::_gimbalYawFactName = "GimbalYaw"; const char* StructureScanComplexItem::jsonComplexItemTypeValue = "StructureScan"; const char* StructureScanComplexItem::_jsonCameraCalcKey = "CameraCalc"; @@ -46,8 +44,6 @@ StructureScanComplexItem::StructureScanComplexItem(Vehicle* vehicle, QObject* pa , _cameraCalc (vehicle) , _altitudeFact (0, _altitudeFactName, FactMetaData::valueTypeDouble) , _layersFact (0, _layersFactName, FactMetaData::valueTypeUint32) - , _gimbalPitchFact (0, _gimbalPitchFactName, FactMetaData::valueTypeDouble) - , _gimbalYawFact (0, _gimbalYawFactName, FactMetaData::valueTypeDouble) { _editorQml = "qrc:/qml/StructureScanEditor.qml"; @@ -57,20 +53,14 @@ StructureScanComplexItem::StructureScanComplexItem(Vehicle* vehicle, QObject* pa _altitudeFact.setMetaData (_metaDataMap[_altitudeFactName]); _layersFact.setMetaData (_metaDataMap[_layersFactName]); - _gimbalPitchFact.setMetaData(_metaDataMap[_gimbalPitchFactName]); - _gimbalYawFact.setMetaData (_metaDataMap[_gimbalYawFactName]); _altitudeFact.setRawValue (_altitudeFact.rawDefaultValue()); _layersFact.setRawValue (_layersFact.rawDefaultValue()); - _gimbalPitchFact.setRawValue(_gimbalPitchFact.rawDefaultValue()); - _gimbalYawFact.setRawValue (_gimbalYawFact.rawDefaultValue()); _altitudeFact.setRawValue(qgcApp()->toolbox()->settingsManager()->appSettings()->defaultMissionItemAltitude()->rawValue()); connect(&_altitudeFact, &Fact::valueChanged, this, &StructureScanComplexItem::_setDirty); connect(&_layersFact, &Fact::valueChanged, this, &StructureScanComplexItem::_setDirty); - connect(&_gimbalPitchFact, &Fact::valueChanged, this, &StructureScanComplexItem::_setDirty); - connect(&_gimbalYawFact, &Fact::valueChanged, this, &StructureScanComplexItem::_setDirty); connect(&_layersFact, &Fact::valueChanged, this, &StructureScanComplexItem::_recalcLayerInfo); connect(&_structureHeightFact, &Fact::valueChanged, this, &StructureScanComplexItem::_recalcLayerInfo); @@ -89,7 +79,6 @@ StructureScanComplexItem::StructureScanComplexItem(Vehicle* vehicle, QObject* pa connect(&_flightPolygon, &QGCMapPolygon::pathChanged, this, &StructureScanComplexItem::_flightPathChanged); connect(_cameraCalc.distanceToSurface(), &Fact::valueChanged, this, &StructureScanComplexItem::_rebuildFlightPolygon); - connect(&_cameraCalc, &CameraCalc::cameraNameChanged, this, &StructureScanComplexItem::_resetGimbal); connect(&_flightPolygon, &QGCMapPolygon::pathChanged, this, &StructureScanComplexItem::_recalcCameraShots); connect(_cameraCalc.adjustedFootprintSide(), &Fact::valueChanged, this, &StructureScanComplexItem::_recalcCameraShots); @@ -132,9 +121,9 @@ int StructureScanComplexItem::lastSequenceNumber(void) const { return _sequenceNumber + (_layersFact.rawValue().toInt() * - ((_flightPolygon.count() + 1) + // 1 waypoint for each polygon vertex + 1 to go back to first polygon vertex for each layer - 2)) + // Camera trigger start/stop for each layer - 1; // Gimbal control command + ((_flightPolygon.count() + 1) + // 1 waypoint for each polygon vertex + 1 to go back to first polygon vertex for each layer + 2)) + // Camera trigger start/stop for each layer + 2; // ROI_WPNEXT_OFFSET and ROI_NONE commands } void StructureScanComplexItem::setDirty(bool dirty) @@ -154,8 +143,6 @@ void StructureScanComplexItem::save(QJsonArray& missionItems) saveObject[VisualMissionItem::jsonTypeKey] = VisualMissionItem::jsonTypeComplexItemValue; saveObject[ComplexMissionItem::jsonComplexItemTypeKey] = jsonComplexItemTypeValue; - saveObject[_gimbalPitchFactName] = _gimbalPitchFact.rawValue().toDouble(); - saveObject[_gimbalYawFactName] = _gimbalYawFact.rawValue().toDouble(); saveObject[_altitudeFactName] = _altitudeFact.rawValue().toDouble(); saveObject[_structureHeightFactName] = _structureHeightFact.rawValue().toDouble(); saveObject[_jsonAltitudeRelativeKey] = _altitudeRelative; @@ -186,8 +173,6 @@ bool StructureScanComplexItem::load(const QJsonObject& complexObject, int sequen { VisualMissionItem::jsonTypeKey, QJsonValue::String, true }, { ComplexMissionItem::jsonComplexItemTypeKey, QJsonValue::String, true }, { QGCMapPolygon::jsonPolygonKey, QJsonValue::Array, true }, - { _gimbalPitchFactName, QJsonValue::Double, true }, - { _gimbalYawFactName, QJsonValue::Double, true }, { _altitudeFactName, QJsonValue::Double, true }, { _structureHeightFactName, QJsonValue::Double, true }, { _jsonAltitudeRelativeKey, QJsonValue::Bool, true }, @@ -220,8 +205,6 @@ bool StructureScanComplexItem::load(const QJsonObject& complexObject, int sequen return false; } - _gimbalPitchFact.setRawValue(complexObject[_gimbalPitchFactName].toDouble()); - _gimbalYawFact.setRawValue (complexObject[_gimbalYawFactName].toDouble()); _altitudeFact.setRawValue (complexObject[_altitudeFactName].toDouble()); _layersFact.setRawValue (complexObject[_layersFactName].toDouble()); _altitudeRelative = complexObject[_jsonAltitudeRelativeKey].toBool(true); @@ -268,13 +251,11 @@ void StructureScanComplexItem::appendMissionItems(QList& items, QO double baseAltitude = _altitudeFact.rawValue().toDouble(); MissionItem* item = new MissionItem(seqNum++, - MAV_CMD_DO_MOUNT_CONTROL, + MAV_CMD_DO_SET_ROI_WPNEXT_OFFSET, MAV_FRAME_MISSION, - _gimbalPitchFact.rawValue().toDouble(), - 0, // Gimbal roll - _gimbalYawFact.rawValue().toDouble(), - 0, 0, 0, // param 4-6 not used - MAV_MOUNT_MODE_MAVLINK_TARGETING, + 0, 0, 0, 0, // param 1-4 not used + 0, 0, // Pitch and Roll stay in standard orientation + 90, // 90 degreee yaw offset to point to structure true, // autoContinue false, // isCurrentItem missionItemParent); @@ -349,6 +330,15 @@ void StructureScanComplexItem::appendMissionItems(QList& items, QO missionItemParent); items.append(item); } + + item = new MissionItem(seqNum++, + MAV_CMD_DO_SET_ROI_NONE, + MAV_FRAME_MISSION, + 0, 0, 0,0, 0, 0, 0, // param 1-7 not used + true, // autoContinue + false, // isCurrentItem + missionItemParent); + items.append(item); } int StructureScanComplexItem::cameraShots(void) const @@ -447,12 +437,6 @@ void StructureScanComplexItem::_recalcCameraShots(void) _setCameraShots(cameraShots * _layersFact.rawValue().toInt()); } -void StructureScanComplexItem::_resetGimbal(void) -{ - _gimbalPitchFact.setCookedValue(0); - _gimbalYawFact.setCookedValue(90); -} - void StructureScanComplexItem::setAltitudeRelative(bool altitudeRelative) { if (altitudeRelative != _altitudeRelative) { diff --git a/src/MissionManager/StructureScanComplexItem.h b/src/MissionManager/StructureScanComplexItem.h index f7faadcac..8ebffef51 100644 --- a/src/MissionManager/StructureScanComplexItem.h +++ b/src/MissionManager/StructureScanComplexItem.h @@ -28,8 +28,6 @@ public: StructureScanComplexItem(Vehicle* vehicle, QObject* parent = NULL); Q_PROPERTY(CameraCalc* cameraCalc READ cameraCalc CONSTANT) - Q_PROPERTY(Fact* gimbalPitch READ gimbalPitch CONSTANT) - Q_PROPERTY(Fact* gimbalYaw READ gimbalYaw CONSTANT) Q_PROPERTY(Fact* altitude READ altitude CONSTANT) Q_PROPERTY(Fact* structureHeight READ structureHeight CONSTANT) Q_PROPERTY(Fact* layers READ layers CONSTANT) @@ -47,8 +45,6 @@ public: bool altitudeRelative (void) const { return _altitudeRelative; } int cameraShots (void) const; - Fact* gimbalPitch (void) { return &_gimbalPitchFact; } - Fact* gimbalYaw (void) { return &_gimbalYawFact; } double timeBetweenShots (void); QGCMapPolygon* structurePolygon (void) { return &_structurePolygon; } QGCMapPolygon* flightPolygon (void) { return &_flightPolygon; } @@ -111,7 +107,6 @@ private slots: void _updateCoordinateAltitudes (void); void _rebuildFlightPolygon (void); void _recalcCameraShots (void); - void _resetGimbal (void); void _recalcLayerInfo (void); private: @@ -140,14 +135,10 @@ private: Fact _altitudeFact; Fact _structureHeightFact; Fact _layersFact; - Fact _gimbalPitchFact; - Fact _gimbalYawFact; static const char* _altitudeFactName; static const char* _structureHeightFactName; static const char* _layersFactName; - static const char* _gimbalPitchFactName; - static const char* _gimbalYawFactName; static const char* _jsonCameraCalcKey; static const char* _jsonAltitudeRelativeKey; diff --git a/src/MissionManager/StructureScanComplexItemTest.cc b/src/MissionManager/StructureScanComplexItemTest.cc index 9058f1ae9..3a2a4081f 100644 --- a/src/MissionManager/StructureScanComplexItemTest.cc +++ b/src/MissionManager/StructureScanComplexItemTest.cc @@ -59,7 +59,7 @@ void StructureScanComplexItemTest::_testDirty(void) // These facts should set dirty when changed QList rgFacts; - rgFacts << _structureScanItem->gimbalPitch() << _structureScanItem->gimbalYaw() << _structureScanItem->altitude() << _structureScanItem->layers(); + rgFacts << _structureScanItem->altitude() << _structureScanItem->layers(); foreach(Fact* fact, rgFacts) { qDebug() << fact->name(); QVERIFY(!_structureScanItem->dirty()); @@ -93,8 +93,6 @@ void StructureScanComplexItemTest::_initItem(void) } _structureScanItem->cameraCalc()->setCameraName(CameraCalc::manualCameraName()); - _structureScanItem->gimbalPitch()->setCookedValue(45); - _structureScanItem->gimbalYaw()->setCookedValue(45); _structureScanItem->layers()->setCookedValue(2); _structureScanItem->setDirty(false); @@ -112,8 +110,6 @@ void StructureScanComplexItemTest::_validateItem(StructureScanComplexItem* item) } QCOMPARE(_structureScanItem->cameraCalc()->cameraName() , CameraCalc::manualCameraName()); - QCOMPARE(item->gimbalPitch()->cookedValue().toDouble(), 45.0); - QCOMPARE(item->gimbalYaw()->cookedValue().toDouble(), 45.0); QCOMPARE(item->layers()->cookedValue().toInt(), 2); } @@ -132,17 +128,6 @@ void StructureScanComplexItemTest::_testSaveLoad(void) newItem->deleteLater(); } -void StructureScanComplexItemTest::_testGimbalAngleUpdate(void) -{ - // This sets the item to CameraCalc::CameraSpecNone and non-standard gimbal angles - _initItem(); - - // Switching to a camera specific setup should set gimbal angles to defaults - _structureScanItem->cameraCalc()->setCameraName(CameraCalc::customCameraName()); - QCOMPARE(_structureScanItem->gimbalPitch()->cookedValue().toDouble(), 0.0); - QCOMPARE(_structureScanItem->gimbalYaw()->cookedValue().toDouble(), 90.0); -} - void StructureScanComplexItemTest::_testItemCount(void) { QList items; diff --git a/src/MissionManager/StructureScanComplexItemTest.h b/src/MissionManager/StructureScanComplexItemTest.h index c6c42432b..06f5e1b1a 100644 --- a/src/MissionManager/StructureScanComplexItemTest.h +++ b/src/MissionManager/StructureScanComplexItemTest.h @@ -27,7 +27,6 @@ protected: private slots: void _testDirty(void); void _testSaveLoad(void); - void _testGimbalAngleUpdate(void); void _testItemCount(void); private: diff --git a/src/PlanView/StructureScanEditor.qml b/src/PlanView/StructureScanEditor.qml index 9511c5380..60079ea9d 100644 --- a/src/PlanView/StructureScanEditor.qml +++ b/src/PlanView/StructureScanEditor.qml @@ -81,33 +81,6 @@ Rectangle { sideDistanceLabel: qsTr("Trigger Distance") } - GridLayout { - anchors.left: parent.left - anchors.right: parent.right - columnSpacing: ScreenTools.defaultFontPixelWidth / 2 - rowSpacing: 0 - columns: 3 - visible: missionItem.cameraCalc.isManualCamera - - Item { width: 1; height: 1 } - QGCLabel { text: qsTr("Pitch") } - QGCLabel { text: qsTr("Yaw") } - - QGCLabel { - text: qsTr("Gimbal") - Layout.fillWidth: true - } - FactTextField { - fact: missionItem.gimbalPitch - implicitWidth: ScreenTools.defaultFontPixelWidth * 9 - } - - FactTextField { - fact: missionItem.gimbalYaw - implicitWidth: ScreenTools.defaultFontPixelWidth * 9 - } - } - SectionHeader { id: scanHeader text: qsTr("Scan") -- 2.22.0