Skip to content

Q
qgroundcontrol
Commit 0487ffc3 authored by Don Gagne's avatar Don Gagne Committed by GitHub
Browse files
Options

Merge pull request #4930 from DonLakeFlyer/SurveyRefly 

Support for refly survey at 90 degrees offset
parents 681c3423 eaedc20a
Show whitespace changes
Inline Side-by-side
Showing with 155 additions and 69 deletions
src/MissionManager/SurveyMissionItem.cc
View file @ 0487ffc3
......@@ -45,6 +45,7 @@ const char* SurveyMissionItem::_jsonCameraNameKey = "name";
const char* SurveyMissionItem::_jsonManualGridKey = "manualGrid";
const char* SurveyMissionItem::_jsonCameraOrientationLandscapeKey = "orientationLandscape";
const char* SurveyMissionItem::_jsonFixedValueIsAltitudeKey = "fixedValueIsAltitude";
const char* SurveyMissionItem::_jsonRefly90DegreesKey = "refly90Degrees";
const char* SurveyMissionItem::settingsGroup = "Survey";
const char* SurveyMissionItem::manualGridName = "ManualGrid";
......@@ -75,6 +76,7 @@ SurveyMissionItem::SurveyMissionItem(Vehicle* vehicle, QObject* parent)
, _dirty(false)
, _cameraOrientationFixed(false)
, _missionCommandCount(0)
, _refly90Degrees(false)
, _surveyDistance(0.0)
, _cameraShots(0)
, _coveredArea(0.0)
......@@ -116,6 +118,7 @@ SurveyMissionItem::SurveyMissionItem(Vehicle* vehicle, QObject* parent)
connect(&_cameraTriggerInTurnaroundFact, &Fact::valueChanged, this, &SurveyMissionItem::_generateGrid);
connect(&_hoverAndCaptureFact, &Fact::valueChanged, this, &SurveyMissionItem::_generateGrid);
connect(&_cameraTriggerFact, &Fact::valueChanged, this, &SurveyMissionItem::_generateGrid);
connect(this, &SurveyMissionItem::refly90DegreesChanged, this, &SurveyMissionItem::_generateGrid);
connect(&_gridAltitudeFact, &Fact::valueChanged, this, &SurveyMissionItem::_updateCoordinateAltitude);
......@@ -298,6 +301,7 @@ void SurveyMissionItem::save(QJsonArray& missionItems)
saveObject[_jsonManualGridKey] = _manualGridFact.rawValue().toBool();
saveObject[_jsonFixedValueIsAltitudeKey] = _fixedValueIsAltitudeFact.rawValue().toBool();
saveObject[_jsonHoverAndCaptureKey] = _hoverAndCaptureFact.rawValue().toBool();
saveObject[_jsonRefly90DegreesKey] = _refly90Degrees;
if (_cameraTriggerFact.rawValue().toBool()) {
saveObject[_jsonCameraTriggerDistanceKey] = _cameraTriggerDistanceFact.rawValue().toDouble();
......@@ -389,6 +393,7 @@ bool SurveyMissionItem::load(const QJsonObject& complexObject, int sequenceNumbe
{ _jsonManualGridKey, QJsonValue::Bool, true },
{ _jsonFixedValueIsAltitudeKey, QJsonValue::Bool, true },
{ _jsonHoverAndCaptureKey, QJsonValue::Bool, false },
{ _jsonRefly90DegreesKey, QJsonValue::Bool, false },
};
if (!JsonHelper::validateKeys(v2Object, mainKeyInfoList, errorString)) {
return false;
......@@ -411,6 +416,8 @@ bool SurveyMissionItem::load(const QJsonObject& complexObject, int sequenceNumbe
_gridAltitudeRelativeFact.setRawValue (v2Object[_jsonGridAltitudeRelativeKey].toBool(true));
_hoverAndCaptureFact.setRawValue (v2Object[_jsonHoverAndCaptureKey].toBool(false));
_refly90Degrees = v2Object[_jsonRefly90DegreesKey].toBool(false);
QList<JsonHelper::KeyValidateInfo> gridKeyInfoList = {
{ _jsonGridAltitudeKey, QJsonValue::Double, true },
{ _jsonGridAltitudeRelativeKey, QJsonValue::Bool, true },
......@@ -538,6 +545,37 @@ void _calcCameraShots()
}
void SurveyMissionItem::_convertTransectToGeo(const QList<QList<QPointF>>& transectSegmentsNED, const QGeoCoordinate& tangentOrigin, QList<QList<QGeoCoordinate>>& transectSegmentsGeo)
{
transectSegmentsGeo.clear();
for (int i=0; i<transectSegmentsNED.count(); i++) {
QList<QGeoCoordinate> transectCoords;
const QList<QPointF>& transectPoints = transectSegmentsNED[i];
for (int j=0; j<transectPoints.count(); j++) {
QGeoCoordinate coord;
const QPointF& point = transectPoints[j];
convertNedToGeo(-point.y(), point.x(), 0, tangentOrigin, &coord);
transectCoords.append(coord);
}
transectSegmentsGeo.append(transectCoords);
}
}
void SurveyMissionItem::_convertPointsToGeo(const QList<QPointF>& pointsNED, const QGeoCoordinate& tangentOrigin, QVariantList& pointsGeo)
{
pointsGeo.clear();
for (int i=0; i<pointsNED.count(); i++) {
QGeoCoordinate geoCoord;
const QPointF& point = pointsNED[i];
convertNedToGeo(-point.y(), point.x(), 0, tangentOrigin, &geoCoord);
pointsGeo.append(QVariant::fromValue(geoCoord));
}
}
void SurveyMissionItem::_generateGrid(void)
{
if (_polygonPath.count() < 3 || _gridSpacingFact.rawValue().toDouble() <= 0) {
......@@ -547,6 +585,7 @@ void SurveyMissionItem::_generateGrid(void)
_simpleGridPoints.clear();
_transectSegments.clear();
_reflyTransectSegments.clear();
QList<QPointF> polygonPoints;
QList<QPointF> gridPoints;
......@@ -573,45 +612,34 @@ void SurveyMissionItem::_generateGrid(void)
_setCoveredArea(0.5 * fabs(coveredArea));
// Generate grid
_gridGenerator(polygonPoints, gridPoints, transectSegments);
// Convert simple grid to QGeoCoordinates
double surveyDistance = 0.0;
for (int i=0; i<gridPoints.count(); i++) {
QPointF& point = gridPoints[i];
int cameraShots = 0;
cameraShots += _gridGenerator(polygonPoints, gridPoints, transectSegments, false /* refly */);
_convertPointsToGeo(gridPoints, tangentOrigin, _simpleGridPoints);
_convertTransectToGeo(transectSegments, tangentOrigin, _transectSegments);
if (_refly90Degrees) {
QVariantList reflyPointsGeo;
if (i != 0) {
surveyDistance += sqrt(pow((gridPoints[i] - gridPoints[i - 1]).x(),2.0) + pow((gridPoints[i] - gridPoints[i - 1]).y(),2.0));
gridPoints.clear();
transectSegments.clear();
cameraShots += _gridGenerator(polygonPoints, gridPoints, transectSegments, true /* refly */);
_convertPointsToGeo(gridPoints, tangentOrigin, reflyPointsGeo);
_convertTransectToGeo(transectSegments, tangentOrigin, _reflyTransectSegments);
_simpleGridPoints.append(reflyPointsGeo);
}
QGeoCoordinate geoCoord;
convertNedToGeo(-point.y(), point.x(), 0, tangentOrigin, &geoCoord);
_simpleGridPoints += QVariant::fromValue(geoCoord);
// Calc survey distance
double surveyDistance = 0.0;
for (int i=1; i<_simpleGridPoints.count(); i++) {
QGeoCoordinate coord1 = _simpleGridPoints[i-1].value<QGeoCoordinate>();
QGeoCoordinate coord2 = _simpleGridPoints[i].value<QGeoCoordinate>();
surveyDistance += coord1.distanceTo(coord2);
}
_setSurveyDistance(surveyDistance);
// Convert transect segments to QGeoCoordinate
for (int i=0; i<transectSegments.count(); i++) {
QList<QGeoCoordinate> transectCoords;
const QList<QPointF>& transectPoints = transectSegments[i];
for (int j=0; j<transectPoints.count(); j++) {
QGeoCoordinate coord;
const QPointF& point = transectPoints[j];
convertNedToGeo(-point.y(), point.x(), 0, tangentOrigin, &coord);
transectCoords.append(coord);
}
_transectSegments.append(transectCoords);
}
// Set camera shots here if taking images in turnaround (otherwise it's in _gridGenerator)
if (_triggerCamera()) {
if (_imagesEverywhere()) {
_setCameraShots((int)ceil(surveyDistance / _triggerDistance()));
}
} else {
_setCameraShots(0);
if (cameraShots == 0 && _triggerCamera()) {
cameraShots = (int)ceil(surveyDistance / _triggerDistance());
}
_setCameraShots(cameraShots);
emit gridPointsChanged();
......@@ -640,6 +668,8 @@ void SurveyMissionItem::_generateGrid(void)
exitCoordinate.setAltitude(_gridAltitudeFact.rawValue().toDouble());
_setExitCoordinate(exitCoordinate);
}
setDirty(true);
}
void SurveyMissionItem::_updateCoordinateAltitude(void)
......@@ -764,9 +794,11 @@ void SurveyMissionItem::_adjustLineDirection(const QList<QLineF>& lineList, QLis
}
}
void SurveyMissionItem::_gridGenerator(const QList<QPointF>& polygonPoints, QList<QPointF>& simpleGridPoints, QList<QList<QPointF>>& transectSegments)
int SurveyMissionItem::_gridGenerator(const QList<QPointF>& polygonPoints, QList<QPointF>& simpleGridPoints, QList<QList<QPointF>>& transectSegments, bool refly)
{
double gridAngle = _gridAngleFact.rawValue().toDouble();
int cameraShots = 0;
double gridAngle = _gridAngleFact.rawValue().toDouble() + (refly ? 90 : 0);
double gridSpacing = _gridSpacingFact.rawValue().toDouble();
qCDebug(SurveyMissionItemLog) << "SurveyMissionItem::_gridGenerator gridSpacing:gridAngle" << gridSpacing << gridAngle;
......@@ -827,11 +859,9 @@ void SurveyMissionItem::_gridGenerator(const QList<QPointF>& polygonPoints, QLi
// Calc camera shots here if there are no images in turnaround
if (_triggerCamera() && !_imagesEverywhere()) {
int cameraShots = 0;
for (int i=0; i<resultLines.count(); i++) {
cameraShots += (int)ceil(resultLines[i].length() / _triggerDistance());
}
_setCameraShots(cameraShots);
}
// Turn into a path
......@@ -881,6 +911,8 @@ void SurveyMissionItem::_gridGenerator(const QList<QPointF>& polygonPoints, QLi
transectSegments.append(transectPoints);
}
return cameraShots;
}
int SurveyMissionItem::_appendWaypointToMission(QList<MissionItem*>& items, int seqNum, QGeoCoordinate& coord, CameraTriggerCode cameraTrigger, QObject* missionItemParent)
......@@ -961,13 +993,21 @@ bool SurveyMissionItem::_nextTransectCoord(const QList<QGeoCoordinate>& transect
return true;
}
void SurveyMissionItem::appendMissionItems(QList<MissionItem*>& items, QObject* missionItemParent)
/// Appends the mission items for the survey
/// @param items Mission items are appended to this list
/// @param missionItemParent Parent object for newly created MissionItem objects
/// @param seqNum[in,out] Sequence number to start from
/// @param hasRefly true: misison has a refly section
/// @param buildRefly: true: build the refly section, false: build the first section
/// @return false: Generation failed
bool SurveyMissionItem::_appendMissionItemsWorker(QList<MissionItem*>& items, QObject* missionItemParent, int& seqNum, bool hasRefly, bool buildRefly)
{
int seqNum = _sequenceNumber;
qCDebug(SurveyMissionItemLog) << "hasTurnaround:triggerCamera:hoverAndCapture:imagesEverywhere:hasRefly:buildRefly" << _hasTurnaround() << _triggerCamera() << _hoverAndCaptureEnabled() << _imagesEverywhere() << hasRefly << buildRefly;
qCDebug(SurveyMissionItemLog) << "hasTurnaround:triggerCamera:hoverAndCapture:imagesEverywhere" << _hasTurnaround() << _triggerCamera() << _hoverAndCaptureEnabled() << _imagesEverywhere();
QList<QList<QGeoCoordinate>>& transectSegments = buildRefly ? _reflyTransectSegments : _transectSegments;
if (_imagesEverywhere()) {
if (!buildRefly && _imagesEverywhere()) {
// We are taking images in turnaround, so we start command once at beginning
MissionItem* item = new MissionItem(seqNum++,
MAV_CMD_DO_SET_CAM_TRIGG_DIST,
MAV_FRAME_MISSION,
......@@ -979,52 +1019,52 @@ void SurveyMissionItem::appendMissionItems(QList<MissionItem*>& items, QObject*
items.append(item);
}
for (int segmentIndex=0; segmentIndex<_transectSegments.count(); segmentIndex++) {
for (int segmentIndex=0; segmentIndex<transectSegments.count(); segmentIndex++) {
int pointIndex = 0;
QGeoCoordinate coord;
CameraTriggerCode cameraTrigger;
const QList<QGeoCoordinate>& transectSegment = _transectSegments[segmentIndex];
const QList<QGeoCoordinate>& segment = transectSegments[segmentIndex];
qCDebug(SurveyMissionItemLog) << "transectSegment.count" << transectSegment.count();
qCDebug(SurveyMissionItemLog) << "segment.count" << segment.count();
if (_hasTurnaround()) {
// Add entry turnaround point
if (!_nextTransectCoord(transectSegment, pointIndex++, coord)) {
return;
if (!_nextTransectCoord(segment, pointIndex++, coord)) {
return false;
}
seqNum = _appendWaypointToMission(items, seqNum, coord, CameraTriggerNone, missionItemParent);
}
// Add polygon entry point
if (!_nextTransectCoord(transectSegment, pointIndex++, coord)) {
return;
if (!_nextTransectCoord(segment, pointIndex++, coord)) {
return false;
}
cameraTrigger = _imagesEverywhere() || !_triggerCamera() ? CameraTriggerNone : (_hoverAndCaptureEnabled() ? CameraTriggerHoverAndCapture : CameraTriggerOn);
seqNum = _appendWaypointToMission(items, seqNum, coord, cameraTrigger, missionItemParent);
// Add internal hover and capture points
if (_hoverAndCaptureEnabled()) {
int lastHoverAndCaptureIndex = transectSegment.count() - 1 - (_hasTurnaround() ? 1 : 0);
int lastHoverAndCaptureIndex = segment.count() - 1 - (_hasTurnaround() ? 1 : 0);
qCDebug(SurveyMissionItemLog) << "lastHoverAndCaptureIndex" << lastHoverAndCaptureIndex;
for (; pointIndex < lastHoverAndCaptureIndex; pointIndex++) {
if (!_nextTransectCoord(transectSegment, pointIndex, coord)) {
return;
if (!_nextTransectCoord(segment, pointIndex, coord)) {
return false;
}
seqNum = _appendWaypointToMission(items, seqNum, coord, CameraTriggerHoverAndCapture, missionItemParent);
}
}
// Add polygon exit point
if (!_nextTransectCoord(transectSegment, pointIndex++, coord)) {
return;
if (!_nextTransectCoord(segment, pointIndex++, coord)) {
return false;
}
cameraTrigger = _imagesEverywhere() || !_triggerCamera() ? CameraTriggerNone : (_hoverAndCaptureEnabled() ? CameraTriggerNone : CameraTriggerOff);
seqNum = _appendWaypointToMission(items, seqNum, coord, cameraTrigger, missionItemParent);
if (_hasTurnaround()) {
// Add exit turnaround point
if (!_nextTransectCoord(transectSegment, pointIndex++, coord)) {
return;
if (!_nextTransectCoord(segment, pointIndex++, coord)) {
return false;
}
seqNum = _appendWaypointToMission(items, seqNum, coord, CameraTriggerNone, missionItemParent);
}
......@@ -1032,7 +1072,7 @@ void SurveyMissionItem::appendMissionItems(QList<MissionItem*>& items, QObject*
qCDebug(SurveyMissionItemLog) << "last PointIndex" << pointIndex;
}
if (_imagesEverywhere()) {
if (((hasRefly && buildRefly) || !hasRefly) && _imagesEverywhere()) {
// Turn off camera at end of survey
MissionItem* item = new MissionItem(seqNum++,
MAV_CMD_DO_SET_CAM_TRIGG_DIST,
......@@ -1044,6 +1084,21 @@ void SurveyMissionItem::appendMissionItems(QList<MissionItem*>& items, QObject*
missionItemParent);
items.append(item);
}
return true;
}
void SurveyMissionItem::appendMissionItems(QList<MissionItem*>& items, QObject* missionItemParent)
{
int seqNum = _sequenceNumber;
if (!_appendMissionItemsWorker(items, missionItemParent, seqNum, _refly90Degrees, false /* buildRefly */)) {
return;
}
if (_refly90Degrees) {
_appendMissionItemsWorker(items, missionItemParent, seqNum, _refly90Degrees, true /* buildRefly */);
}
}
int SurveyMissionItem::cameraShots(void) const
......@@ -1113,3 +1168,11 @@ void SurveyMissionItem::applyNewAltitude(double newAltitude)
{
_gridAltitudeFact.setRawValue(newAltitude);
}
void SurveyMissionItem::setRefly90Degrees(bool refly90Degrees)
{
if (refly90Degrees != _refly90Degrees) {
_refly90Degrees = refly90Degrees;
emit refly90DegreesChanged(refly90Degrees);
}
}
src/MissionManager/SurveyMissionItem.h
View file @ 0487ffc3
......@@ -49,6 +49,7 @@ public:
Q_PROPERTY(bool cameraOrientationFixed MEMBER _cameraOrientationFixed NOTIFY cameraOrientationFixedChanged)
Q_PROPERTY(bool hoverAndCaptureAllowed READ hoverAndCaptureAllowed CONSTANT)
Q_PROPERTY(bool refly90Degrees READ refly90Degrees WRITE setRefly90Degrees NOTIFY refly90DegreesChanged)
Q_PROPERTY(double timeBetweenShots READ timeBetweenShots NOTIFY timeBetweenShotsChanged)
Q_PROPERTY(QVariantList polygonPath READ polygonPath NOTIFY polygonPathChanged)
......@@ -95,10 +96,13 @@ public:
Fact* fixedValueIsAltitude (void) { return &_fixedValueIsAltitudeFact; }
Fact* camera (void) { return &_cameraFact; }
int cameraShots(void) const;
double coveredArea(void) const { return _coveredArea; }
double timeBetweenShots(void) const;
bool hoverAndCaptureAllowed(void) const;
int cameraShots (void) const;
double coveredArea (void) const { return _coveredArea; }
double timeBetweenShots (void) const;
bool hoverAndCaptureAllowed (void) const;
bool refly90Degrees (void) const { return _refly90Degrees; }
void setRefly90Degrees(bool refly90Degrees);
// Overrides from ComplexMissionItem
......@@ -108,7 +112,6 @@ public:
double greatestDistanceTo (const QGeoCoordinate &other) const final;
QString mapVisualQML (void) const final { return QStringLiteral("SurveyMapVisual.qml"); }
// Overrides from VisualMissionItem
bool dirty (void) const final { return _dirty; }
......@@ -172,6 +175,7 @@ signals:
void gridTypeChanged (QString gridType);
void timeBetweenShotsChanged (void);
void cameraOrientationFixedChanged (bool cameraOrientationFixed);
void refly90DegreesChanged (bool refly90Degrees);
private slots:
void _setDirty(void);
......@@ -189,7 +193,7 @@ private:
void _clearGrid(void);
void _generateGrid(void);
void _updateCoordinateAltitude(void);
void _gridGenerator(const QList<QPointF>& polygonPoints, QList<QPointF>& simpleGridPoints, QList<QList<QPointF>>& transectSegments);
int _gridGenerator(const QList<QPointF>& polygonPoints, QList<QPointF>& simpleGridPoints, QList<QList<QPointF>>& transectSegments, bool refly);
QPointF _rotatePoint(const QPointF& point, const QPointF& origin, double angle);
void _intersectLinesWithRect(const QList<QLineF>& lineList, const QRectF& boundRect, QList<QLineF>& resultLines);
void _intersectLinesWithPolygon(const QList<QLineF>& lineList, const QPolygonF& polygon, QList<QLineF>& resultLines);
......@@ -206,6 +210,9 @@ private:
bool _hoverAndCaptureEnabled(void) const;
bool _hasTurnaround(void) const;
double _turnaroundDistance(void) const;
void _convertTransectToGeo(const QList<QList<QPointF>>& transectSegmentsNED, const QGeoCoordinate& tangentOrigin, QList<QList<QGeoCoordinate>>& transectSegmentsGeo);
void _convertPointsToGeo(const QList<QPointF>& pointsNED, const QGeoCoordinate& tangentOrigin, QVariantList& pointsGeo);
bool _appendMissionItemsWorker(QList<MissionItem*>& items, QObject* missionItemParent, int& seqNum, bool hasRefly, bool buildRefly);
int _sequenceNumber;
bool _dirty;
......@@ -213,10 +220,12 @@ private:
QmlObjectListModel _polygonModel;
QVariantList _simpleGridPoints; ///< Grid points for drawing simple grid visuals
QList<QList<QGeoCoordinate>> _transectSegments; ///< Internal transect segments including grid exit, turnaround and internal camera points
QList<QList<QGeoCoordinate>> _reflyTransectSegments; ///< Refly segments
QGeoCoordinate _coordinate;
QGeoCoordinate _exitCoordinate;
bool _cameraOrientationFixed;
int _missionCommandCount;
bool _refly90Degrees;
double _surveyDistance;
int _cameraShots;
......@@ -272,6 +281,7 @@ private:
static const char* _jsonCameraNameKey;
static const char* _jsonCameraOrientationLandscapeKey;
static const char* _jsonFixedValueIsAltitudeKey;
static const char* _jsonRefly90DegreesKey;
};
#endif
src/PlanView/SurveyItemEditor.qml
View file @ 0487ffc3
......@@ -402,6 +402,13 @@ Rectangle {
Layout.fillWidth: true
}
QGCCheckBox {
text: qsTr("Refly at 90 degree offset")
checked: missionItem.refly90Degrees
onClicked: missionItem.refly90Degrees = checked
Layout.columnSpan: 2
}
QGCLabel {
wrapMode: Text.WordWrap
font.pointSize: ScreenTools.smallFontPointSize
......@@ -462,6 +469,12 @@ Rectangle {
factLabels: [ qsTr("Angle"), qsTr("Spacing"), qsTr("Altitude"), qsTr("Turnaround dist")]
}
QGCCheckBox {
text: qsTr("Refly at 90 degree offset")
checked: missionItem.refly90Degrees
onClicked: missionItem.refly90Degrees = checked
}
FactCheckBox {
anchors.left: parent.left
text: qsTr("Relative altitude")
......
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