From 61bdd7f2eb0629537755492cee8c405360a88568 Mon Sep 17 00:00:00 2001 From: Don Gagne Date: Sun, 17 Feb 2019 12:38:19 -0800 Subject: [PATCH] Bug fixes for Structure Scan. Also includes support for Entrance/Exit Alt. --- .../StructureScan.SettingsGroup.json | 8 + .../StructureScanComplexItem.cc | 192 ++++++++++++------ src/MissionManager/StructureScanComplexItem.h | 14 +- .../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 ccfdf63c7..12331b531 100644 --- a/src/MissionManager/StructureScan.SettingsGroup.json +++ b/src/MissionManager/StructureScan.SettingsGroup.json @@ -9,6 +9,14 @@ "decimalPlaces": 0, "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.", diff --git a/src/MissionManager/StructureScanComplexItem.cc b/src/MissionManager/StructureScanComplexItem.cc index 629842eef..651f47173 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 810a6dae0..8905f6c4c 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 17fc257bd..66f1c39d4 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 827a6a5df..8ce9f789a 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 } -- 2.22.0