From 61bdd7f2eb0629537755492cee8c405360a88568 Mon Sep 17 00:00:00 2001 From: Don Gagne Date: Sun, 17 Feb 2019 12:38:19 -0800 Subject: [PATCH 1/4] Bug fixes for Structure Scan. Also includes support for Entrance/Exit Alt. --- .../StructureScan.SettingsGroup.json | 8 + src/MissionManager/StructureScanComplexItem.cc | 192 ++++++++++++++------- src/MissionManager/StructureScanComplexItem.h | 14 +- src/MissionManager/StructureScanComplexItemTest.cc | 2 +- src/PlanView/StructureScanEditor.qml | 10 +- 5 files changed, 155 insertions(+), 71 deletions(-) diff --git a/src/MissionManager/StructureScan.SettingsGroup.json b/src/MissionManager/StructureScan.SettingsGroup.json index ccfdf63..12331b5 100644 --- a/src/MissionManager/StructureScan.SettingsGroup.json +++ b/src/MissionManager/StructureScan.SettingsGroup.json @@ -10,6 +10,14 @@ "defaultValue": 0 }, { + "name": "EntranceAltitude", + "shortDescription": "Vehicle will fly to/from the structure at this altitude.", + "type": "double", + "units": "m", + "decimalPlaces": 1, + "defaultValue": 50 +}, +{ "name": "Altitude", "shortDescription": "Altitude for the bottom layer of the structure scan.", "type": "double", diff --git a/src/MissionManager/StructureScanComplexItem.cc b/src/MissionManager/StructureScanComplexItem.cc index 629842e..651f471 100644 --- a/src/MissionManager/StructureScanComplexItem.cc +++ b/src/MissionManager/StructureScanComplexItem.cc @@ -22,7 +22,8 @@ QGC_LOGGING_CATEGORY(StructureScanComplexItemLog, "StructureScanComplexItemLog") const char* StructureScanComplexItem::settingsGroup = "StructureScan"; -const char* StructureScanComplexItem::altitudeName = "Altitude"; +const char* StructureScanComplexItem::_entranceAltName = "EntranceAltitude"; +const char* StructureScanComplexItem::scanBottomAltName = "Altitude"; const char* StructureScanComplexItem::structureHeightName = "StructureHeight"; const char* StructureScanComplexItem::layersName = "Layers"; const char* StructureScanComplexItem::gimbalPitchName = "GimbalPitch"; @@ -43,20 +44,22 @@ StructureScanComplexItem::StructureScanComplexItem(Vehicle* vehicle, bool flyVie , _scanDistance (0.0) , _cameraShots (0) , _cameraCalc (vehicle, settingsGroup) - , _altitudeFact (settingsGroup, _metaDataMap[altitudeName]) + , _scanBottomAltFact (settingsGroup, _metaDataMap[scanBottomAltName]) , _structureHeightFact (settingsGroup, _metaDataMap[structureHeightName]) , _layersFact (settingsGroup, _metaDataMap[layersName]) , _gimbalPitchFact (settingsGroup, _metaDataMap[gimbalPitchName]) , _startFromTopFact (settingsGroup, _metaDataMap[startFromTopName]) + , _entranceAltFact (settingsGroup, _metaDataMap[_entranceAltName]) { _editorQml = "qrc:/qml/StructureScanEditor.qml"; - _altitudeFact.setRawValue(qgcApp()->toolbox()->settingsManager()->appSettings()->defaultMissionItemAltitude()->rawValue()); + _entranceAltFact.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(&_startFromTopFact, &Fact::valueChanged, this, &StructureScanComplexItem::_setDirty); + connect(&_entranceAltFact, &Fact::valueChanged, this, &StructureScanComplexItem::_setDirty); + connect(&_scanBottomAltFact, &Fact::valueChanged, this, &StructureScanComplexItem::_setDirty); + connect(&_layersFact, &Fact::valueChanged, this, &StructureScanComplexItem::_setDirty); + connect(&_gimbalPitchFact, &Fact::valueChanged, this, &StructureScanComplexItem::_setDirty); + connect(&_startFromTopFact, &Fact::valueChanged, this, &StructureScanComplexItem::_setDirty); connect(&_layersFact, &Fact::valueChanged, this, &StructureScanComplexItem::_recalcLayerInfo); connect(&_structureHeightFact, &Fact::valueChanged, this, &StructureScanComplexItem::_recalcLayerInfo); @@ -66,7 +69,7 @@ StructureScanComplexItem::StructureScanComplexItem(Vehicle* vehicle, bool flyVie connect(this, &StructureScanComplexItem::altitudeRelativeChanged, this, &StructureScanComplexItem::coordinateHasRelativeAltitudeChanged); connect(this, &StructureScanComplexItem::altitudeRelativeChanged, this, &StructureScanComplexItem::exitCoordinateHasRelativeAltitudeChanged); - connect(&_altitudeFact, &Fact::valueChanged, this, &StructureScanComplexItem::_updateCoordinateAltitudes); + connect(&_entranceAltFact, &Fact::valueChanged, this, &StructureScanComplexItem::_updateCoordinateAltitudes); connect(&_structurePolygon, &QGCMapPolygon::dirtyChanged, this, &StructureScanComplexItem::_polygonDirtyChanged); connect(&_structurePolygon, &QGCMapPolygon::pathChanged, this, &StructureScanComplexItem::_rebuildFlightPolygon); @@ -75,6 +78,7 @@ StructureScanComplexItem::StructureScanComplexItem(Vehicle* vehicle, bool flyVie connect(&_layersFact, &Fact::valueChanged, this, &StructureScanComplexItem::_updateLastSequenceNumber); connect(&_flightPolygon, &QGCMapPolygon::pathChanged, this, &StructureScanComplexItem::_flightPathChanged); + connect(&_flightPolygon, &QGCMapPolygon::countChanged, this, &StructureScanComplexItem::_validateEntryVertex); connect(_cameraCalc.distanceToSurface(), &Fact::valueChanged, this, &StructureScanComplexItem::_rebuildFlightPolygon); @@ -130,9 +134,12 @@ int StructureScanComplexItem::lastSequenceNumber(void) const // Two commands for camera trigger start/stop int layerItemCount = _flightPolygon.count() + 1 + 2; + // Take into account the number of layers int multiLayerItemCount = layerItemCount * _layersFact.rawValue().toInt(); - int itemCount = multiLayerItemCount + 2; // +2 for ROI_WPNEXT_OFFSET and ROI_NONE commands + // +2 for ROI_WPNEXT_OFFSET and ROI_NONE commands + // +2 for entrance/exit waypoints + int itemCount = multiLayerItemCount + 2 + 2; return _sequenceNumber + itemCount - 1; } @@ -154,7 +161,8 @@ void StructureScanComplexItem::save(QJsonArray& missionItems) saveObject[VisualMissionItem::jsonTypeKey] = VisualMissionItem::jsonTypeComplexItemValue; saveObject[ComplexMissionItem::jsonComplexItemTypeKey] = jsonComplexItemTypeValue; - saveObject[altitudeName] = _altitudeFact.rawValue().toDouble(); + saveObject[_entranceAltName] = _entranceAltFact.rawValue().toDouble(); + saveObject[scanBottomAltName] = _scanBottomAltFact.rawValue().toDouble(); saveObject[structureHeightName] = _structureHeightFact.rawValue().toDouble(); saveObject[_jsonAltitudeRelativeKey] = _altitudeRelative; saveObject[layersName] = _layersFact.rawValue().toDouble(); @@ -186,13 +194,15 @@ bool StructureScanComplexItem::load(const QJsonObject& complexObject, int sequen { VisualMissionItem::jsonTypeKey, QJsonValue::String, true }, { ComplexMissionItem::jsonComplexItemTypeKey, QJsonValue::String, true }, { QGCMapPolygon::jsonPolygonKey, QJsonValue::Array, true }, - { altitudeName, QJsonValue::Double, true }, + { scanBottomAltName, QJsonValue::Double, true }, { structureHeightName, QJsonValue::Double, true }, { _jsonAltitudeRelativeKey, QJsonValue::Bool, true }, { layersName, QJsonValue::Double, true }, - { gimbalPitchName, QJsonValue::Double, false }, // This value was added after initial implementation so may be missing from older files - { startFromTopName, QJsonValue::Bool, false }, // This value was added after initial implementation so may be missing from older files { _jsonCameraCalcKey, QJsonValue::Object, true }, + // The following values were added after initial implementation so they may be missing from older files. Hence optional. + { _entranceAltName, QJsonValue::Double, false }, + { gimbalPitchName, QJsonValue::Double, false }, + { startFromTopName, QJsonValue::Bool, false }, }; if (!JsonHelper::validateKeys(complexObject, keyInfoList, errorString)) { return false; @@ -220,10 +230,17 @@ bool StructureScanComplexItem::load(const QJsonObject& complexObject, int sequen return false; } - _altitudeFact.setRawValue (complexObject[altitudeName].toDouble()); + _scanBottomAltFact.setRawValue (complexObject[scanBottomAltName].toDouble()); _layersFact.setRawValue (complexObject[layersName].toDouble()); _structureHeightFact.setRawValue(complexObject[structureHeightName].toDouble()); - _startFromTopFact.setRawValue (complexObject[startFromTopName].toBool(false)); // Set the false if doesn't exist, which matches previous functionality prior to setting + + // Start from top is a new setting which may not exist in older saved structure scans. + // Default to false if doesn't exist, which matches previous functionality. + _startFromTopFact.setRawValue (complexObject[startFromTopName].toBool(false)); + + // Entrance altitude is a new setting which may not exist in older saved structure scans. + // We default it to the top of the structure which is the safest thing to do without a value. + _entranceAltFact.setRawValue (complexObject[startFromTopName].toDouble(_scanBottomAltFact.rawValue().toDouble() + _structureHeightFact.rawValue().toDouble())); _altitudeRelative = complexObject[_jsonAltitudeRelativeKey].toBool(true); @@ -297,20 +314,41 @@ void StructureScanComplexItem::appendMissionItems(QList& items, QO { int seqNum = _sequenceNumber; bool startFromTop = _startFromTopFact.rawValue().toBool(); - double startAltitude = _altitudeFact.rawValue().toDouble() + (startFromTop ? _structureHeightFact.rawValue().toDouble() : 0); - - MissionItem* item = new MissionItem(seqNum++, - MAV_CMD_DO_SET_ROI_WPNEXT_OFFSET, - MAV_FRAME_MISSION, - 0, 0, 0, 0, // param 1-4 not used - _gimbalPitchFact.rawValue().toDouble(), - 0, // Roll stays in standard orientation - 90, // 90 degreee yaw offset to point to structure - true, // autoContinue - false, // isCurrentItem - missionItemParent); + double startAltitude = _scanBottomAltFact.rawValue().toDouble() + (startFromTop ? _structureHeightFact.rawValue().toDouble() : 0); + + MissionItem* item = nullptr; + + // Entrance waypoint + QGeoCoordinate entranceExitCoord = _flightPolygon.vertexCoordinate(_entryVertex); + item = new MissionItem(seqNum++, + MAV_CMD_NAV_WAYPOINT, + _altitudeRelative ? MAV_FRAME_GLOBAL_RELATIVE_ALT : MAV_FRAME_GLOBAL, + 0, // No hold time + 0.0, // No acceptance radius specified + 0.0, // Pass through waypoint + std::numeric_limits::quiet_NaN(), // Yaw unchanged + entranceExitCoord.latitude(), + entranceExitCoord.longitude(), + _entranceAltFact.rawValue().toDouble(), + true, // autoContinue + false, // isCurrentItem + missionItemParent); items.append(item); + // Point camera at structure + item = new MissionItem(seqNum++, + MAV_CMD_DO_SET_ROI_WPNEXT_OFFSET, + MAV_FRAME_MISSION, + 0, 0, 0, 0, // param 1-4 not used + _gimbalPitchFact.rawValue().toDouble(), + 0, // Roll stays in standard orientation + 90, // 90 degreee yaw offset to point to structure + true, // autoContinue + false, // isCurrentItem + missionItemParent); + items.append(item); + + // Fly a layer pattern for (int layer=0; layer<_layersFact.rawValue().toInt(); layer++) { bool addTriggerStart = true; double layerIncrement = (_cameraCalc.adjustedFootprintFrontal()->rawValue().toDouble() / 2.0) + (layer * _cameraCalc.adjustedFootprintFrontal()->rawValue().toDouble()); @@ -322,24 +360,28 @@ void StructureScanComplexItem::appendMissionItems(QList& items, QO layerAltitude = startAltitude + layerIncrement; } - for (int i=0; i<_flightPolygon.count(); i++) { - QGeoCoordinate vertexCoord = _flightPolygon.vertexCoordinate(i); - - MissionItem* item = new MissionItem(seqNum++, - MAV_CMD_NAV_WAYPOINT, - _altitudeRelative ? MAV_FRAME_GLOBAL_RELATIVE_ALT : MAV_FRAME_GLOBAL, - 0, // No hold time - 0.0, // No acceptance radius specified - 0.0, // Pass through waypoint - std::numeric_limits::quiet_NaN(), // Yaw unchanged - vertexCoord.latitude(), - vertexCoord.longitude(), - layerAltitude, - true, // autoContinue - false, // isCurrentItem - missionItemParent); + bool done = false; + int currentVertex = _entryVertex; + int processedVertices = 0; + do { + QGeoCoordinate vertexCoord = _flightPolygon.vertexCoordinate(currentVertex); + + item = new MissionItem(seqNum++, + MAV_CMD_NAV_WAYPOINT, + _altitudeRelative ? MAV_FRAME_GLOBAL_RELATIVE_ALT : MAV_FRAME_GLOBAL, + 0, // No hold time + 0.0, // No acceptance radius specified + 0.0, // Pass through waypoint + std::numeric_limits::quiet_NaN(), // Yaw unchanged + vertexCoord.latitude(), + vertexCoord.longitude(), + layerAltitude, + true, // autoContinue + false, // isCurrentItem + missionItemParent); items.append(item); + // Start camera triggering after first waypoint in layer if (addTriggerStart) { addTriggerStart = false; item = new MissionItem(seqNum++, @@ -354,25 +396,19 @@ void StructureScanComplexItem::appendMissionItems(QList& items, QO missionItemParent); items.append(item); } - } - QGeoCoordinate vertexCoord = _flightPolygon.vertexCoordinate(0); - - MissionItem* item = new MissionItem(seqNum++, - MAV_CMD_NAV_WAYPOINT, - _altitudeRelative ? MAV_FRAME_GLOBAL_RELATIVE_ALT : MAV_FRAME_GLOBAL, - 0, // No hold time - 0.0, // No acceptance radius specified - 0.0, // Pass through waypoint - std::numeric_limits::quiet_NaN(), // Yaw unchanged - vertexCoord.latitude(), - vertexCoord.longitude(), - layerAltitude, - true, // autoContinue - false, // isCurrentItem - missionItemParent); - items.append(item); + // Move to next vertext + currentVertex++; + if (currentVertex >= _flightPolygon.count()) { + currentVertex = 0; + } + + // Have we processed all the vertices + processedVertices++; + done = processedVertices == _flightPolygon.count() + 1; + } while (!done); + // Stop camera triggering after last waypoint in layer item = new MissionItem(seqNum++, MAV_CMD_DO_SET_CAM_TRIGG_DIST, MAV_FRAME_MISSION, @@ -386,6 +422,7 @@ void StructureScanComplexItem::appendMissionItems(QList& items, QO items.append(item); } + // Return camera to neutral position item = new MissionItem(seqNum++, MAV_CMD_DO_SET_ROI_NONE, MAV_FRAME_MISSION, @@ -394,11 +431,28 @@ void StructureScanComplexItem::appendMissionItems(QList& items, QO false, // isCurrentItem missionItemParent); items.append(item); + + // Exit waypoint + item = new MissionItem(seqNum++, + MAV_CMD_NAV_WAYPOINT, + _altitudeRelative ? MAV_FRAME_GLOBAL_RELATIVE_ALT : MAV_FRAME_GLOBAL, + 0, // No hold time + 0.0, // No acceptance radius specified + 0.0, // Pass through waypoint + std::numeric_limits::quiet_NaN(), // Yaw unchanged + entranceExitCoord.latitude(), + entranceExitCoord.longitude(), + _entranceAltFact.rawValue().toDouble(), + true, // autoContinue + false, // isCurrentItem + missionItemParent); + items.append(item); + } int StructureScanComplexItem::cameraShots(void) const { - return true /*_triggerCamera()*/ ? _cameraShots : 0; + return _cameraShots; } void StructureScanComplexItem::setMissionFlightStatus(MissionController::MissionFlightStatus_t& missionFlightStatus) @@ -417,7 +471,7 @@ void StructureScanComplexItem::_setDirty(void) void StructureScanComplexItem::applyNewAltitude(double newAltitude) { - _altitudeFact.setRawValue(newAltitude); + _entranceAltFact.setRawValue(newAltitude); } void StructureScanComplexItem::_polygonDirtyChanged(bool dirty) @@ -435,8 +489,9 @@ double StructureScanComplexItem::timeBetweenShots(void) QGeoCoordinate StructureScanComplexItem::coordinate(void) const { if (_flightPolygon.count() > 0) { - int entryVertex = qMax(qMin(_entryVertex, _flightPolygon.count() - 1), 0); - return _flightPolygon.vertexCoordinate(entryVertex); + QGeoCoordinate coord = _flightPolygon.vertexCoordinate(_entryVertex); + coord.setAltitude(_entranceAltFact.rawValue().toDouble()); + return coord; } else { return QGeoCoordinate(); } @@ -529,3 +584,12 @@ void StructureScanComplexItem::_updateGimbalPitch(void) _gimbalPitchFact.setRawValue(0); } } + +void StructureScanComplexItem::_validateEntryVertex(void) +{ + if (_entryVertex >= _flightPolygon.count()) { + _entryVertex = 0; + emit coordinateChanged(coordinate()); + emit exitCoordinateChanged(exitCoordinate()); + } +} diff --git a/src/MissionManager/StructureScanComplexItem.h b/src/MissionManager/StructureScanComplexItem.h index 810a6da..8905f6c 100644 --- a/src/MissionManager/StructureScanComplexItem.h +++ b/src/MissionManager/StructureScanComplexItem.h @@ -31,7 +31,8 @@ public: StructureScanComplexItem(Vehicle* vehicle, bool flyView, const QString& kmlOrSHPFile, QObject* parent); Q_PROPERTY(CameraCalc* cameraCalc READ cameraCalc CONSTANT) - Q_PROPERTY(Fact* altitude READ altitude CONSTANT) + Q_PROPERTY(Fact* entranceAlt READ entranceAlt CONSTANT) + Q_PROPERTY(Fact* scanBottomAlt READ scanBottomAlt CONSTANT) Q_PROPERTY(Fact* structureHeight READ structureHeight CONSTANT) Q_PROPERTY(Fact* layers READ layers CONSTANT) Q_PROPERTY(Fact* gimbalPitch READ gimbalPitch CONSTANT) @@ -43,7 +44,8 @@ public: Q_PROPERTY(QGCMapPolygon* flightPolygon READ flightPolygon CONSTANT) CameraCalc* cameraCalc (void) { return &_cameraCalc; } - Fact* altitude (void) { return &_altitudeFact; } + Fact* entranceAlt (void) { return &_entranceAltFact; } + Fact* scanBottomAlt (void) { return &_scanBottomAltFact; } Fact* structureHeight (void) { return &_structureHeightFact; } Fact* layers (void) { return &_layersFact; } Fact* gimbalPitch (void) { return &_gimbalPitchFact; } @@ -100,7 +102,7 @@ public: static const char* jsonComplexItemTypeValue; static const char* settingsGroup; - static const char* altitudeName; + static const char* scanBottomAltName; static const char* structureHeightName; static const char* layersName; static const char* gimbalPitchName; @@ -122,6 +124,7 @@ private slots: void _recalcLayerInfo (void); void _updateLastSequenceNumber (void); void _updateGimbalPitch (void); + void _validateEntryVertex (void); private: void _setExitCoordinate(const QGeoCoordinate& coordinate); @@ -146,15 +149,18 @@ private: CameraCalc _cameraCalc; - SettingsFact _altitudeFact; + SettingsFact _scanBottomAltFact; SettingsFact _structureHeightFact; SettingsFact _layersFact; SettingsFact _gimbalPitchFact; SettingsFact _startFromTopFact; + SettingsFact _entranceAltFact; static const char* _jsonCameraCalcKey; static const char* _jsonAltitudeRelativeKey; + static const char* _entranceAltName; // This value cannot be overriden + friend class StructureScanComplexItemTest; }; diff --git a/src/MissionManager/StructureScanComplexItemTest.cc b/src/MissionManager/StructureScanComplexItemTest.cc index 17fc257..66f1c39 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->altitude() << _structureScanItem->layers(); + rgFacts << _structureScanItem->entranceAlt() << _structureScanItem->layers(); for(Fact* fact: rgFacts) { qDebug() << fact->name(); QVERIFY(!_structureScanItem->dirty()); diff --git a/src/PlanView/StructureScanEditor.qml b/src/PlanView/StructureScanEditor.qml index 827a6a5..8ce9f78 100644 --- a/src/PlanView/StructureScanEditor.qml +++ b/src/PlanView/StructureScanEditor.qml @@ -128,9 +128,15 @@ Rectangle { visible: missionItem.cameraCalc.isManualCamera } - QGCLabel { text: qsTr("Bottom layer alt") } + QGCLabel { text: qsTr("Scan bottom alt") } FactTextField { - fact: missionItem.altitude + fact: missionItem.scanBottomAlt + Layout.fillWidth: true + } + + QGCLabel { text: qsTr("Entrance/Exit alt") } + FactTextField { + fact: missionItem.entranceAlt Layout.fillWidth: true } From 449861be56664b5db03fd81a9655b389d1c6cb1d Mon Sep 17 00:00:00 2001 From: Don Gagne Date: Sun, 17 Feb 2019 14:32:22 -0800 Subject: [PATCH 2/4] More work on Structure Scan rewrite --- ChangeLog.md | 2 +- .../StructureScan.SettingsGroup.json | 6 +- src/MissionManager/StructureScanComplexItem.cc | 158 +++++++++++---------- src/MissionManager/StructureScanComplexItem.h | 22 ++- src/MissionManager/StructureScanComplexItemTest.cc | 9 +- src/PlanView/StructureScanEditor.qml | 39 ++--- 6 files changed, 110 insertions(+), 126 deletions(-) diff --git a/ChangeLog.md b/ChangeLog.md index cb09904..de9e9c8 100644 --- a/ChangeLog.md +++ b/ChangeLog.md @@ -6,7 +6,7 @@ Note: This file only contains high level features or important fixes. ### 3.6.0 - Daily Build -* No changes yet +* Major rewrite and bug fix pass through Structure Scan. Previous version had such bad problems that it can no longer be supported. Plans with Structure Scan will need to be recreated. New QGC will not load old Structure Scan plans. ### 3.5.1 - Not yet released * Fix tile set count but in OfflineMaps which would cause image and elevation tile set to have incorrect counts and be incorrectly marked as download incomplete. diff --git a/src/MissionManager/StructureScan.SettingsGroup.json b/src/MissionManager/StructureScan.SettingsGroup.json index 12331b5..8f3d1b2 100644 --- a/src/MissionManager/StructureScan.SettingsGroup.json +++ b/src/MissionManager/StructureScan.SettingsGroup.json @@ -18,8 +18,8 @@ "defaultValue": 50 }, { - "name": "Altitude", - "shortDescription": "Altitude for the bottom layer of the structure scan.", + "name": "ScanBottomAlt", + "shortDescription": "Altitude for the bottomost covered area of the scan. You can adjust this value such that the Bottom Layer Alt will fly above obstacles on the ground.", "type": "double", "units": "m", "decimalPlaces": 1, @@ -43,7 +43,7 @@ }, { "name": "StartFromTop", - "shortDescription": "Start scan from top of structure.", + "shortDescription": "Start scanning from top of structure.", "type": "bool", "defaultValue": true } diff --git a/src/MissionManager/StructureScanComplexItem.cc b/src/MissionManager/StructureScanComplexItem.cc index 651f471..c22f386 100644 --- a/src/MissionManager/StructureScanComplexItem.cc +++ b/src/MissionManager/StructureScanComplexItem.cc @@ -23,7 +23,7 @@ QGC_LOGGING_CATEGORY(StructureScanComplexItemLog, "StructureScanComplexItemLog") const char* StructureScanComplexItem::settingsGroup = "StructureScan"; const char* StructureScanComplexItem::_entranceAltName = "EntranceAltitude"; -const char* StructureScanComplexItem::scanBottomAltName = "Altitude"; +const char* StructureScanComplexItem::scanBottomAltName = "ScanBottomAlt"; const char* StructureScanComplexItem::structureHeightName = "StructureHeight"; const char* StructureScanComplexItem::layersName = "Layers"; const char* StructureScanComplexItem::gimbalPitchName = "GimbalPitch"; @@ -31,14 +31,11 @@ const char* StructureScanComplexItem::startFromTopName = "StartFromTo const char* StructureScanComplexItem::jsonComplexItemTypeValue = "StructureScan"; const char* StructureScanComplexItem::_jsonCameraCalcKey = "CameraCalc"; -const char* StructureScanComplexItem::_jsonAltitudeRelativeKey = "altitudeRelative"; StructureScanComplexItem::StructureScanComplexItem(Vehicle* vehicle, bool flyView, const QString& kmlOrShpFile, QObject* parent) : ComplexMissionItem (vehicle, flyView, parent) , _metaDataMap (FactMetaData::createMapFromJsonFile(QStringLiteral(":/json/StructureScan.SettingsGroup.json"), this /* QObject parent */)) , _sequenceNumber (0) - , _dirty (false) - , _altitudeRelative (true) , _entryVertex (0) , _ignoreRecalc (false) , _scanDistance (0.0) @@ -61,14 +58,13 @@ StructureScanComplexItem::StructureScanComplexItem(Vehicle* vehicle, bool flyVie connect(&_gimbalPitchFact, &Fact::valueChanged, this, &StructureScanComplexItem::_setDirty); connect(&_startFromTopFact, &Fact::valueChanged, this, &StructureScanComplexItem::_setDirty); - connect(&_layersFact, &Fact::valueChanged, this, &StructureScanComplexItem::_recalcLayerInfo); + connect(&_startFromTopFact, &Fact::valueChanged, this, &StructureScanComplexItem::_signalTopBottomAltChanged); + connect(&_layersFact, &Fact::valueChanged, this, &StructureScanComplexItem::_signalTopBottomAltChanged); + connect(&_structureHeightFact, &Fact::valueChanged, this, &StructureScanComplexItem::_recalcLayerInfo); + connect(&_scanBottomAltFact, &Fact::valueChanged, this, &StructureScanComplexItem::_recalcLayerInfo); connect(_cameraCalc.adjustedFootprintFrontal(), &Fact::valueChanged, this, &StructureScanComplexItem::_recalcLayerInfo); - connect(this, &StructureScanComplexItem::altitudeRelativeChanged, this, &StructureScanComplexItem::_setDirty); - connect(this, &StructureScanComplexItem::altitudeRelativeChanged, this, &StructureScanComplexItem::coordinateHasRelativeAltitudeChanged); - connect(this, &StructureScanComplexItem::altitudeRelativeChanged, this, &StructureScanComplexItem::exitCoordinateHasRelativeAltitudeChanged); - connect(&_entranceAltFact, &Fact::valueChanged, this, &StructureScanComplexItem::_updateCoordinateAltitudes); connect(&_structurePolygon, &QGCMapPolygon::dirtyChanged, this, &StructureScanComplexItem::_polygonDirtyChanged); @@ -78,7 +74,6 @@ StructureScanComplexItem::StructureScanComplexItem(Vehicle* vehicle, bool flyVie connect(&_layersFact, &Fact::valueChanged, this, &StructureScanComplexItem::_updateLastSequenceNumber); connect(&_flightPolygon, &QGCMapPolygon::pathChanged, this, &StructureScanComplexItem::_flightPathChanged); - connect(&_flightPolygon, &QGCMapPolygon::countChanged, this, &StructureScanComplexItem::_validateEntryVertex); connect(_cameraCalc.distanceToSurface(), &Fact::valueChanged, this, &StructureScanComplexItem::_rebuildFlightPolygon); @@ -157,17 +152,16 @@ void StructureScanComplexItem::save(QJsonArray& missionItems) QJsonObject saveObject; // Header - saveObject[JsonHelper::jsonVersionKey] = 2; + saveObject[JsonHelper::jsonVersionKey] = 3; saveObject[VisualMissionItem::jsonTypeKey] = VisualMissionItem::jsonTypeComplexItemValue; saveObject[ComplexMissionItem::jsonComplexItemTypeKey] = jsonComplexItemTypeValue; - saveObject[_entranceAltName] = _entranceAltFact.rawValue().toDouble(); - saveObject[scanBottomAltName] = _scanBottomAltFact.rawValue().toDouble(); - saveObject[structureHeightName] = _structureHeightFact.rawValue().toDouble(); - saveObject[_jsonAltitudeRelativeKey] = _altitudeRelative; - saveObject[layersName] = _layersFact.rawValue().toDouble(); - saveObject[gimbalPitchName] = _gimbalPitchFact.rawValue().toDouble(); - saveObject[startFromTopName] = _startFromTopFact.rawValue().toBool(); + saveObject[_entranceAltName] = _entranceAltFact.rawValue().toDouble(); + saveObject[scanBottomAltName] = _scanBottomAltFact.rawValue().toDouble(); + saveObject[structureHeightName] = _structureHeightFact.rawValue().toDouble(); + saveObject[layersName] = _layersFact.rawValue().toDouble(); + saveObject[gimbalPitchName] = _gimbalPitchFact.rawValue().toDouble(); + saveObject[startFromTopName] = _startFromTopFact.rawValue().toBool(); QJsonObject cameraCalcObject; _cameraCalc.save(cameraCalcObject); @@ -196,13 +190,11 @@ bool StructureScanComplexItem::load(const QJsonObject& complexObject, int sequen { QGCMapPolygon::jsonPolygonKey, QJsonValue::Array, true }, { scanBottomAltName, QJsonValue::Double, true }, { structureHeightName, QJsonValue::Double, true }, - { _jsonAltitudeRelativeKey, QJsonValue::Bool, true }, { layersName, QJsonValue::Double, true }, { _jsonCameraCalcKey, QJsonValue::Object, true }, - // The following values were added after initial implementation so they may be missing from older files. Hence optional. - { _entranceAltName, QJsonValue::Double, false }, - { gimbalPitchName, QJsonValue::Double, false }, - { startFromTopName, QJsonValue::Bool, false }, + { _entranceAltName, QJsonValue::Double, true }, + { gimbalPitchName, QJsonValue::Double, true }, + { startFromTopName, QJsonValue::Bool, true }, }; if (!JsonHelper::validateKeys(complexObject, keyInfoList, errorString)) { return false; @@ -218,8 +210,8 @@ bool StructureScanComplexItem::load(const QJsonObject& complexObject, int sequen } int version = complexObject[JsonHelper::jsonVersionKey].toInt(); - if (version != 2) { - errorString = tr("%1 complex item version %2 not supported").arg(jsonComplexItemTypeValue).arg(version); + if (version != 3) { + errorString = tr("%1 version %2 not supported").arg(jsonComplexItemTypeValue).arg(version); return false; } @@ -230,25 +222,12 @@ bool StructureScanComplexItem::load(const QJsonObject& complexObject, int sequen return false; } - _scanBottomAltFact.setRawValue (complexObject[scanBottomAltName].toDouble()); - _layersFact.setRawValue (complexObject[layersName].toDouble()); - _structureHeightFact.setRawValue(complexObject[structureHeightName].toDouble()); - - // Start from top is a new setting which may not exist in older saved structure scans. - // Default to false if doesn't exist, which matches previous functionality. - _startFromTopFact.setRawValue (complexObject[startFromTopName].toBool(false)); - - // Entrance altitude is a new setting which may not exist in older saved structure scans. - // We default it to the top of the structure which is the safest thing to do without a value. - _entranceAltFact.setRawValue (complexObject[startFromTopName].toDouble(_scanBottomAltFact.rawValue().toDouble() + _structureHeightFact.rawValue().toDouble())); - - _altitudeRelative = complexObject[_jsonAltitudeRelativeKey].toBool(true); - - double gimbalPitchValue = 0; - if (complexObject.contains(gimbalPitchName)) { - gimbalPitchValue = complexObject[gimbalPitchName].toDouble(); - } - _gimbalPitchFact.setRawValue(gimbalPitchValue); + _scanBottomAltFact.setRawValue (complexObject[scanBottomAltName].toDouble()); + _layersFact.setRawValue (complexObject[layersName].toDouble()); + _structureHeightFact.setRawValue (complexObject[structureHeightName].toDouble()); + _startFromTopFact.setRawValue (complexObject[startFromTopName].toBool()); + _entranceAltFact.setRawValue (complexObject[startFromTopName].toDouble()); + _gimbalPitchFact.setRawValue (complexObject[gimbalPitchName].toDouble()); if (!_structurePolygon.loadFromJson(complexObject, true /* required */, errorString)) { _structurePolygon.clear(); @@ -322,7 +301,7 @@ void StructureScanComplexItem::appendMissionItems(QList& items, QO QGeoCoordinate entranceExitCoord = _flightPolygon.vertexCoordinate(_entryVertex); item = new MissionItem(seqNum++, MAV_CMD_NAV_WAYPOINT, - _altitudeRelative ? MAV_FRAME_GLOBAL_RELATIVE_ALT : MAV_FRAME_GLOBAL, + MAV_FRAME_GLOBAL_RELATIVE_ALT, 0, // No hold time 0.0, // No acceptance radius specified 0.0, // Pass through waypoint @@ -351,13 +330,14 @@ void StructureScanComplexItem::appendMissionItems(QList& items, QO // Fly a layer pattern for (int layer=0; layer<_layersFact.rawValue().toInt(); layer++) { bool addTriggerStart = true; - double layerIncrement = (_cameraCalc.adjustedFootprintFrontal()->rawValue().toDouble() / 2.0) + (layer * _cameraCalc.adjustedFootprintFrontal()->rawValue().toDouble()); - double layerAltitude; + double halfLayerHeight = _cameraCalc.adjustedFootprintFrontal()->rawValue().toDouble() / 2.0; + double layerAltitude = startAltitude; + // Move down to the middle of the layer if (startFromTop) { - layerAltitude = startAltitude - layerIncrement; + layerAltitude -= halfLayerHeight; } else { - layerAltitude = startAltitude + layerIncrement; + layerAltitude += halfLayerHeight; } bool done = false; @@ -368,7 +348,7 @@ void StructureScanComplexItem::appendMissionItems(QList& items, QO item = new MissionItem(seqNum++, MAV_CMD_NAV_WAYPOINT, - _altitudeRelative ? MAV_FRAME_GLOBAL_RELATIVE_ALT : MAV_FRAME_GLOBAL, + MAV_FRAME_GLOBAL_RELATIVE_ALT, 0, // No hold time 0.0, // No acceptance radius specified 0.0, // Pass through waypoint @@ -420,6 +400,13 @@ void StructureScanComplexItem::appendMissionItems(QList& items, QO false, // isCurrentItem missionItemParent); items.append(item); + + // Move to next layer altitude + if (startFromTop) { + layerAltitude -= halfLayerHeight * 2; + } else { + layerAltitude += halfLayerHeight * 2; + } } // Return camera to neutral position @@ -435,7 +422,7 @@ void StructureScanComplexItem::appendMissionItems(QList& items, QO // Exit waypoint item = new MissionItem(seqNum++, MAV_CMD_NAV_WAYPOINT, - _altitudeRelative ? MAV_FRAME_GLOBAL_RELATIVE_ALT : MAV_FRAME_GLOBAL, + MAV_FRAME_GLOBAL_RELATIVE_ALT, 0, // No hold time 0.0, // No acceptance radius specified 0.0, // Pass through waypoint @@ -520,8 +507,21 @@ void StructureScanComplexItem::rotateEntryPoint(void) void StructureScanComplexItem::_rebuildFlightPolygon(void) { + // While this is happening all hell breaks loose signal-wise which can cause a bad vertex reference. + // So we reset to a safe value first and then double check validity when putting it back + int savedEntryVertex = _entryVertex; + _entryVertex = 0; + _flightPolygon = _structurePolygon; _flightPolygon.offset(_cameraCalc.distanceToSurface()->rawValue().toDouble()); + + if (savedEntryVertex >= _flightPolygon.count()) { + _entryVertex = 0; + } else { + _entryVertex = savedEntryVertex; + } + emit coordinateChanged(coordinate()); + emit exitCoordinateChanged(exitCoordinate()); } void StructureScanComplexItem::_recalcCameraShots(void) @@ -553,43 +553,49 @@ void StructureScanComplexItem::_recalcCameraShots(void) _setCameraShots(cameraShots * _layersFact.rawValue().toInt()); } -void StructureScanComplexItem::setAltitudeRelative(bool altitudeRelative) +void StructureScanComplexItem::_recalcLayerInfo(void) { - if (altitudeRelative != _altitudeRelative) { - _altitudeRelative = altitudeRelative; - emit altitudeRelativeChanged(altitudeRelative); + double surfaceHeight = qMax(_structureHeightFact.rawValue().toDouble() - _scanBottomAltFact.rawValue().toDouble(), 0.0); + + // Layer count is calculated from surface and layer heights + _layersFact.setRawValue(qMax(qCeil(surfaceHeight / _cameraCalc.adjustedFootprintFrontal()->rawValue().toDouble()), 1)); +} + +void StructureScanComplexItem::_updateGimbalPitch(void) +{ + if (!_cameraCalc.isManualCamera()) { + _gimbalPitchFact.setRawValue(0); } } -void StructureScanComplexItem::_recalcLayerInfo(void) +double StructureScanComplexItem::bottomFlightAlt(void) { - if (_cameraCalc.isManualCamera()) { - // Structure height is calculated from layer count, layer height. - _structureHeightFact.setSendValueChangedSignals(false); - _structureHeightFact.setRawValue(_layersFact.rawValue().toInt() * _cameraCalc.adjustedFootprintFrontal()->rawValue().toDouble()); - _structureHeightFact.clearDeferredValueChangeSignal(); - _structureHeightFact.setSendValueChangedSignals(true); + if (_startFromTopFact.rawValue().toBool()) { + // Structure Height minus the topmost layers + double layerIncrement = (_cameraCalc.adjustedFootprintFrontal()->rawValue().toDouble() / 2.0) + ((_layersFact.rawValue().toInt() - 1) * _cameraCalc.adjustedFootprintFrontal()->rawValue().toDouble()); + return _structureHeightFact.rawValue().toDouble() - layerIncrement; } else { - // Layer count is calculated from structure and layer heights - _layersFact.setSendValueChangedSignals(false); - _layersFact.setRawValue(qCeil(_structureHeightFact.rawValue().toDouble() / _cameraCalc.adjustedFootprintFrontal()->rawValue().toDouble())); - _layersFact.clearDeferredValueChangeSignal(); - _layersFact.setSendValueChangedSignals(true); + // Bottom alt plus half the height of a layer + double layerIncrement = _cameraCalc.adjustedFootprintFrontal()->rawValue().toDouble() / 2.0; + return _scanBottomAltFact.rawValue().toDouble() + layerIncrement; } } -void StructureScanComplexItem::_updateGimbalPitch(void) +double StructureScanComplexItem:: topFlightAlt(void) { - if (!_cameraCalc.isManualCamera()) { - _gimbalPitchFact.setRawValue(0); + if (_startFromTopFact.rawValue().toBool()) { + // Structure Height minus half the layer height + double layerIncrement = _cameraCalc.adjustedFootprintFrontal()->rawValue().toDouble() / 2.0; + return _structureHeightFact.rawValue().toDouble() - layerIncrement; + } else { + // Bottom alt plus all layers + double layerIncrement = (_cameraCalc.adjustedFootprintFrontal()->rawValue().toDouble() / 2.0) + ((_layersFact.rawValue().toInt() - 1) * _cameraCalc.adjustedFootprintFrontal()->rawValue().toDouble()); + return _scanBottomAltFact.rawValue().toDouble() + layerIncrement; } } -void StructureScanComplexItem::_validateEntryVertex(void) +void StructureScanComplexItem::_signalTopBottomAltChanged(void) { - if (_entryVertex >= _flightPolygon.count()) { - _entryVertex = 0; - emit coordinateChanged(coordinate()); - emit exitCoordinateChanged(exitCoordinate()); - } + emit topFlightAltChanged(); + emit bottomFlightAltChanged(); } diff --git a/src/MissionManager/StructureScanComplexItem.h b/src/MissionManager/StructureScanComplexItem.h index 8905f6c..ce0dcb9 100644 --- a/src/MissionManager/StructureScanComplexItem.h +++ b/src/MissionManager/StructureScanComplexItem.h @@ -32,12 +32,13 @@ public: Q_PROPERTY(CameraCalc* cameraCalc READ cameraCalc CONSTANT) Q_PROPERTY(Fact* entranceAlt READ entranceAlt CONSTANT) - Q_PROPERTY(Fact* scanBottomAlt READ scanBottomAlt CONSTANT) Q_PROPERTY(Fact* structureHeight READ structureHeight CONSTANT) + Q_PROPERTY(Fact* scanBottomAlt READ scanBottomAlt CONSTANT) Q_PROPERTY(Fact* layers READ layers CONSTANT) Q_PROPERTY(Fact* gimbalPitch READ gimbalPitch CONSTANT) Q_PROPERTY(Fact* startFromTop READ startFromTop CONSTANT) - Q_PROPERTY(bool altitudeRelative READ altitudeRelative WRITE setAltitudeRelative NOTIFY altitudeRelativeChanged) + Q_PROPERTY(double bottomFlightAlt READ bottomFlightAlt NOTIFY bottomFlightAltChanged) + Q_PROPERTY(double topFlightAlt READ topFlightAlt NOTIFY topFlightAltChanged) Q_PROPERTY(int cameraShots READ cameraShots NOTIFY cameraShotsChanged) Q_PROPERTY(double timeBetweenShots READ timeBetweenShots NOTIFY timeBetweenShotsChanged) Q_PROPERTY(QGCMapPolygon* structurePolygon READ structurePolygon CONSTANT) @@ -51,14 +52,13 @@ public: Fact* gimbalPitch (void) { return &_gimbalPitchFact; } Fact* startFromTop (void) { return &_startFromTopFact; } - bool altitudeRelative (void) const { return _altitudeRelative; } + double bottomFlightAlt (void); + double topFlightAlt (void); int cameraShots (void) const; double timeBetweenShots (void); QGCMapPolygon* structurePolygon (void) { return &_structurePolygon; } QGCMapPolygon* flightPolygon (void) { return &_flightPolygon; } - void setAltitudeRelative (bool altitudeRelative); - Q_INVOKABLE void rotateEntryPoint(void); // Overrides from ComplexMissionItem @@ -90,8 +90,8 @@ public: void applyNewAltitude (double newAltitude) final; double additionalTimeDelay (void) const final { return 0; } - bool coordinateHasRelativeAltitude (void) const final { return _altitudeRelative; } - bool exitCoordinateHasRelativeAltitude (void) const final { return _altitudeRelative; } + bool coordinateHasRelativeAltitude (void) const final { return true; } + bool exitCoordinateHasRelativeAltitude (void) const final { return true; } bool exitCoordinateSameAsEntry (void) const final { return true; } void setDirty (bool dirty) final; @@ -111,7 +111,8 @@ public: signals: void cameraShotsChanged (int cameraShots); void timeBetweenShotsChanged (void); - void altitudeRelativeChanged (bool altitudeRelative); + void bottomFlightAltChanged (void); + void topFlightAltChanged (void); private slots: void _setDirty(void); @@ -124,7 +125,7 @@ private slots: void _recalcLayerInfo (void); void _updateLastSequenceNumber (void); void _updateGimbalPitch (void); - void _validateEntryVertex (void); + void _signalTopBottomAltChanged (void); private: void _setExitCoordinate(const QGeoCoordinate& coordinate); @@ -135,10 +136,8 @@ private: QMap _metaDataMap; int _sequenceNumber; - bool _dirty; QGCMapPolygon _structurePolygon; QGCMapPolygon _flightPolygon; - bool _altitudeRelative; int _entryVertex; // Polygon vertext which is used as the mission entry point bool _ignoreRecalc; @@ -157,7 +156,6 @@ private: SettingsFact _entranceAltFact; static const char* _jsonCameraCalcKey; - static const char* _jsonAltitudeRelativeKey; static const char* _entranceAltName; // This value cannot be overriden diff --git a/src/MissionManager/StructureScanComplexItemTest.cc b/src/MissionManager/StructureScanComplexItemTest.cc index 66f1c39..22882e9 100644 --- a/src/MissionManager/StructureScanComplexItemTest.cc +++ b/src/MissionManager/StructureScanComplexItemTest.cc @@ -11,7 +11,7 @@ #include "QGCApplication.h" StructureScanComplexItemTest::StructureScanComplexItemTest(void) - : _offlineVehicle(NULL) + : _offlineVehicle(nullptr) { _polyPoints << QGeoCoordinate(47.633550640000003, -122.08982199) << QGeoCoordinate(47.634129020000003, -122.08887249) << QGeoCoordinate(47.633619320000001, -122.08811074) << QGeoCoordinate(47.633189139999999, -122.08900124); @@ -74,13 +74,6 @@ void StructureScanComplexItemTest::_testDirty(void) _multiSpy->clearAllSignals(); } rgFacts.clear(); - - QVERIFY(!_structureScanItem->dirty()); - _structureScanItem->setAltitudeRelative(!_structureScanItem->altitudeRelative()); - QVERIFY(_multiSpy->checkSignalByMask(dirtyChangedMask)); - QVERIFY(_multiSpy->pullBoolFromSignalIndex(dirtyChangedIndex)); - _structureScanItem->setDirty(false); - _multiSpy->clearAllSignals(); } void StructureScanComplexItemTest::_initItem(void) diff --git a/src/PlanView/StructureScanEditor.qml b/src/PlanView/StructureScanEditor.qml index 8ce9f78..eaf3852 100644 --- a/src/PlanView/StructureScanEditor.qml +++ b/src/PlanView/StructureScanEditor.qml @@ -110,38 +110,26 @@ Rectangle { QGCLabel { text: qsTr("Structure height") - visible: !missionItem.cameraCalc.isManualCamera } FactTextField { fact: missionItem.structureHeight Layout.fillWidth: true - visible: !missionItem.cameraCalc.isManualCamera } - QGCLabel { - text: qsTr("# Layers") - visible: missionItem.cameraCalc.isManualCamera - } - FactTextField { - fact: missionItem.layers - Layout.fillWidth: true - visible: missionItem.cameraCalc.isManualCamera - } - - QGCLabel { text: qsTr("Scan bottom alt") } + QGCLabel { text: qsTr("Scan Bottom Alt") } FactTextField { fact: missionItem.scanBottomAlt Layout.fillWidth: true } - QGCLabel { text: qsTr("Entrance/Exit alt") } + QGCLabel { text: qsTr("Entrance/Exit Alt") } FactTextField { fact: missionItem.entranceAlt Layout.fillWidth: true } QGCLabel { - text: qsTr("Gimbal pitch") + text: qsTr("Gimbal Pitch") visible: missionItem.cameraCalc.isManualCamera } FactTextField { @@ -149,13 +137,6 @@ Rectangle { Layout.fillWidth: true visible: missionItem.cameraCalc.isManualCamera } - - QGCCheckBox { - text: qsTr("Relative altitude") - checked: missionItem.altitudeRelative - Layout.columnSpan: 2 - onClicked: missionItem.altitudeRelative = checked - } } Item { @@ -182,16 +163,22 @@ Rectangle { QGCLabel { text: qsTr("Layers") } QGCLabel { text: missionItem.layers.valueString } - QGCLabel { text: qsTr("Layer height") } + QGCLabel { text: qsTr("Layer Height") } QGCLabel { text: missionItem.cameraCalc.adjustedFootprintFrontal.valueString + " " + QGroundControl.appSettingsDistanceUnitsString } - QGCLabel { text: qsTr("Photo count") } + QGCLabel { text: qsTr("Top Layer Alt") } + QGCLabel { text: QGroundControl.metersToAppSettingsDistanceUnits(missionItem.topFlightAlt).toFixed(1) + " " + QGroundControl.appSettingsDistanceUnitsString } + + QGCLabel { text: qsTr("Bottom Layer Alt") } + QGCLabel { text: QGroundControl.metersToAppSettingsDistanceUnits(missionItem.bottomFlightAlt).toFixed(1) + " " + QGroundControl.appSettingsDistanceUnitsString } + + QGCLabel { text: qsTr("Photo Count") } QGCLabel { text: missionItem.cameraShots } - QGCLabel { text: qsTr("Photo interval") } + QGCLabel { text: qsTr("Photo Interval") } QGCLabel { text: missionItem.timeBetweenShots.toFixed(1) + " " + qsTr("secs") } - QGCLabel { text: qsTr("Trigger distance") } + QGCLabel { text: qsTr("Trigger Distance") } QGCLabel { text: missionItem.cameraCalc.adjustedFootprintSide.valueString + " " + QGroundControl.appSettingsDistanceUnitsString } } } // Column From b92ffa74986a97bcc57dbe9540e0d8698a6335c5 Mon Sep 17 00:00:00 2001 From: Don Gagne Date: Sun, 17 Feb 2019 14:46:43 -0800 Subject: [PATCH 3/4] Fix layer heights --- src/MissionManager/StructureScanComplexItem.cc | 21 ++++++++++----------- 1 file changed, 10 insertions(+), 11 deletions(-) diff --git a/src/MissionManager/StructureScanComplexItem.cc b/src/MissionManager/StructureScanComplexItem.cc index c22f386..4544f2b 100644 --- a/src/MissionManager/StructureScanComplexItem.cc +++ b/src/MissionManager/StructureScanComplexItem.cc @@ -327,18 +327,17 @@ void StructureScanComplexItem::appendMissionItems(QList& items, QO missionItemParent); items.append(item); - // Fly a layer pattern - for (int layer=0; layer<_layersFact.rawValue().toInt(); layer++) { - bool addTriggerStart = true; - double halfLayerHeight = _cameraCalc.adjustedFootprintFrontal()->rawValue().toDouble() / 2.0; - double layerAltitude = startAltitude; + // Set up for the first layer + double layerAltitude = startAltitude; + double halfLayerHeight = _cameraCalc.adjustedFootprintFrontal()->rawValue().toDouble() / 2.0; + if (startFromTop) { + layerAltitude -= halfLayerHeight; + } else { + layerAltitude += halfLayerHeight; + } - // Move down to the middle of the layer - if (startFromTop) { - layerAltitude -= halfLayerHeight; - } else { - layerAltitude += halfLayerHeight; - } + for (int layer=0; layer<_layersFact.rawValue().toInt(); layer++) { + bool addTriggerStart = true; bool done = false; int currentVertex = _entryVertex; From 92e497da33723255c3128f1043819facb20e52dc Mon Sep 17 00:00:00 2001 From: Don Gagne Date: Mon, 18 Feb 2019 12:14:10 -0800 Subject: [PATCH 4/4] Fix capatilization --- src/PlanView/StructureScanEditor.qml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/PlanView/StructureScanEditor.qml b/src/PlanView/StructureScanEditor.qml index eaf3852..380ef8f 100644 --- a/src/PlanView/StructureScanEditor.qml +++ b/src/PlanView/StructureScanEditor.qml @@ -109,7 +109,7 @@ Rectangle { } QGCLabel { - text: qsTr("Structure height") + text: qsTr("Structure Height") } FactTextField { fact: missionItem.structureHeight