Unverified Commit b91e9897 authored by Don Gagne's avatar Don Gagne Committed by GitHub

Merge pull request #7239 from DonLakeFlyer/StructureScanFixes

Structure scan fixes (V2)
parents a40661ca 92e497da
...@@ -6,7 +6,7 @@ Note: This file only contains high level features or important fixes. ...@@ -6,7 +6,7 @@ Note: This file only contains high level features or important fixes.
### 3.6.0 - Daily Build ### 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 ### 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. * 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.
......
...@@ -10,8 +10,16 @@ ...@@ -10,8 +10,16 @@
"defaultValue": 0 "defaultValue": 0
}, },
{ {
"name": "Altitude", "name": "EntranceAltitude",
"shortDescription": "Altitude for the bottom layer of the structure scan.", "shortDescription": "Vehicle will fly to/from the structure at this altitude.",
"type": "double",
"units": "m",
"decimalPlaces": 1,
"defaultValue": 50
},
{
"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", "type": "double",
"units": "m", "units": "m",
"decimalPlaces": 1, "decimalPlaces": 1,
...@@ -35,7 +43,7 @@ ...@@ -35,7 +43,7 @@
}, },
{ {
"name": "StartFromTop", "name": "StartFromTop",
"shortDescription": "Start scan from top of structure.", "shortDescription": "Start scanning from top of structure.",
"type": "bool", "type": "bool",
"defaultValue": true "defaultValue": true
} }
......
...@@ -22,7 +22,8 @@ ...@@ -22,7 +22,8 @@
QGC_LOGGING_CATEGORY(StructureScanComplexItemLog, "StructureScanComplexItemLog") QGC_LOGGING_CATEGORY(StructureScanComplexItemLog, "StructureScanComplexItemLog")
const char* StructureScanComplexItem::settingsGroup = "StructureScan"; const char* StructureScanComplexItem::settingsGroup = "StructureScan";
const char* StructureScanComplexItem::altitudeName = "Altitude"; const char* StructureScanComplexItem::_entranceAltName = "EntranceAltitude";
const char* StructureScanComplexItem::scanBottomAltName = "ScanBottomAlt";
const char* StructureScanComplexItem::structureHeightName = "StructureHeight"; const char* StructureScanComplexItem::structureHeightName = "StructureHeight";
const char* StructureScanComplexItem::layersName = "Layers"; const char* StructureScanComplexItem::layersName = "Layers";
const char* StructureScanComplexItem::gimbalPitchName = "GimbalPitch"; const char* StructureScanComplexItem::gimbalPitchName = "GimbalPitch";
...@@ -30,43 +31,41 @@ const char* StructureScanComplexItem::startFromTopName = "StartFromTo ...@@ -30,43 +31,41 @@ const char* StructureScanComplexItem::startFromTopName = "StartFromTo
const char* StructureScanComplexItem::jsonComplexItemTypeValue = "StructureScan"; const char* StructureScanComplexItem::jsonComplexItemTypeValue = "StructureScan";
const char* StructureScanComplexItem::_jsonCameraCalcKey = "CameraCalc"; const char* StructureScanComplexItem::_jsonCameraCalcKey = "CameraCalc";
const char* StructureScanComplexItem::_jsonAltitudeRelativeKey = "altitudeRelative";
StructureScanComplexItem::StructureScanComplexItem(Vehicle* vehicle, bool flyView, const QString& kmlOrShpFile, QObject* parent) StructureScanComplexItem::StructureScanComplexItem(Vehicle* vehicle, bool flyView, const QString& kmlOrShpFile, QObject* parent)
: ComplexMissionItem (vehicle, flyView, parent) : ComplexMissionItem (vehicle, flyView, parent)
, _metaDataMap (FactMetaData::createMapFromJsonFile(QStringLiteral(":/json/StructureScan.SettingsGroup.json"), this /* QObject parent */)) , _metaDataMap (FactMetaData::createMapFromJsonFile(QStringLiteral(":/json/StructureScan.SettingsGroup.json"), this /* QObject parent */))
, _sequenceNumber (0) , _sequenceNumber (0)
, _dirty (false)
, _altitudeRelative (true)
, _entryVertex (0) , _entryVertex (0)
, _ignoreRecalc (false) , _ignoreRecalc (false)
, _scanDistance (0.0) , _scanDistance (0.0)
, _cameraShots (0) , _cameraShots (0)
, _cameraCalc (vehicle, settingsGroup) , _cameraCalc (vehicle, settingsGroup)
, _altitudeFact (settingsGroup, _metaDataMap[altitudeName]) , _scanBottomAltFact (settingsGroup, _metaDataMap[scanBottomAltName])
, _structureHeightFact (settingsGroup, _metaDataMap[structureHeightName]) , _structureHeightFact (settingsGroup, _metaDataMap[structureHeightName])
, _layersFact (settingsGroup, _metaDataMap[layersName]) , _layersFact (settingsGroup, _metaDataMap[layersName])
, _gimbalPitchFact (settingsGroup, _metaDataMap[gimbalPitchName]) , _gimbalPitchFact (settingsGroup, _metaDataMap[gimbalPitchName])
, _startFromTopFact (settingsGroup, _metaDataMap[startFromTopName]) , _startFromTopFact (settingsGroup, _metaDataMap[startFromTopName])
, _entranceAltFact (settingsGroup, _metaDataMap[_entranceAltName])
{ {
_editorQml = "qrc:/qml/StructureScanEditor.qml"; _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(&_entranceAltFact, &Fact::valueChanged, this, &StructureScanComplexItem::_setDirty);
connect(&_layersFact, &Fact::valueChanged, this, &StructureScanComplexItem::_setDirty); connect(&_scanBottomAltFact, &Fact::valueChanged, this, &StructureScanComplexItem::_setDirty);
connect(&_gimbalPitchFact, &Fact::valueChanged, this, &StructureScanComplexItem::_setDirty); connect(&_layersFact, &Fact::valueChanged, this, &StructureScanComplexItem::_setDirty);
connect(&_startFromTopFact, &Fact::valueChanged, this, &StructureScanComplexItem::_setDirty); connect(&_gimbalPitchFact, &Fact::valueChanged, this, &StructureScanComplexItem::_setDirty);
connect(&_startFromTopFact, &Fact::valueChanged, this, &StructureScanComplexItem::_setDirty);
connect(&_startFromTopFact, &Fact::valueChanged, this, &StructureScanComplexItem::_signalTopBottomAltChanged);
connect(&_layersFact, &Fact::valueChanged, this, &StructureScanComplexItem::_signalTopBottomAltChanged);
connect(&_layersFact, &Fact::valueChanged, this, &StructureScanComplexItem::_recalcLayerInfo);
connect(&_structureHeightFact, &Fact::valueChanged, this, &StructureScanComplexItem::_recalcLayerInfo); connect(&_structureHeightFact, &Fact::valueChanged, this, &StructureScanComplexItem::_recalcLayerInfo);
connect(&_scanBottomAltFact, &Fact::valueChanged, this, &StructureScanComplexItem::_recalcLayerInfo);
connect(_cameraCalc.adjustedFootprintFrontal(), &Fact::valueChanged, this, &StructureScanComplexItem::_recalcLayerInfo); connect(_cameraCalc.adjustedFootprintFrontal(), &Fact::valueChanged, this, &StructureScanComplexItem::_recalcLayerInfo);
connect(this, &StructureScanComplexItem::altitudeRelativeChanged, this, &StructureScanComplexItem::_setDirty); connect(&_entranceAltFact, &Fact::valueChanged, this, &StructureScanComplexItem::_updateCoordinateAltitudes);
connect(this, &StructureScanComplexItem::altitudeRelativeChanged, this, &StructureScanComplexItem::coordinateHasRelativeAltitudeChanged);
connect(this, &StructureScanComplexItem::altitudeRelativeChanged, this, &StructureScanComplexItem::exitCoordinateHasRelativeAltitudeChanged);
connect(&_altitudeFact, &Fact::valueChanged, this, &StructureScanComplexItem::_updateCoordinateAltitudes);
connect(&_structurePolygon, &QGCMapPolygon::dirtyChanged, this, &StructureScanComplexItem::_polygonDirtyChanged); connect(&_structurePolygon, &QGCMapPolygon::dirtyChanged, this, &StructureScanComplexItem::_polygonDirtyChanged);
connect(&_structurePolygon, &QGCMapPolygon::pathChanged, this, &StructureScanComplexItem::_rebuildFlightPolygon); connect(&_structurePolygon, &QGCMapPolygon::pathChanged, this, &StructureScanComplexItem::_rebuildFlightPolygon);
...@@ -130,9 +129,12 @@ int StructureScanComplexItem::lastSequenceNumber(void) const ...@@ -130,9 +129,12 @@ int StructureScanComplexItem::lastSequenceNumber(void) const
// Two commands for camera trigger start/stop // Two commands for camera trigger start/stop
int layerItemCount = _flightPolygon.count() + 1 + 2; int layerItemCount = _flightPolygon.count() + 1 + 2;
// Take into account the number of layers
int multiLayerItemCount = layerItemCount * _layersFact.rawValue().toInt(); 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; return _sequenceNumber + itemCount - 1;
} }
...@@ -150,16 +152,16 @@ void StructureScanComplexItem::save(QJsonArray& missionItems) ...@@ -150,16 +152,16 @@ void StructureScanComplexItem::save(QJsonArray& missionItems)
QJsonObject saveObject; QJsonObject saveObject;
// Header // Header
saveObject[JsonHelper::jsonVersionKey] = 2; saveObject[JsonHelper::jsonVersionKey] = 3;
saveObject[VisualMissionItem::jsonTypeKey] = VisualMissionItem::jsonTypeComplexItemValue; saveObject[VisualMissionItem::jsonTypeKey] = VisualMissionItem::jsonTypeComplexItemValue;
saveObject[ComplexMissionItem::jsonComplexItemTypeKey] = jsonComplexItemTypeValue; saveObject[ComplexMissionItem::jsonComplexItemTypeKey] = jsonComplexItemTypeValue;
saveObject[altitudeName] = _altitudeFact.rawValue().toDouble(); saveObject[_entranceAltName] = _entranceAltFact.rawValue().toDouble();
saveObject[structureHeightName] = _structureHeightFact.rawValue().toDouble(); saveObject[scanBottomAltName] = _scanBottomAltFact.rawValue().toDouble();
saveObject[_jsonAltitudeRelativeKey] = _altitudeRelative; saveObject[structureHeightName] = _structureHeightFact.rawValue().toDouble();
saveObject[layersName] = _layersFact.rawValue().toDouble(); saveObject[layersName] = _layersFact.rawValue().toDouble();
saveObject[gimbalPitchName] = _gimbalPitchFact.rawValue().toDouble(); saveObject[gimbalPitchName] = _gimbalPitchFact.rawValue().toDouble();
saveObject[startFromTopName] = _startFromTopFact.rawValue().toBool(); saveObject[startFromTopName] = _startFromTopFact.rawValue().toBool();
QJsonObject cameraCalcObject; QJsonObject cameraCalcObject;
_cameraCalc.save(cameraCalcObject); _cameraCalc.save(cameraCalcObject);
...@@ -186,13 +188,13 @@ bool StructureScanComplexItem::load(const QJsonObject& complexObject, int sequen ...@@ -186,13 +188,13 @@ bool StructureScanComplexItem::load(const QJsonObject& complexObject, int sequen
{ VisualMissionItem::jsonTypeKey, QJsonValue::String, true }, { VisualMissionItem::jsonTypeKey, QJsonValue::String, true },
{ ComplexMissionItem::jsonComplexItemTypeKey, QJsonValue::String, true }, { ComplexMissionItem::jsonComplexItemTypeKey, QJsonValue::String, true },
{ QGCMapPolygon::jsonPolygonKey, QJsonValue::Array, true }, { QGCMapPolygon::jsonPolygonKey, QJsonValue::Array, true },
{ altitudeName, QJsonValue::Double, true }, { scanBottomAltName, QJsonValue::Double, true },
{ structureHeightName, QJsonValue::Double, true }, { structureHeightName, QJsonValue::Double, true },
{ _jsonAltitudeRelativeKey, QJsonValue::Bool, true },
{ layersName, QJsonValue::Double, 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 }, { _jsonCameraCalcKey, QJsonValue::Object, true },
{ _entranceAltName, QJsonValue::Double, true },
{ gimbalPitchName, QJsonValue::Double, true },
{ startFromTopName, QJsonValue::Bool, true },
}; };
if (!JsonHelper::validateKeys(complexObject, keyInfoList, errorString)) { if (!JsonHelper::validateKeys(complexObject, keyInfoList, errorString)) {
return false; return false;
...@@ -208,8 +210,8 @@ bool StructureScanComplexItem::load(const QJsonObject& complexObject, int sequen ...@@ -208,8 +210,8 @@ bool StructureScanComplexItem::load(const QJsonObject& complexObject, int sequen
} }
int version = complexObject[JsonHelper::jsonVersionKey].toInt(); int version = complexObject[JsonHelper::jsonVersionKey].toInt();
if (version != 2) { if (version != 3) {
errorString = tr("%1 complex item version %2 not supported").arg(jsonComplexItemTypeValue).arg(version); errorString = tr("%1 version %2 not supported").arg(jsonComplexItemTypeValue).arg(version);
return false; return false;
} }
...@@ -220,18 +222,12 @@ bool StructureScanComplexItem::load(const QJsonObject& complexObject, int sequen ...@@ -220,18 +222,12 @@ bool StructureScanComplexItem::load(const QJsonObject& complexObject, int sequen
return false; return false;
} }
_altitudeFact.setRawValue (complexObject[altitudeName].toDouble()); _scanBottomAltFact.setRawValue (complexObject[scanBottomAltName].toDouble());
_layersFact.setRawValue (complexObject[layersName].toDouble()); _layersFact.setRawValue (complexObject[layersName].toDouble());
_structureHeightFact.setRawValue(complexObject[structureHeightName].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 _startFromTopFact.setRawValue (complexObject[startFromTopName].toBool());
_entranceAltFact.setRawValue (complexObject[startFromTopName].toDouble());
_altitudeRelative = complexObject[_jsonAltitudeRelativeKey].toBool(true); _gimbalPitchFact.setRawValue (complexObject[gimbalPitchName].toDouble());
double gimbalPitchValue = 0;
if (complexObject.contains(gimbalPitchName)) {
gimbalPitchValue = complexObject[gimbalPitchName].toDouble();
}
_gimbalPitchFact.setRawValue(gimbalPitchValue);
if (!_structurePolygon.loadFromJson(complexObject, true /* required */, errorString)) { if (!_structurePolygon.loadFromJson(complexObject, true /* required */, errorString)) {
_structurePolygon.clear(); _structurePolygon.clear();
...@@ -297,49 +293,74 @@ void StructureScanComplexItem::appendMissionItems(QList<MissionItem*>& items, QO ...@@ -297,49 +293,74 @@ void StructureScanComplexItem::appendMissionItems(QList<MissionItem*>& items, QO
{ {
int seqNum = _sequenceNumber; int seqNum = _sequenceNumber;
bool startFromTop = _startFromTopFact.rawValue().toBool(); bool startFromTop = _startFromTopFact.rawValue().toBool();
double startAltitude = _altitudeFact.rawValue().toDouble() + (startFromTop ? _structureHeightFact.rawValue().toDouble() : 0); double startAltitude = _scanBottomAltFact.rawValue().toDouble() + (startFromTop ? _structureHeightFact.rawValue().toDouble() : 0);
MissionItem* item = new MissionItem(seqNum++, MissionItem* item = nullptr;
MAV_CMD_DO_SET_ROI_WPNEXT_OFFSET,
MAV_FRAME_MISSION, // Entrance waypoint
0, 0, 0, 0, // param 1-4 not used QGeoCoordinate entranceExitCoord = _flightPolygon.vertexCoordinate(_entryVertex);
_gimbalPitchFact.rawValue().toDouble(), item = new MissionItem(seqNum++,
0, // Roll stays in standard orientation MAV_CMD_NAV_WAYPOINT,
90, // 90 degreee yaw offset to point to structure MAV_FRAME_GLOBAL_RELATIVE_ALT,
true, // autoContinue 0, // No hold time
false, // isCurrentItem 0.0, // No acceptance radius specified
missionItemParent); 0.0, // Pass through waypoint
std::numeric_limits<double>::quiet_NaN(), // Yaw unchanged
entranceExitCoord.latitude(),
entranceExitCoord.longitude(),
_entranceAltFact.rawValue().toDouble(),
true, // autoContinue
false, // isCurrentItem
missionItemParent);
items.append(item); items.append(item);
for (int layer=0; layer<_layersFact.rawValue().toInt(); layer++) { // Point camera at structure
bool addTriggerStart = true; item = new MissionItem(seqNum++,
double layerIncrement = (_cameraCalc.adjustedFootprintFrontal()->rawValue().toDouble() / 2.0) + (layer * _cameraCalc.adjustedFootprintFrontal()->rawValue().toDouble()); MAV_CMD_DO_SET_ROI_WPNEXT_OFFSET,
double layerAltitude; 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);
if (startFromTop) { // Set up for the first layer
layerAltitude = startAltitude - layerIncrement; double layerAltitude = startAltitude;
} else { double halfLayerHeight = _cameraCalc.adjustedFootprintFrontal()->rawValue().toDouble() / 2.0;
layerAltitude = startAltitude + layerIncrement; if (startFromTop) {
} layerAltitude -= halfLayerHeight;
} else {
layerAltitude += halfLayerHeight;
}
for (int i=0; i<_flightPolygon.count(); i++) { for (int layer=0; layer<_layersFact.rawValue().toInt(); layer++) {
QGeoCoordinate vertexCoord = _flightPolygon.vertexCoordinate(i); bool addTriggerStart = true;
MissionItem* item = new MissionItem(seqNum++, bool done = false;
MAV_CMD_NAV_WAYPOINT, int currentVertex = _entryVertex;
_altitudeRelative ? MAV_FRAME_GLOBAL_RELATIVE_ALT : MAV_FRAME_GLOBAL, int processedVertices = 0;
0, // No hold time do {
0.0, // No acceptance radius specified QGeoCoordinate vertexCoord = _flightPolygon.vertexCoordinate(currentVertex);
0.0, // Pass through waypoint
std::numeric_limits<double>::quiet_NaN(), // Yaw unchanged item = new MissionItem(seqNum++,
vertexCoord.latitude(), MAV_CMD_NAV_WAYPOINT,
vertexCoord.longitude(), MAV_FRAME_GLOBAL_RELATIVE_ALT,
layerAltitude, 0, // No hold time
true, // autoContinue 0.0, // No acceptance radius specified
false, // isCurrentItem 0.0, // Pass through waypoint
missionItemParent); std::numeric_limits<double>::quiet_NaN(), // Yaw unchanged
vertexCoord.latitude(),
vertexCoord.longitude(),
layerAltitude,
true, // autoContinue
false, // isCurrentItem
missionItemParent);
items.append(item); items.append(item);
// Start camera triggering after first waypoint in layer
if (addTriggerStart) { if (addTriggerStart) {
addTriggerStart = false; addTriggerStart = false;
item = new MissionItem(seqNum++, item = new MissionItem(seqNum++,
...@@ -354,25 +375,19 @@ void StructureScanComplexItem::appendMissionItems(QList<MissionItem*>& items, QO ...@@ -354,25 +375,19 @@ void StructureScanComplexItem::appendMissionItems(QList<MissionItem*>& items, QO
missionItemParent); missionItemParent);
items.append(item); items.append(item);
} }
}
QGeoCoordinate vertexCoord = _flightPolygon.vertexCoordinate(0); // Move to next vertext
currentVertex++;
MissionItem* item = new MissionItem(seqNum++, if (currentVertex >= _flightPolygon.count()) {
MAV_CMD_NAV_WAYPOINT, currentVertex = 0;
_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<double>::quiet_NaN(), // Yaw unchanged
vertexCoord.latitude(),
vertexCoord.longitude(),
layerAltitude,
true, // autoContinue
false, // isCurrentItem
missionItemParent);
items.append(item);
// 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++, item = new MissionItem(seqNum++,
MAV_CMD_DO_SET_CAM_TRIGG_DIST, MAV_CMD_DO_SET_CAM_TRIGG_DIST,
MAV_FRAME_MISSION, MAV_FRAME_MISSION,
...@@ -384,8 +399,16 @@ void StructureScanComplexItem::appendMissionItems(QList<MissionItem*>& items, QO ...@@ -384,8 +399,16 @@ void StructureScanComplexItem::appendMissionItems(QList<MissionItem*>& items, QO
false, // isCurrentItem false, // isCurrentItem
missionItemParent); missionItemParent);
items.append(item); items.append(item);
// Move to next layer altitude
if (startFromTop) {
layerAltitude -= halfLayerHeight * 2;
} else {
layerAltitude += halfLayerHeight * 2;
}
} }
// Return camera to neutral position
item = new MissionItem(seqNum++, item = new MissionItem(seqNum++,
MAV_CMD_DO_SET_ROI_NONE, MAV_CMD_DO_SET_ROI_NONE,
MAV_FRAME_MISSION, MAV_FRAME_MISSION,
...@@ -394,11 +417,28 @@ void StructureScanComplexItem::appendMissionItems(QList<MissionItem*>& items, QO ...@@ -394,11 +417,28 @@ void StructureScanComplexItem::appendMissionItems(QList<MissionItem*>& items, QO
false, // isCurrentItem false, // isCurrentItem
missionItemParent); missionItemParent);
items.append(item); items.append(item);
// Exit waypoint
item = new MissionItem(seqNum++,
MAV_CMD_NAV_WAYPOINT,
MAV_FRAME_GLOBAL_RELATIVE_ALT,
0, // No hold time
0.0, // No acceptance radius specified
0.0, // Pass through waypoint
std::numeric_limits<double>::quiet_NaN(), // Yaw unchanged
entranceExitCoord.latitude(),
entranceExitCoord.longitude(),
_entranceAltFact.rawValue().toDouble(),
true, // autoContinue
false, // isCurrentItem
missionItemParent);
items.append(item);
} }
int StructureScanComplexItem::cameraShots(void) const int StructureScanComplexItem::cameraShots(void) const
{ {
return true /*_triggerCamera()*/ ? _cameraShots : 0; return _cameraShots;
} }
void StructureScanComplexItem::setMissionFlightStatus(MissionController::MissionFlightStatus_t& missionFlightStatus) void StructureScanComplexItem::setMissionFlightStatus(MissionController::MissionFlightStatus_t& missionFlightStatus)
...@@ -417,7 +457,7 @@ void StructureScanComplexItem::_setDirty(void) ...@@ -417,7 +457,7 @@ void StructureScanComplexItem::_setDirty(void)
void StructureScanComplexItem::applyNewAltitude(double newAltitude) void StructureScanComplexItem::applyNewAltitude(double newAltitude)
{ {
_altitudeFact.setRawValue(newAltitude); _entranceAltFact.setRawValue(newAltitude);
} }
void StructureScanComplexItem::_polygonDirtyChanged(bool dirty) void StructureScanComplexItem::_polygonDirtyChanged(bool dirty)
...@@ -435,8 +475,9 @@ double StructureScanComplexItem::timeBetweenShots(void) ...@@ -435,8 +475,9 @@ double StructureScanComplexItem::timeBetweenShots(void)
QGeoCoordinate StructureScanComplexItem::coordinate(void) const QGeoCoordinate StructureScanComplexItem::coordinate(void) const
{ {
if (_flightPolygon.count() > 0) { if (_flightPolygon.count() > 0) {
int entryVertex = qMax(qMin(_entryVertex, _flightPolygon.count() - 1), 0); QGeoCoordinate coord = _flightPolygon.vertexCoordinate(_entryVertex);
return _flightPolygon.vertexCoordinate(entryVertex); coord.setAltitude(_entranceAltFact.rawValue().toDouble());
return coord;
} else { } else {
return QGeoCoordinate(); return QGeoCoordinate();
} }
...@@ -465,8 +506,21 @@ void StructureScanComplexItem::rotateEntryPoint(void) ...@@ -465,8 +506,21 @@ void StructureScanComplexItem::rotateEntryPoint(void)
void StructureScanComplexItem::_rebuildFlightPolygon(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 = _structurePolygon;
_flightPolygon.offset(_cameraCalc.distanceToSurface()->rawValue().toDouble()); _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) void StructureScanComplexItem::_recalcCameraShots(void)
...@@ -498,34 +552,49 @@ void StructureScanComplexItem::_recalcCameraShots(void) ...@@ -498,34 +552,49 @@ void StructureScanComplexItem::_recalcCameraShots(void)
_setCameraShots(cameraShots * _layersFact.rawValue().toInt()); _setCameraShots(cameraShots * _layersFact.rawValue().toInt());
} }
void StructureScanComplexItem::setAltitudeRelative(bool altitudeRelative) void StructureScanComplexItem::_recalcLayerInfo(void)
{
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 (altitudeRelative != _altitudeRelative) { if (!_cameraCalc.isManualCamera()) {
_altitudeRelative = altitudeRelative; _gimbalPitchFact.setRawValue(0);
emit altitudeRelativeChanged(altitudeRelative);
} }
} }
void StructureScanComplexItem::_recalcLayerInfo(void) double StructureScanComplexItem::bottomFlightAlt(void)
{ {
if (_cameraCalc.isManualCamera()) { if (_startFromTopFact.rawValue().toBool()) {
// Structure height is calculated from layer count, layer height. // Structure Height minus the topmost layers
_structureHeightFact.setSendValueChangedSignals(false); double layerIncrement = (_cameraCalc.adjustedFootprintFrontal()->rawValue().toDouble() / 2.0) + ((_layersFact.rawValue().toInt() - 1) * _cameraCalc.adjustedFootprintFrontal()->rawValue().toDouble());
_structureHeightFact.setRawValue(_layersFact.rawValue().toInt() * _cameraCalc.adjustedFootprintFrontal()->rawValue().toDouble()); return _structureHeightFact.rawValue().toDouble() - layerIncrement;
_structureHeightFact.clearDeferredValueChangeSignal();
_structureHeightFact.setSendValueChangedSignals(true);
} else { } else {
// Layer count is calculated from structure and layer heights // Bottom alt plus half the height of a layer
_layersFact.setSendValueChangedSignals(false); double layerIncrement = _cameraCalc.adjustedFootprintFrontal()->rawValue().toDouble() / 2.0;
_layersFact.setRawValue(qCeil(_structureHeightFact.rawValue().toDouble() / _cameraCalc.adjustedFootprintFrontal()->rawValue().toDouble())); return _scanBottomAltFact.rawValue().toDouble() + layerIncrement;
_layersFact.clearDeferredValueChangeSignal();
_layersFact.setSendValueChangedSignals(true);
} }
} }
void StructureScanComplexItem::_updateGimbalPitch(void) double StructureScanComplexItem:: topFlightAlt(void)
{ {
if (!_cameraCalc.isManualCamera()) { if (_startFromTopFact.rawValue().toBool()) {
_gimbalPitchFact.setRawValue(0); // 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::_signalTopBottomAltChanged(void)
{
emit topFlightAltChanged();
emit bottomFlightAltChanged();
}
...@@ -31,32 +31,34 @@ public: ...@@ -31,32 +31,34 @@ public:
StructureScanComplexItem(Vehicle* vehicle, bool flyView, const QString& kmlOrSHPFile, QObject* parent); StructureScanComplexItem(Vehicle* vehicle, bool flyView, const QString& kmlOrSHPFile, QObject* parent);
Q_PROPERTY(CameraCalc* cameraCalc READ cameraCalc CONSTANT) Q_PROPERTY(CameraCalc* cameraCalc READ cameraCalc CONSTANT)
Q_PROPERTY(Fact* altitude READ altitude CONSTANT) Q_PROPERTY(Fact* entranceAlt READ entranceAlt CONSTANT)
Q_PROPERTY(Fact* structureHeight READ structureHeight 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* layers READ layers CONSTANT)
Q_PROPERTY(Fact* gimbalPitch READ gimbalPitch CONSTANT) Q_PROPERTY(Fact* gimbalPitch READ gimbalPitch CONSTANT)
Q_PROPERTY(Fact* startFromTop READ startFromTop 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(int cameraShots READ cameraShots NOTIFY cameraShotsChanged)
Q_PROPERTY(double timeBetweenShots READ timeBetweenShots NOTIFY timeBetweenShotsChanged) Q_PROPERTY(double timeBetweenShots READ timeBetweenShots NOTIFY timeBetweenShotsChanged)
Q_PROPERTY(QGCMapPolygon* structurePolygon READ structurePolygon CONSTANT) Q_PROPERTY(QGCMapPolygon* structurePolygon READ structurePolygon CONSTANT)
Q_PROPERTY(QGCMapPolygon* flightPolygon READ flightPolygon CONSTANT) Q_PROPERTY(QGCMapPolygon* flightPolygon READ flightPolygon CONSTANT)
CameraCalc* cameraCalc (void) { return &_cameraCalc; } 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* structureHeight (void) { return &_structureHeightFact; }
Fact* layers (void) { return &_layersFact; } Fact* layers (void) { return &_layersFact; }
Fact* gimbalPitch (void) { return &_gimbalPitchFact; } Fact* gimbalPitch (void) { return &_gimbalPitchFact; }
Fact* startFromTop (void) { return &_startFromTopFact; } Fact* startFromTop (void) { return &_startFromTopFact; }
bool altitudeRelative (void) const { return _altitudeRelative; } double bottomFlightAlt (void);
double topFlightAlt (void);
int cameraShots (void) const; int cameraShots (void) const;
double timeBetweenShots (void); double timeBetweenShots (void);
QGCMapPolygon* structurePolygon (void) { return &_structurePolygon; } QGCMapPolygon* structurePolygon (void) { return &_structurePolygon; }
QGCMapPolygon* flightPolygon (void) { return &_flightPolygon; } QGCMapPolygon* flightPolygon (void) { return &_flightPolygon; }
void setAltitudeRelative (bool altitudeRelative);
Q_INVOKABLE void rotateEntryPoint(void); Q_INVOKABLE void rotateEntryPoint(void);
// Overrides from ComplexMissionItem // Overrides from ComplexMissionItem
...@@ -88,8 +90,8 @@ public: ...@@ -88,8 +90,8 @@ public:
void applyNewAltitude (double newAltitude) final; void applyNewAltitude (double newAltitude) final;
double additionalTimeDelay (void) const final { return 0; } double additionalTimeDelay (void) const final { return 0; }
bool coordinateHasRelativeAltitude (void) const final { return _altitudeRelative; } bool coordinateHasRelativeAltitude (void) const final { return true; }
bool exitCoordinateHasRelativeAltitude (void) const final { return _altitudeRelative; } bool exitCoordinateHasRelativeAltitude (void) const final { return true; }
bool exitCoordinateSameAsEntry (void) const final { return true; } bool exitCoordinateSameAsEntry (void) const final { return true; }
void setDirty (bool dirty) final; void setDirty (bool dirty) final;
...@@ -100,7 +102,7 @@ public: ...@@ -100,7 +102,7 @@ public:
static const char* jsonComplexItemTypeValue; static const char* jsonComplexItemTypeValue;
static const char* settingsGroup; static const char* settingsGroup;
static const char* altitudeName; static const char* scanBottomAltName;
static const char* structureHeightName; static const char* structureHeightName;
static const char* layersName; static const char* layersName;
static const char* gimbalPitchName; static const char* gimbalPitchName;
...@@ -109,7 +111,8 @@ public: ...@@ -109,7 +111,8 @@ public:
signals: signals:
void cameraShotsChanged (int cameraShots); void cameraShotsChanged (int cameraShots);
void timeBetweenShotsChanged (void); void timeBetweenShotsChanged (void);
void altitudeRelativeChanged (bool altitudeRelative); void bottomFlightAltChanged (void);
void topFlightAltChanged (void);
private slots: private slots:
void _setDirty(void); void _setDirty(void);
...@@ -122,6 +125,7 @@ private slots: ...@@ -122,6 +125,7 @@ private slots:
void _recalcLayerInfo (void); void _recalcLayerInfo (void);
void _updateLastSequenceNumber (void); void _updateLastSequenceNumber (void);
void _updateGimbalPitch (void); void _updateGimbalPitch (void);
void _signalTopBottomAltChanged (void);
private: private:
void _setExitCoordinate(const QGeoCoordinate& coordinate); void _setExitCoordinate(const QGeoCoordinate& coordinate);
...@@ -132,10 +136,8 @@ private: ...@@ -132,10 +136,8 @@ private:
QMap<QString, FactMetaData*> _metaDataMap; QMap<QString, FactMetaData*> _metaDataMap;
int _sequenceNumber; int _sequenceNumber;
bool _dirty;
QGCMapPolygon _structurePolygon; QGCMapPolygon _structurePolygon;
QGCMapPolygon _flightPolygon; QGCMapPolygon _flightPolygon;
bool _altitudeRelative;
int _entryVertex; // Polygon vertext which is used as the mission entry point int _entryVertex; // Polygon vertext which is used as the mission entry point
bool _ignoreRecalc; bool _ignoreRecalc;
...@@ -146,14 +148,16 @@ private: ...@@ -146,14 +148,16 @@ private:
CameraCalc _cameraCalc; CameraCalc _cameraCalc;
SettingsFact _altitudeFact; SettingsFact _scanBottomAltFact;
SettingsFact _structureHeightFact; SettingsFact _structureHeightFact;
SettingsFact _layersFact; SettingsFact _layersFact;
SettingsFact _gimbalPitchFact; SettingsFact _gimbalPitchFact;
SettingsFact _startFromTopFact; SettingsFact _startFromTopFact;
SettingsFact _entranceAltFact;
static const char* _jsonCameraCalcKey; static const char* _jsonCameraCalcKey;
static const char* _jsonAltitudeRelativeKey;
static const char* _entranceAltName; // This value cannot be overriden
friend class StructureScanComplexItemTest; friend class StructureScanComplexItemTest;
}; };
......
...@@ -11,7 +11,7 @@ ...@@ -11,7 +11,7 @@
#include "QGCApplication.h" #include "QGCApplication.h"
StructureScanComplexItemTest::StructureScanComplexItemTest(void) StructureScanComplexItemTest::StructureScanComplexItemTest(void)
: _offlineVehicle(NULL) : _offlineVehicle(nullptr)
{ {
_polyPoints << QGeoCoordinate(47.633550640000003, -122.08982199) << QGeoCoordinate(47.634129020000003, -122.08887249) << _polyPoints << QGeoCoordinate(47.633550640000003, -122.08982199) << QGeoCoordinate(47.634129020000003, -122.08887249) <<
QGeoCoordinate(47.633619320000001, -122.08811074) << QGeoCoordinate(47.633189139999999, -122.08900124); QGeoCoordinate(47.633619320000001, -122.08811074) << QGeoCoordinate(47.633189139999999, -122.08900124);
...@@ -59,7 +59,7 @@ void StructureScanComplexItemTest::_testDirty(void) ...@@ -59,7 +59,7 @@ void StructureScanComplexItemTest::_testDirty(void)
// These facts should set dirty when changed // These facts should set dirty when changed
QList<Fact*> rgFacts; QList<Fact*> rgFacts;
rgFacts << _structureScanItem->altitude() << _structureScanItem->layers(); rgFacts << _structureScanItem->entranceAlt() << _structureScanItem->layers();
for(Fact* fact: rgFacts) { for(Fact* fact: rgFacts) {
qDebug() << fact->name(); qDebug() << fact->name();
QVERIFY(!_structureScanItem->dirty()); QVERIFY(!_structureScanItem->dirty());
...@@ -74,13 +74,6 @@ void StructureScanComplexItemTest::_testDirty(void) ...@@ -74,13 +74,6 @@ void StructureScanComplexItemTest::_testDirty(void)
_multiSpy->clearAllSignals(); _multiSpy->clearAllSignals();
} }
rgFacts.clear(); 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) void StructureScanComplexItemTest::_initItem(void)
......
...@@ -109,33 +109,27 @@ Rectangle { ...@@ -109,33 +109,27 @@ Rectangle {
} }
QGCLabel { QGCLabel {
text: qsTr("Structure height") text: qsTr("Structure Height")
visible: !missionItem.cameraCalc.isManualCamera
} }
FactTextField { FactTextField {
fact: missionItem.structureHeight fact: missionItem.structureHeight
Layout.fillWidth: true Layout.fillWidth: true
visible: !missionItem.cameraCalc.isManualCamera
} }
QGCLabel { QGCLabel { text: qsTr("Scan Bottom Alt") }
text: qsTr("# Layers")
visible: missionItem.cameraCalc.isManualCamera
}
FactTextField { FactTextField {
fact: missionItem.layers fact: missionItem.scanBottomAlt
Layout.fillWidth: true Layout.fillWidth: true
visible: missionItem.cameraCalc.isManualCamera
} }
QGCLabel { text: qsTr("Bottom layer alt") } QGCLabel { text: qsTr("Entrance/Exit Alt") }
FactTextField { FactTextField {
fact: missionItem.altitude fact: missionItem.entranceAlt
Layout.fillWidth: true Layout.fillWidth: true
} }
QGCLabel { QGCLabel {
text: qsTr("Gimbal pitch") text: qsTr("Gimbal Pitch")
visible: missionItem.cameraCalc.isManualCamera visible: missionItem.cameraCalc.isManualCamera
} }
FactTextField { FactTextField {
...@@ -143,13 +137,6 @@ Rectangle { ...@@ -143,13 +137,6 @@ Rectangle {
Layout.fillWidth: true Layout.fillWidth: true
visible: missionItem.cameraCalc.isManualCamera visible: missionItem.cameraCalc.isManualCamera
} }
QGCCheckBox {
text: qsTr("Relative altitude")
checked: missionItem.altitudeRelative
Layout.columnSpan: 2
onClicked: missionItem.altitudeRelative = checked
}
} }
Item { Item {
...@@ -176,16 +163,22 @@ Rectangle { ...@@ -176,16 +163,22 @@ Rectangle {
QGCLabel { text: qsTr("Layers") } QGCLabel { text: qsTr("Layers") }
QGCLabel { text: missionItem.layers.valueString } 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: 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: missionItem.cameraShots }
QGCLabel { text: qsTr("Photo interval") } QGCLabel { text: qsTr("Photo Interval") }
QGCLabel { text: missionItem.timeBetweenShots.toFixed(1) + " " + qsTr("secs") } 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 } QGCLabel { text: missionItem.cameraCalc.adjustedFootprintSide.valueString + " " + QGroundControl.appSettingsDistanceUnitsString }
} }
} // Column } // Column
......
Markdown is supported
0% or
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment