zhaohuwei
/
QG4
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

Bug fixes for Structure Scan. Also includes support for Entrance/Exit Alt.

QGC4.4
Don Gagne 6 years ago
parent
efd77cee82
commit
61bdd7f2eb
5 changed files with 155 additions and 71 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 8
      src/MissionManager/StructureScan.SettingsGroup.json
  2. 192
      src/MissionManager/StructureScanComplexItem.cc
  3. 14
      src/MissionManager/StructureScanComplexItem.h
  4. 2
      src/MissionManager/StructureScanComplexItemTest.cc
  5. 10
      src/PlanView/StructureScanEditor.qml

8
src/MissionManager/StructureScan.SettingsGroup.json
Unescape View File

@ -10,6 +10,14 @@ @@ -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",

192
src/MissionManager/StructureScanComplexItem.cc
Unescape View File

@ -22,7 +22,8 @@ @@ -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 @@ -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 @@ -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 @@ -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 @@ -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) @@ -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 @@ -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 @@ -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<MissionItem*>& items, QO @@ -297,20 +314,41 @@ void StructureScanComplexItem::appendMissionItems(QList<MissionItem*>& 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<double>::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<MissionItem*>& items, QO @@ -322,24 +360,28 @@ void StructureScanComplexItem::appendMissionItems(QList<MissionItem*>& 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<double>::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<double>::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<MissionItem*>& items, QO @@ -354,25 +396,19 @@ void StructureScanComplexItem::appendMissionItems(QList<MissionItem*>& 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<double>::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<MissionItem*>& items, QO @@ -386,6 +422,7 @@ void StructureScanComplexItem::appendMissionItems(QList<MissionItem*>& 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<MissionItem*>& items, QO @@ -394,11 +431,28 @@ void StructureScanComplexItem::appendMissionItems(QList<MissionItem*>& 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<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
{
return true /*_triggerCamera()*/ ? _cameraShots : 0;
return _cameraShots;
}
void StructureScanComplexItem::setMissionFlightStatus(MissionController::MissionFlightStatus_t& missionFlightStatus)
@ -417,7 +471,7 @@ void StructureScanComplexItem::_setDirty(void) @@ -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) @@ -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) @@ -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());
}
}

14
src/MissionManager/StructureScanComplexItem.h
Unescape View File

@ -31,7 +31,8 @@ public: @@ -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: @@ -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: @@ -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: @@ -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: @@ -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;
};

2
src/MissionManager/StructureScanComplexItemTest.cc
Unescape View File

@ -59,7 +59,7 @@ void StructureScanComplexItemTest::_testDirty(void) @@ -59,7 +59,7 @@ void StructureScanComplexItemTest::_testDirty(void)
// These facts should set dirty when changed
QList<Fact*> rgFacts;
rgFacts << _structureScanItem->altitude() << _structureScanItem->layers();
rgFacts << _structureScanItem->entranceAlt() << _structureScanItem->layers();
for(Fact* fact: rgFacts) {
qDebug() << fact->name();
QVERIFY(!_structureScanItem->dirty());

10
src/PlanView/StructureScanEditor.qml
Unescape View File

@ -128,9 +128,15 @@ Rectangle { @@ -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
}

Write Preview
Loading…
Cancel
Save