Skip to content

Q
qgroundcontrol
Commit 957a4b1b authored by Don Gagne's avatar Don Gagne Committed by GitHub
Browse files
Options

Merge pull request #3955 from DonLakeFlyer/SurveyMissionItem 

Break out SurveyMissionItem->ComplexMissionItem class hierarchy
parents 914f512f 7bd7938c
Hide whitespace changes
Inline Side-by-side
Showing with 792 additions and 739 deletions
qgroundcontrol.pro
View file @ 957a4b1b
......@@ -271,14 +271,15 @@ HEADERS += \
src/JsonHelper.h \
src/LogCompressor.h \
src/MG.h \
src/MissionManager/ComplexMissionItem.h \
src/MissionManager/MissionCommandList.h \
src/MissionManager/MissionCommandTree.h \
src/MissionManager/MissionCommandUIInfo.h \
src/MissionManager/MissionController.h \
src/MissionManager/MissionItem.h \
src/MissionManager/MissionManager.h \
src/MissionManager/ComplexMissionItem.h \
src/MissionManager/SimpleMissionItem.h \
src/MissionManager/SurveyMissionItem.h \
src/MissionManager/VisualMissionItem.h \
src/QGC.h \
src/QGCApplication.h \
......@@ -431,14 +432,15 @@ SOURCES += \
src/FollowMe/FollowMe.cc \
src/LogCompressor.cc \
src/main.cc \
src/MissionManager/ComplexMissionItem.cc \
src/MissionManager/MissionCommandList.cc \
src/MissionManager/MissionCommandTree.cc \
src/MissionManager/MissionCommandUIInfo.cc \
src/MissionManager/MissionController.cc \
src/MissionManager/MissionItem.cc \
src/MissionManager/MissionManager.cc \
src/MissionManager/ComplexMissionItem.cc \
src/MissionManager/SimpleMissionItem.cc \
src/MissionManager/SurveyMissionItem.cc \
src/MissionManager/VisualMissionItem.cc \
src/QGC.cc \
src/QGCApplication.cc \
......
src/MissionManager/ComplexMissionItem.cc
View file @ 957a4b1b
......@@ -9,612 +9,16 @@
#include "ComplexMissionItem.h"
#include "JsonHelper.h"
#include "MissionController.h"
#include "QGCGeo.h"
#include <QPolygonF>
QGC_LOGGING_CATEGORY(ComplexMissionItemLog, "ComplexMissionItemLog")
const char* ComplexMissionItem::_jsonVersionKey = "version";
const char* ComplexMissionItem::_jsonTypeKey = "type";
const char* ComplexMissionItem::_jsonPolygonKey = "polygon";
const char* ComplexMissionItem::_jsonIdKey = "id";
const char* ComplexMissionItem::_jsonGridAltitudeKey = "gridAltitude";
const char* ComplexMissionItem::_jsonGridAltitudeRelativeKey = "gridAltitudeRelative";
const char* ComplexMissionItem::_jsonGridAngleKey = "gridAngle";
const char* ComplexMissionItem::_jsonGridSpacingKey = "gridSpacing";
const char* ComplexMissionItem::_jsonCameraTriggerKey = "cameraTrigger";
const char* ComplexMissionItem::_jsonCameraTriggerDistanceKey = "cameraTriggerDistance";
const char* ComplexMissionItem::_complexType = "survey";
ComplexMissionItem::ComplexMissionItem(Vehicle* vehicle, QObject* parent)
: VisualMissionItem(vehicle, parent)
, _sequenceNumber(0)
, _dirty(false)
, _cameraTrigger(false)
, _gridAltitudeRelative(true)
, _surveyDistance(0.0)
, _cameraShots(0)
, _coveredArea(0.0)
, _gridAltitudeFact (0, "Altitude:", FactMetaData::valueTypeDouble)
, _gridAngleFact (0, "Grid angle:", FactMetaData::valueTypeDouble)
, _gridSpacingFact (0, "Grid spacing:", FactMetaData::valueTypeDouble)
, _cameraTriggerDistanceFact(0, "Camera trigger distance", FactMetaData::valueTypeDouble)
{
_gridAltitudeFact.setRawValue(25);
_gridSpacingFact.setRawValue(10);
_cameraTriggerDistanceFact.setRawValue(25);
connect(&_gridSpacingFact, &Fact::valueChanged, this, &ComplexMissionItem::_generateGrid);
connect(&_gridAngleFact, &Fact::valueChanged, this, &ComplexMissionItem::_generateGrid);
connect(&_cameraTriggerDistanceFact, &Fact::valueChanged, this, &ComplexMissionItem::_generateGrid);
connect(this, &ComplexMissionItem::cameraTriggerChanged, this, &ComplexMissionItem::_cameraTriggerChanged);
}
const ComplexMissionItem& ComplexMissionItem::operator=(const ComplexMissionItem& other)
{
_vehicle = other._vehicle;
setIsCurrentItem(other._isCurrentItem);
setDirty(other._dirty);
setAltDifference(other._altDifference);
setAltPercent(other._altPercent);
setAzimuth(other._azimuth);
setDistance(other._distance);
setSurveyDistance(other._surveyDistance);
setCameraShots(other._cameraShots);
setCoveredArea(other._coveredArea);
VisualMissionItem::operator=(other);
return *this;
}
void ComplexMissionItem::setSurveyDistance(double surveyDistance)
{
if (!qFuzzyCompare(_surveyDistance, surveyDistance)) {
_surveyDistance = surveyDistance;
emit surveyDistanceChanged(_surveyDistance);
}
}
void ComplexMissionItem::setCameraShots(int cameraShots)
{
if (_cameraShots != cameraShots) {
_cameraShots = cameraShots;
emit cameraShotsChanged(_cameraShots);
}
}
void ComplexMissionItem::setCoveredArea(double coveredArea)
{
if (!qFuzzyCompare(_coveredArea, coveredArea)) {
_coveredArea = coveredArea;
emit coveredAreaChanged(_coveredArea);
}
}
void ComplexMissionItem::clearPolygon(void)
{
// Bug workaround, see below
while (_polygonPath.count() > 1) {
_polygonPath.takeLast();
}
emit polygonPathChanged();
// Although this code should remove the polygon from the map it doesn't. There appears
// to be a bug in MapPolygon which causes it to not be redrawn if the list is empty. So
// we work around it by using the code above to remove all but the last point which in turn
// will cause the polygon to go away.
_polygonPath.clear();
_clearGrid();
setDirty(true);
emit specifiesCoordinateChanged();
emit lastSequenceNumberChanged(lastSequenceNumber());
}
void ComplexMissionItem::addPolygonCoordinate(const QGeoCoordinate coordinate)
{
_polygonPath << QVariant::fromValue(coordinate);
emit polygonPathChanged();
int pointCount = _polygonPath.count();
if (pointCount >= 3) {
if (pointCount == 3) {
emit specifiesCoordinateChanged();
}
_generateGrid();
}
setDirty(true);
}
void ComplexMissionItem::adjustPolygonCoordinate(int vertexIndex, const QGeoCoordinate coordinate)
{
_polygonPath[vertexIndex] = QVariant::fromValue(coordinate);
emit polygonPathChanged();
_generateGrid();
setDirty(true);
}
int ComplexMissionItem::lastSequenceNumber(void) const
{
int lastSeq = _sequenceNumber;
if (_gridPoints.count()) {
lastSeq += _gridPoints.count() - 1;
if (_cameraTrigger) {
// Account for two trigger messages
lastSeq += 2;
}
}
return lastSeq;
}
void ComplexMissionItem::setCoordinate(const QGeoCoordinate& coordinate)
{
if (_coordinate != coordinate) {
_coordinate = coordinate;
emit coordinateChanged(_coordinate);
}
}
void ComplexMissionItem::setDirty(bool dirty)
{
if (_dirty != dirty) {
_dirty = dirty;
emit dirtyChanged(_dirty);
}
}
void ComplexMissionItem::save(QJsonObject& saveObject) const
{
saveObject[_jsonVersionKey] = 1;
saveObject[_jsonTypeKey] = _complexType;
saveObject[_jsonIdKey] = sequenceNumber();
saveObject[_jsonGridAltitudeKey] = _gridAltitudeFact.rawValue().toDouble();
saveObject[_jsonGridAltitudeRelativeKey] = _gridAltitudeRelative;
saveObject[_jsonGridAngleKey] = _gridAngleFact.rawValue().toDouble();
saveObject[_jsonGridSpacingKey] = _gridSpacingFact.rawValue().toDouble();
saveObject[_jsonCameraTriggerKey] = _cameraTrigger;
saveObject[_jsonCameraTriggerDistanceKey] = _cameraTriggerDistanceFact.rawValue().toDouble();
// Polygon shape
QJsonArray polygonArray;
for (int i=0; i<_polygonPath.count(); i++) {
const QVariant& polyVar = _polygonPath[i];
QJsonValue jsonValue;
JsonHelper::writeQGeoCoordinate(jsonValue, polyVar.value<QGeoCoordinate>(), false /* writeAltitude */);
polygonArray += jsonValue;
}
saveObject[_jsonPolygonKey] = polygonArray;
}
void ComplexMissionItem::setSequenceNumber(int sequenceNumber)
{
if (_sequenceNumber != sequenceNumber) {
_sequenceNumber = sequenceNumber;
emit sequenceNumberChanged(sequenceNumber);
emit lastSequenceNumberChanged(lastSequenceNumber());
}
}
void ComplexMissionItem::_clear(void)
{
clearPolygon();
_clearGrid();
}
bool ComplexMissionItem::load(const QJsonObject& complexObject, QString& errorString)
{
_clear();
// Validate requires keys
QStringList requiredKeys;
requiredKeys << _jsonVersionKey << _jsonTypeKey << _jsonIdKey << _jsonPolygonKey << _jsonGridAltitudeKey << _jsonGridAngleKey << _jsonGridSpacingKey <<
_jsonCameraTriggerKey << _jsonCameraTriggerDistanceKey << _jsonGridAltitudeRelativeKey;
if (!JsonHelper::validateRequiredKeys(complexObject, requiredKeys, errorString)) {
_clear();
return false;
}
// Validate types
QStringList keyList;
QList<QJsonValue::Type> typeList;
keyList << _jsonVersionKey << _jsonTypeKey << _jsonIdKey << _jsonPolygonKey << _jsonGridAltitudeKey << _jsonGridAngleKey << _jsonGridSpacingKey <<
_jsonCameraTriggerKey << _jsonCameraTriggerDistanceKey << _jsonGridAltitudeRelativeKey;
typeList << QJsonValue::Double << QJsonValue::String << QJsonValue::Double << QJsonValue::Array << QJsonValue::Double << QJsonValue::Double<< QJsonValue::Double <<
QJsonValue::Bool << QJsonValue::Double << QJsonValue::Bool;
if (!JsonHelper::validateKeyTypes(complexObject, keyList, typeList, errorString)) {
_clear();
return false;
}
// Version check
if (complexObject[_jsonVersionKey].toInt() != 1) {
errorString = tr("QGroundControl does not support this version of survey items");
_clear();
return false;
}
QString complexType = complexObject[_jsonTypeKey].toString();
if (complexType != _complexType) {
errorString = tr("QGroundControl does not support loading this complex mission item type: %1").arg(complexType);
_clear();
return false;
}
setSequenceNumber(complexObject[_jsonIdKey].toInt());
_cameraTrigger = complexObject[_jsonCameraTriggerKey].toBool();
_gridAltitudeRelative = complexObject[_jsonGridAltitudeRelativeKey].toBool();
_gridAltitudeFact.setRawValue (complexObject[_jsonGridAltitudeKey].toDouble());
_gridAngleFact.setRawValue (complexObject[_jsonGridAngleKey].toDouble());
_gridSpacingFact.setRawValue (complexObject[_jsonGridSpacingKey].toDouble());
_cameraTriggerDistanceFact.setRawValue (complexObject[_jsonCameraTriggerDistanceKey].toDouble());
// Polygon shape
QJsonArray polygonArray(complexObject[_jsonPolygonKey].toArray());
for (int i=0; i<polygonArray.count(); i++) {
const QJsonValue& pointValue = polygonArray[i];
QGeoCoordinate pointCoord;
if (!JsonHelper::toQGeoCoordinate(pointValue, pointCoord, false /* altitudeRequired */, errorString)) {
_clear();
return false;
}
_polygonPath << QVariant::fromValue(pointCoord);
}
_generateGrid();
return true;
}
double ComplexMissionItem::greatestDistanceTo(const QGeoCoordinate &other) const
{
double greatestDistance = 0.0;
for (int i=0; i<_gridPoints.count(); i++) {
QGeoCoordinate currentCoord = _gridPoints[i].value<QGeoCoordinate>();
double distance = currentCoord.distanceTo(other);
if (distance > greatestDistance) {
greatestDistance = distance;
}
}
return greatestDistance;
}
void ComplexMissionItem::_setExitCoordinate(const QGeoCoordinate& coordinate)
{
if (_exitCoordinate != coordinate) {
_exitCoordinate = coordinate;
emit exitCoordinateChanged(coordinate);
}
}
bool ComplexMissionItem::specifiesCoordinate(void) const
{
return _polygonPath.count() > 2;
}
void ComplexMissionItem::_clearGrid(void)
{
// Bug workaround
while (_gridPoints.count() > 1) {
_gridPoints.takeLast();
}
emit gridPointsChanged();
_gridPoints.clear();
}
void ComplexMissionItem::_generateGrid(void)
{
if (_polygonPath.count() < 3) {
_clearGrid();
return;
}
_gridPoints.clear();
QList<QPointF> polygonPoints;
QList<QPointF> gridPoints;
// Convert polygon to Qt coordinate system (y positive is down)
qCDebug(ComplexMissionItemLog) << "Convert polygon";
QGeoCoordinate tangentOrigin = _polygonPath[0].value<QGeoCoordinate>();
for (int i=0; i<_polygonPath.count(); i++) {
double y, x, down;
convertGeoToNed(_polygonPath[i].value<QGeoCoordinate>(), tangentOrigin, &y, &x, &down);
polygonPoints += QPointF(x, -y);
qCDebug(ComplexMissionItemLog) << _polygonPath[i].value<QGeoCoordinate>() << polygonPoints.last().x() << polygonPoints.last().y();
}
double coveredArea = 0.0;
for (int i=0; i<polygonPoints.count(); i++) {
if (i != 0) {
coveredArea += polygonPoints[i - 1].x() * polygonPoints[i].y() - polygonPoints[i].x() * polygonPoints[i -1].y();
} else {
coveredArea += polygonPoints.last().x() * polygonPoints[i].y() - polygonPoints[i].x() * polygonPoints.last().y();
}
}
setCoveredArea(0.5 * fabs(coveredArea));
// Generate grid
_gridGenerator(polygonPoints, gridPoints);
double surveyDistance = 0.0;
// Convert to Geo and set altitude
for (int i=0; i<gridPoints.count(); i++) {
QPointF& point = gridPoints[i];
if (i != 0) {
surveyDistance += sqrt(pow((gridPoints[i] - gridPoints[i - 1]).x(),2.0) + pow((gridPoints[i] - gridPoints[i - 1]).y(),2.0));
}
QGeoCoordinate geoCoord;
convertNedToGeo(-point.y(), point.x(), 0, tangentOrigin, &geoCoord);
_gridPoints += QVariant::fromValue(geoCoord);
}
setSurveyDistance(surveyDistance);
setCameraShots((int)floor(surveyDistance / _cameraTriggerDistanceFact.rawValue().toDouble()));
emit gridPointsChanged();
emit lastSequenceNumberChanged(lastSequenceNumber());
if (_gridPoints.count()) {
setCoordinate(_gridPoints.first().value<QGeoCoordinate>());
_setExitCoordinate(_gridPoints.last().value<QGeoCoordinate>());
}
}
QPointF ComplexMissionItem::_rotatePoint(const QPointF& point, const QPointF& origin, double angle)
{
QPointF rotated;
double radians = (M_PI / 180.0) * angle;
rotated.setX(((point.x() - origin.x()) * cos(radians)) - ((point.y() - origin.y()) * sin(radians)) + origin.x());
rotated.setY(((point.x() - origin.x()) * sin(radians)) + ((point.y() - origin.y()) * cos(radians)) + origin.y());
return rotated;
}
void ComplexMissionItem::_intersectLinesWithRect(const QList<QLineF>& lineList, const QRectF& boundRect, QList<QLineF>& resultLines)
{
QLineF topLine (boundRect.topLeft(), boundRect.topRight());
QLineF bottomLine (boundRect.bottomLeft(), boundRect.bottomRight());
QLineF leftLine (boundRect.topLeft(), boundRect.bottomLeft());
QLineF rightLine (boundRect.topRight(), boundRect.bottomRight());
for (int i=0; i<lineList.count(); i++) {
QPointF intersectPoint;
QLineF intersectLine;
const QLineF& line = lineList[i];
int foundCount = 0;
if (line.intersect(topLine, &intersectPoint) == QLineF::BoundedIntersection) {
intersectLine.setP1(intersectPoint);
foundCount++;
}
if (line.intersect(rightLine, &intersectPoint) == QLineF::BoundedIntersection) {
if (foundCount == 0) {
intersectLine.setP1(intersectPoint);
} else {
if (foundCount != 1) {
qWarning() << "Found more than two intersecting points";
}
intersectLine.setP2(intersectPoint);
}
foundCount++;
}
if (line.intersect(bottomLine, &intersectPoint) == QLineF::BoundedIntersection) {
if (foundCount == 0) {
intersectLine.setP1(intersectPoint);
} else {
if (foundCount != 1) {
qWarning() << "Found more than two intersecting points";
}
intersectLine.setP2(intersectPoint);
}
foundCount++;
}
if (line.intersect(leftLine, &intersectPoint) == QLineF::BoundedIntersection) {
if (foundCount == 0) {
intersectLine.setP1(intersectPoint);
} else {
if (foundCount != 1) {
qWarning() << "Found more than two intersecting points";
}
intersectLine.setP2(intersectPoint);
}
foundCount++;
}
if (foundCount == 2) {
resultLines += intersectLine;
}
}
}
void ComplexMissionItem::_intersectLinesWithPolygon(const QList<QLineF>& lineList, const QPolygonF& polygon, QList<QLineF>& resultLines)
{
for (int i=0; i<lineList.count(); i++) {
int foundCount = 0;
QLineF intersectLine;
const QLineF& line = lineList[i];
for (int j=0; j<polygon.count()-1; j++) {
QPointF intersectPoint;
QLineF polygonLine = QLineF(polygon[j], polygon[j+1]);
if (line.intersect(polygonLine, &intersectPoint) == QLineF::BoundedIntersection) {
if (foundCount == 0) {
foundCount++;
intersectLine.setP1(intersectPoint);
} else {
foundCount++;
intersectLine.setP2(intersectPoint);
break;
}
}
}
if (foundCount == 2) {
resultLines += intersectLine;
}
}
}
/// Adjust the line segments such that they are all going the same direction with respect to going from P1->P2
void ComplexMissionItem::_adjustLineDirection(const QList<QLineF>& lineList, QList<QLineF>& resultLines)
{
for (int i=0; i<lineList.count(); i++) {
const QLineF& line = lineList[i];
QLineF adjustedLine;
if (line.angle() > 180.0) {
adjustedLine.setP1(line.p2());
adjustedLine.setP2(line.p1());
} else {
adjustedLine = line;
}
resultLines += adjustedLine;
}
}
void ComplexMissionItem::_gridGenerator(const QList<QPointF>& polygonPoints, QList<QPointF>& gridPoints)
{
double gridAngle = _gridAngleFact.rawValue().toDouble();
gridPoints.clear();
// Convert polygon to bounding rect
qCDebug(ComplexMissionItemLog) << "Polygon";
QPolygonF polygon;
for (int i=0; i<polygonPoints.count(); i++) {
qCDebug(ComplexMissionItemLog) << polygonPoints[i];
polygon << polygonPoints[i];
}
polygon << polygonPoints[0];
QRectF smallBoundRect = polygon.boundingRect();
QPointF center = smallBoundRect.center();
qCDebug(ComplexMissionItemLog) << "Bounding rect" << smallBoundRect.topLeft().x() << smallBoundRect.topLeft().y() << smallBoundRect.bottomRight().x() << smallBoundRect.bottomRight().y();
// Rotate the bounding rect around it's center to generate the larger bounding rect
QPolygonF boundPolygon;
boundPolygon << _rotatePoint(smallBoundRect.topLeft(), center, gridAngle);
boundPolygon << _rotatePoint(smallBoundRect.topRight(), center, gridAngle);
boundPolygon << _rotatePoint(smallBoundRect.bottomRight(), center, gridAngle);
boundPolygon << _rotatePoint(smallBoundRect.bottomLeft(), center, gridAngle);
boundPolygon << boundPolygon[0];
QRectF largeBoundRect = boundPolygon.boundingRect();
qCDebug(ComplexMissionItemLog) << "Rotated bounding rect" << largeBoundRect.topLeft().x() << largeBoundRect.topLeft().y() << largeBoundRect.bottomRight().x() << largeBoundRect.bottomRight().y();
// Create set of rotated parallel lines within the expanded bounding rect. Make the lines larger than the
// bounding box to guarantee intersection.
QList<QLineF> lineList;
float x = largeBoundRect.topLeft().x();
float gridSpacing = _gridSpacingFact.rawValue().toDouble();
while (x < largeBoundRect.bottomRight().x()) {
float yTop = largeBoundRect.topLeft().y() - 100.0;
float yBottom = largeBoundRect.bottomRight().y() + 100.0;
lineList += QLineF(_rotatePoint(QPointF(x, yTop), center, gridAngle), _rotatePoint(QPointF(x, yBottom), center, gridAngle));
qCDebug(ComplexMissionItemLog) << "line(" << lineList.last().x1() << ", " << lineList.last().y1() << ")-(" << lineList.last().x2() <<", " << lineList.last().y2() << ")";
x += gridSpacing;
}
// Now intersect the lines with the polygon
QList<QLineF> intersectLines;
#if 1
_intersectLinesWithPolygon(lineList, polygon, intersectLines);
#else
// This is handy for debugging grid problems, not for release
intersectLines = lineList;
#endif
// Make sure all lines are going to same direction. Polygon intersection leads to line which
// can be in varied directions depending on the order of the intesecting sides.
QList<QLineF> resultLines;
_adjustLineDirection(intersectLines, resultLines);
// Turn into a path
for (int i=0; i<resultLines.count(); i++) {
const QLineF& line = resultLines[i];
if (i & 1) {
gridPoints << line.p2() << line.p1();
} else {
gridPoints << line.p1() << line.p2();
}
}
}
QmlObjectListModel* ComplexMissionItem::getMissionItems(void) const
{
QmlObjectListModel* pMissionItems = new QmlObjectListModel;
int seqNum = _sequenceNumber;
for (int i=0; i<_gridPoints.count(); i++) {
QGeoCoordinate coord = _gridPoints[i].value<QGeoCoordinate>();
double altitude = _gridAltitudeFact.rawValue().toDouble();
MissionItem* item = new MissionItem(seqNum++, // sequence number
MAV_CMD_NAV_WAYPOINT, // MAV_CMD
_gridAltitudeRelative ? MAV_FRAME_GLOBAL_RELATIVE_ALT : MAV_FRAME_GLOBAL, // MAV_FRAME
0.0, 0.0, 0.0, 0.0, // param 1-4
coord.latitude(),
coord.longitude(),
altitude,
true, // autoContinue
false, // isCurrentItem
pMissionItems); // parent - allow delete on pMissionItems to delete everthing
pMissionItems->append(item);
if (_cameraTrigger && i == 0) {
MissionItem* item = new MissionItem(seqNum++, // sequence number
MAV_CMD_DO_SET_CAM_TRIGG_DIST, // MAV_CMD
MAV_FRAME_MISSION, // MAV_FRAME
_cameraTriggerDistanceFact.rawValue().toDouble(), // trigger distance
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, // param 2-7
true, // autoContinue
false, // isCurrentItem
pMissionItems); // parent - allow delete on pMissionItems to delete everthing
pMissionItems->append(item);
}
}
if (_cameraTrigger) {
MissionItem* item = new MissionItem(seqNum++, // sequence number
MAV_CMD_DO_SET_CAM_TRIGG_DIST, // MAV_CMD
MAV_FRAME_MISSION, // MAV_FRAME
0.0, // trigger distance
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, // param 2-7
true, // autoContinue
false, // isCurrentItem
pMissionItems); // parent - allow delete on pMissionItems to delete everthing
pMissionItems->append(item);
}
return pMissionItems;
}
void ComplexMissionItem::_cameraTriggerChanged(void)
{
setDirty(true);
if (_gridPoints.count()) {
// If we have grid turn on/off camera trigger will add/remove two camera trigger mission items
emit lastSequenceNumberChanged(lastSequenceNumber());
}
}
src/MissionManager/ComplexMissionItem.h
View file @ 957a4b1b
......@@ -7,16 +7,10 @@
*
****************************************************************************/
#ifndef ComplexMissionItem_H
#define ComplexMissionItem_H
#include "VisualMissionItem.h"
#include "MissionItem.h"
#include "Fact.h"
#include "QGCLoggingCategory.h"
Q_DECLARE_LOGGING_CATEGORY(ComplexMissionItemLog)
class ComplexMissionItem : public VisualMissionItem
{
......@@ -27,139 +21,33 @@ public:
const ComplexMissionItem& operator=(const ComplexMissionItem& other);
Q_PROPERTY(Fact* gridAltitude READ gridAltitude CONSTANT)
Q_PROPERTY(bool gridAltitudeRelative MEMBER _gridAltitudeRelative NOTIFY gridAltitudeRelativeChanged)
Q_PROPERTY(Fact* gridAngle READ gridAngle CONSTANT)
Q_PROPERTY(Fact* gridSpacing READ gridSpacing CONSTANT)
Q_PROPERTY(bool cameraTrigger MEMBER _cameraTrigger NOTIFY cameraTriggerChanged)
Q_PROPERTY(Fact* cameraTriggerDistance READ cameraTriggerDistance CONSTANT)
Q_PROPERTY(QVariantList polygonPath READ polygonPath NOTIFY polygonPathChanged)
Q_PROPERTY(int lastSequenceNumber READ lastSequenceNumber NOTIFY lastSequenceNumberChanged)
Q_PROPERTY(QVariantList gridPoints READ gridPoints NOTIFY gridPointsChanged)
Q_PROPERTY(double surveyDistance READ surveyDistance NOTIFY surveyDistanceChanged)
Q_PROPERTY(int cameraShots READ cameraShots NOTIFY cameraShotsChanged)
Q_PROPERTY(double coveredArea READ coveredArea NOTIFY coveredAreaChanged)
Q_INVOKABLE void clearPolygon(void);
Q_INVOKABLE void addPolygonCoordinate(const QGeoCoordinate coordinate);
Q_INVOKABLE void adjustPolygonCoordinate(int vertexIndex, const QGeoCoordinate coordinate);
QVariantList polygonPath(void) { return _polygonPath; }
QVariantList gridPoints (void) { return _gridPoints; }
Q_PROPERTY(int lastSequenceNumber READ lastSequenceNumber NOTIFY lastSequenceNumberChanged)
Q_PROPERTY(double complexDistance READ complexDistance NOTIFY complexDistanceChanged)
Fact* gridAltitude(void) { return &_gridAltitudeFact; }
Fact* gridAngle(void) { return &_gridAngleFact; }
Fact* gridSpacing(void) { return &_gridSpacingFact; }
Fact* cameraTriggerDistance(void) { return &_cameraTriggerDistanceFact; }
double surveyDistance (void) const { return _surveyDistance; }
int cameraShots (void) const { return _cameraShots; }
double coveredArea (void) const { return _coveredArea; }
void setSurveyDistance (double surveyDistance);
void setCameraShots (int cameraShots);
void setCoveredArea (double coveredArea);
/// @return The distance covered the complex mission item in meters.
virtual double complexDistance(void) const = 0;
/// @return The last sequence number used by this item. Takes into account child items of the complex item
int lastSequenceNumber(void) const;
virtual int lastSequenceNumber(void) const = 0;
/// Returns the mission items associated with the complex item. Caller is responsible for freeing. Calling
/// delete on returned QmlObjectListModel will free all memory including internal items.
QmlObjectListModel* getMissionItems(void) const;
virtual QmlObjectListModel* getMissionItems(void) const = 0;
/// Load the complex mission item from Json
/// @param complexObject Complex mission item json object
/// @param[out] errorString Error if load fails
/// @return true: load success, false: load failed, errorString set
bool load(const QJsonObject& complexObject, QString& errorString);
virtual bool load(const QJsonObject& complexObject, QString& errorString) = 0;
/// Get the point of complex mission item furthest away from a coordinate
/// @param other QGeoCoordinate to which distance is calculated
/// @return the greatest distance from any point of the complex item to some coordinate
double greatestDistanceTo(const QGeoCoordinate &other) const;
// Overrides from VisualMissionItem
bool dirty (void) const final { return _dirty; }
bool isSimpleItem (void) const final { return false; }
bool isStandaloneCoordinate (void) const final { return false; }
bool specifiesCoordinate (void) const final;
QString commandDescription (void) const final { return "Survey"; }
QString commandName (void) const final { return "Survey"; }
QString abbreviation (void) const final { return "S"; }
QGeoCoordinate coordinate (void) const final { return _coordinate; }
QGeoCoordinate exitCoordinate (void) const final { return _exitCoordinate; }
int sequenceNumber (void) const final { return _sequenceNumber; }
bool coordinateHasRelativeAltitude (void) const final { return _gridAltitudeRelative; }
bool exitCoordinateHasRelativeAltitude (void) const final { return _gridAltitudeRelative; }
bool exitCoordinateSameAsEntry (void) const final { return false; }
void setDirty (bool dirty) final;
void setCoordinate (const QGeoCoordinate& coordinate) final;
void setSequenceNumber (int sequenceNumber) final;
void save (QJsonObject& saveObject) const final;
virtual double greatestDistanceTo(const QGeoCoordinate &other) const = 0;
signals:
void polygonPathChanged (void);
void lastSequenceNumberChanged (int lastSequenceNumber);
void altitudeChanged (double altitude);
void gridAngleChanged (double gridAngle);
void gridPointsChanged (void);
void cameraTriggerChanged (bool cameraTrigger);
void gridAltitudeRelativeChanged (bool gridAltitudeRelative);
void surveyDistanceChanged (double surveyDistance);
void cameraShotsChanged (int cameraShots);
void coveredAreaChanged (double coveredArea);
private slots:
void _cameraTriggerChanged(void);
private:
void _clear(void);
void _setExitCoordinate(const QGeoCoordinate& coordinate);
void _clearGrid(void);
void _generateGrid(void);
void _gridGenerator(const QList<QPointF>& polygonPoints, QList<QPointF>& gridPoints);
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);
void _adjustLineDirection(const QList<QLineF>& lineList, QList<QLineF>& resultLines);
int _sequenceNumber;
bool _dirty;
QVariantList _polygonPath;
QVariantList _gridPoints;
QGeoCoordinate _coordinate;
QGeoCoordinate _exitCoordinate;
double _altitude;
double _gridAngle;
bool _cameraTrigger;
bool _gridAltitudeRelative;
double _surveyDistance;
int _cameraShots;
double _coveredArea;
Fact _gridAltitudeFact;
Fact _gridAngleFact;
Fact _gridSpacingFact;
Fact _cameraTriggerDistanceFact;
static const char* _jsonVersionKey;
static const char* _jsonTypeKey;
static const char* _jsonPolygonKey;
static const char* _jsonIdKey;
static const char* _jsonGridAltitudeKey;
static const char* _jsonGridAltitudeRelativeKey;
static const char* _jsonGridAngleKey;
static const char* _jsonGridSpacingKey;
static const char* _jsonCameraTriggerKey;
static const char* _jsonCameraTriggerDistanceKey;
static const char* _complexType;
void lastSequenceNumberChanged (int lastSequenceNumber);
void complexDistanceChanged (double complexDistance);
};
#endif
src/MissionManager/ComplexMissionItemTest.cc
View file @ 957a4b1b
......@@ -45,7 +45,7 @@ void ComplexMissionItemTest::init(void)
_rgComplexMissionItemSignals[exitCoordinateHasRelativeAltitudeChangedIndex] = SIGNAL(exitCoordinateHasRelativeAltitudeChanged(bool));
_rgComplexMissionItemSignals[exitCoordinateSameAsEntryChangedIndex] = SIGNAL(exitCoordinateSameAsEntryChanged(bool));
_complexItem = new ComplexMissionItem(NULL /* Vehicle */, this);
_complexItem = new SurveyMissionItem(NULL /* Vehicle */, this);
// It's important to check that the right signals are emitted at the right time since that drives ui change.
// It's also important to check that things are not being over-signalled when they should not be, since that can lead
......
src/MissionManager/ComplexMissionItemTest.h
View file @ 957a4b1b
......@@ -14,7 +14,7 @@
#include "UnitTest.h"
#include "TCPLink.h"
#include "MultiSignalSpy.h"
#include "ComplexMissionItem.h"
#include "SurveyMissionItem.h"
#include <QGeoCoordinate>
......@@ -95,7 +95,7 @@ private:
const char* _rgComplexMissionItemSignals[_cComplexMissionItemSignals];
MultiSignalSpy* _multiSpy;
ComplexMissionItem* _complexItem;
SurveyMissionItem* _complexItem;
QList<QGeoCoordinate> _polyPoints;
};
......
src/MissionManager/MissionController.cc
View file @ 957a4b1b
......@@ -15,7 +15,7 @@
#include "FirmwarePlugin.h"
#include "QGCApplication.h"
#include "SimpleMissionItem.h"
#include "ComplexMissionItem.h"
#include "SurveyMissionItem.h"
#include "JsonHelper.h"
#include "ParameterLoader.h"
#include "QGroundControlQmlGlobal.h"
......@@ -200,7 +200,7 @@ int MissionController::insertSimpleMissionItem(QGeoCoordinate coordinate, int i)
int MissionController::insertComplexMissionItem(QGeoCoordinate coordinate, int i)
{
int sequenceNumber = _nextSequenceNumber();
ComplexMissionItem* newItem = new ComplexMissionItem(_activeVehicle, this);
SurveyMissionItem* newItem = new SurveyMissionItem(_activeVehicle, this);
newItem->setSequenceNumber(sequenceNumber);
newItem->setCoordinate(coordinate);
_initVisualItem(newItem);
......@@ -220,9 +220,7 @@ void MissionController::removeMissionItem(int index)
_deinitVisualItem(item);
if (!item->isSimpleItem()) {
ComplexMissionItem* complexItem = qobject_cast<ComplexMissionItem*>(_complexItems->removeOne(item));
if (complexItem) {
complexItem->deleteLater();
} else {
if (!complexItem) {
qWarning() << "Complex item missing";
}
}
......@@ -299,7 +297,7 @@ bool MissionController::_loadJsonMissionFile(const QByteArray& bytes, QmlObjectL
return false;
}
ComplexMissionItem* item = new ComplexMissionItem(_activeVehicle, this);
SurveyMissionItem* item = new SurveyMissionItem(_activeVehicle, this);
if (item->load(itemValue.toObject(), errorString)) {
qCDebug(MissionControllerLog) << "Json load: complex item start:stop" << item->sequenceNumber() << item->lastSequenceNumber();
complexItems->append(item);
......@@ -321,7 +319,7 @@ bool MissionController::_loadJsonMissionFile(const QByteArray& bytes, QmlObjectL
// If there is a complex item that should be next in sequence add it in
if (nextComplexItemIndex < complexItems->count()) {
ComplexMissionItem* complexItem = qobject_cast<ComplexMissionItem*>(complexItems->get(nextComplexItemIndex));
SurveyMissionItem* complexItem = qobject_cast<SurveyMissionItem*>(complexItems->get(nextComplexItemIndex));
if (complexItem->sequenceNumber() == nextSequenceNumber) {
qCDebug(MissionControllerLog) << "Json load: injecting complex item expectedSequence:actualSequence:" << nextSequenceNumber << complexItem->sequenceNumber();
......@@ -832,11 +830,11 @@ void MissionController::_recalcAltitudeRangeBearing()
}
}
} else {
missionDistance += qobject_cast<ComplexMissionItem*>(item)->surveyDistance();
missionDistance += qobject_cast<ComplexMissionItem*>(item)->complexDistance();
telemetryDistance = qobject_cast<ComplexMissionItem*>(item)->greatestDistanceTo(homeItem->exitCoordinate());
if (vtolCalc){
cruiseDistance += qobject_cast<ComplexMissionItem*>(item)->surveyDistance(); //assume all survey missions undertaken in cruise
cruiseDistance += qobject_cast<ComplexMissionItem*>(item)->complexDistance(); //assume all survey missions undertaken in cruise
}
}
......@@ -845,11 +843,11 @@ void MissionController::_recalcAltitudeRangeBearing()
}
}
else if (lastCoordinateItem == homeItem && !item->isSimpleItem()){
missionDistance += qobject_cast<ComplexMissionItem*>(item)->surveyDistance();
missionDistance += qobject_cast<ComplexMissionItem*>(item)->complexDistance();
missionMaxTelemetry = qobject_cast<ComplexMissionItem*>(item)->greatestDistanceTo(homeItem->exitCoordinate());
if (vtolCalc){
cruiseDistance += qobject_cast<ComplexMissionItem*>(item)->surveyDistance(); //assume all survey missions undertaken in cruise
cruiseDistance += qobject_cast<ComplexMissionItem*>(item)->complexDistance(); //assume all survey missions undertaken in cruise
}
}
lastCoordinateItem = item;
......@@ -1015,7 +1013,7 @@ void MissionController::_initVisualItem(VisualMissionItem* visualItem)
// We need to track changes of lastSequenceNumber so we can recalc sequence numbers for subsequence items
ComplexMissionItem* complexItem = qobject_cast<ComplexMissionItem*>(visualItem);
connect(complexItem, &ComplexMissionItem::lastSequenceNumberChanged, this, &MissionController::_recalcSequence);
connect(complexItem, &ComplexMissionItem::surveyDistanceChanged, this, &MissionController::_recalcAltitudeRangeBearing);
connect(complexItem, &ComplexMissionItem::complexDistanceChanged, this, &MissionController::_recalcAltitudeRangeBearing);
}
}
......
src/MissionManager/MissionItem.h
View file @ 957a4b1b
......@@ -26,7 +26,7 @@
#include "QGCLoggingCategory.h"
#include "QmlObjectListModel.h"
class ComplexMissionItem;
class SurveyMissionItem;
class SimpleMissionItem;
class MissionController;
#ifdef UNITTEST_BUILD
......@@ -127,7 +127,7 @@ private:
static const char* _jsonAutoContinueKey;
static const char* _jsonCoordinateKey;
friend class ComplexMissionItem;
friend class SurveyMissionItem;
friend class SimpleMissionItem;
friend class MissionController;
#ifdef UNITTEST_BUILD
......
src/MissionManager/SimpleMissionItem.cc
View file @ 957a4b1b
......@@ -126,7 +126,7 @@ SimpleMissionItem::SimpleMissionItem(const SimpleMissionItem& other, QObject* pa
const SimpleMissionItem& SimpleMissionItem::operator=(const SimpleMissionItem& other)
{
static_cast<VisualMissionItem&>(*this) = other;
VisualMissionItem::operator=(other);
setRawEdit(other._rawEdit);
setDirty(other._dirty);
......
src/MissionManager/SurveyMissionItem.cc 0 → 100644
View file @ 957a4b1b
/****************************************************************************
*
* (c) 2009-2016 QGROUNDCONTROL PROJECT <http://www.qgroundcontrol.org>
*
* QGroundControl is licensed according to the terms in the file
* COPYING.md in the root of the source code directory.
*
****************************************************************************/
#include "SurveyMissionItem.h"
#include "JsonHelper.h"
#include "MissionController.h"
#include "QGCGeo.h"
#include <QPolygonF>
QGC_LOGGING_CATEGORY(SurveyMissionItemLog, "SurveyMissionItemLog")
const char* SurveyMissionItem::_jsonVersionKey = "version";
const char* SurveyMissionItem::_jsonTypeKey = "type";
const char* SurveyMissionItem::_jsonPolygonKey = "polygon";
const char* SurveyMissionItem::_jsonIdKey = "id";
const char* SurveyMissionItem::_jsonGridAltitudeKey = "gridAltitude";
const char* SurveyMissionItem::_jsonGridAltitudeRelativeKey = "gridAltitudeRelative";
const char* SurveyMissionItem::_jsonGridAngleKey = "gridAngle";
const char* SurveyMissionItem::_jsonGridSpacingKey = "gridSpacing";
const char* SurveyMissionItem::_jsonCameraTriggerKey = "cameraTrigger";
const char* SurveyMissionItem::_jsonCameraTriggerDistanceKey = "cameraTriggerDistance";
const char* SurveyMissionItem::_complexType = "survey";
SurveyMissionItem::SurveyMissionItem(Vehicle* vehicle, QObject* parent)
: ComplexMissionItem(vehicle, parent)
, _sequenceNumber(0)
, _dirty(false)
, _cameraTrigger(false)
, _gridAltitudeRelative(true)
, _surveyDistance(0.0)
, _cameraShots(0)
, _coveredArea(0.0)
, _gridAltitudeFact (0, "Altitude:", FactMetaData::valueTypeDouble)
, _gridAngleFact (0, "Grid angle:", FactMetaData::valueTypeDouble)
, _gridSpacingFact (0, "Grid spacing:", FactMetaData::valueTypeDouble)
, _cameraTriggerDistanceFact(0, "Camera trigger distance", FactMetaData::valueTypeDouble)
{
_gridAltitudeFact.setRawValue(25);
_gridSpacingFact.setRawValue(10);
_cameraTriggerDistanceFact.setRawValue(25);
connect(&_gridSpacingFact, &Fact::valueChanged, this, &SurveyMissionItem::_generateGrid);
connect(&_gridAngleFact, &Fact::valueChanged, this, &SurveyMissionItem::_generateGrid);
connect(&_cameraTriggerDistanceFact, &Fact::valueChanged, this, &SurveyMissionItem::_generateGrid);
connect(this, &SurveyMissionItem::cameraTriggerChanged, this, &SurveyMissionItem::_cameraTriggerChanged);
}
const SurveyMissionItem& SurveyMissionItem::operator=(const SurveyMissionItem& other)
{
ComplexMissionItem::operator=(other);
_setSurveyDistance(other._surveyDistance);
_setCameraShots(other._cameraShots);
_setCoveredArea(other._coveredArea);
return *this;
}
void SurveyMissionItem::_setSurveyDistance(double surveyDistance)
{
if (!qFuzzyCompare(_surveyDistance, surveyDistance)) {
_surveyDistance = surveyDistance;
emit complexDistanceChanged(_surveyDistance);
}
}
void SurveyMissionItem::_setCameraShots(int cameraShots)
{
if (_cameraShots != cameraShots) {
_cameraShots = cameraShots;
emit cameraShotsChanged(_cameraShots);
}
}
void SurveyMissionItem::_setCoveredArea(double coveredArea)
{
if (!qFuzzyCompare(_coveredArea, coveredArea)) {
_coveredArea = coveredArea;
emit coveredAreaChanged(_coveredArea);
}
}
void SurveyMissionItem::clearPolygon(void)
{
// Bug workaround, see below
while (_polygonPath.count() > 1) {
_polygonPath.takeLast();
}
emit polygonPathChanged();
// Although this code should remove the polygon from the map it doesn't. There appears
// to be a bug in MapPolygon which causes it to not be redrawn if the list is empty. So
// we work around it by using the code above to remove all but the last point which in turn
// will cause the polygon to go away.
_polygonPath.clear();
_clearGrid();
setDirty(true);
emit specifiesCoordinateChanged();
emit lastSequenceNumberChanged(lastSequenceNumber());
}
void SurveyMissionItem::addPolygonCoordinate(const QGeoCoordinate coordinate)
{
_polygonPath << QVariant::fromValue(coordinate);
emit polygonPathChanged();
int pointCount = _polygonPath.count();
if (pointCount >= 3) {
if (pointCount == 3) {
emit specifiesCoordinateChanged();
}
_generateGrid();
}
setDirty(true);
}
void SurveyMissionItem::adjustPolygonCoordinate(int vertexIndex, const QGeoCoordinate coordinate)
{
_polygonPath[vertexIndex] = QVariant::fromValue(coordinate);
emit polygonPathChanged();
_generateGrid();
setDirty(true);
}
int SurveyMissionItem::lastSequenceNumber(void) const
{
int lastSeq = _sequenceNumber;
if (_gridPoints.count()) {
lastSeq += _gridPoints.count() - 1;
if (_cameraTrigger) {
// Account for two trigger messages
lastSeq += 2;
}
}
return lastSeq;
}
void SurveyMissionItem::setCoordinate(const QGeoCoordinate& coordinate)
{
if (_coordinate != coordinate) {
_coordinate = coordinate;
emit coordinateChanged(_coordinate);
}
}
void SurveyMissionItem::setDirty(bool dirty)
{
if (_dirty != dirty) {
_dirty = dirty;
emit dirtyChanged(_dirty);
}
}
void SurveyMissionItem::save(QJsonObject& saveObject) const
{
saveObject[_jsonVersionKey] = 1;
saveObject[_jsonTypeKey] = _complexType;
saveObject[_jsonIdKey] = sequenceNumber();
saveObject[_jsonGridAltitudeKey] = _gridAltitudeFact.rawValue().toDouble();
saveObject[_jsonGridAltitudeRelativeKey] = _gridAltitudeRelative;
saveObject[_jsonGridAngleKey] = _gridAngleFact.rawValue().toDouble();
saveObject[_jsonGridSpacingKey] = _gridSpacingFact.rawValue().toDouble();
saveObject[_jsonCameraTriggerKey] = _cameraTrigger;
saveObject[_jsonCameraTriggerDistanceKey] = _cameraTriggerDistanceFact.rawValue().toDouble();
// Polygon shape
QJsonArray polygonArray;
for (int i=0; i<_polygonPath.count(); i++) {
const QVariant& polyVar = _polygonPath[i];
QJsonValue jsonValue;
JsonHelper::writeQGeoCoordinate(jsonValue, polyVar.value<QGeoCoordinate>(), false /* writeAltitude */);
polygonArray += jsonValue;
}
saveObject[_jsonPolygonKey] = polygonArray;
}
void SurveyMissionItem::setSequenceNumber(int sequenceNumber)
{
if (_sequenceNumber != sequenceNumber) {
_sequenceNumber = sequenceNumber;
emit sequenceNumberChanged(sequenceNumber);
emit lastSequenceNumberChanged(lastSequenceNumber());
}
}
void SurveyMissionItem::_clear(void)
{
clearPolygon();
_clearGrid();
}
bool SurveyMissionItem::load(const QJsonObject& complexObject, QString& errorString)
{
_clear();
// Validate requires keys
QStringList requiredKeys;
requiredKeys << _jsonVersionKey << _jsonTypeKey << _jsonIdKey << _jsonPolygonKey << _jsonGridAltitudeKey << _jsonGridAngleKey << _jsonGridSpacingKey <<
_jsonCameraTriggerKey << _jsonCameraTriggerDistanceKey << _jsonGridAltitudeRelativeKey;
if (!JsonHelper::validateRequiredKeys(complexObject, requiredKeys, errorString)) {
_clear();
return false;
}
// Validate types
QStringList keyList;
QList<QJsonValue::Type> typeList;
keyList << _jsonVersionKey << _jsonTypeKey << _jsonIdKey << _jsonPolygonKey << _jsonGridAltitudeKey << _jsonGridAngleKey << _jsonGridSpacingKey <<
_jsonCameraTriggerKey << _jsonCameraTriggerDistanceKey << _jsonGridAltitudeRelativeKey;
typeList << QJsonValue::Double << QJsonValue::String << QJsonValue::Double << QJsonValue::Array << QJsonValue::Double << QJsonValue::Double<< QJsonValue::Double <<
QJsonValue::Bool << QJsonValue::Double << QJsonValue::Bool;
if (!JsonHelper::validateKeyTypes(complexObject, keyList, typeList, errorString)) {
_clear();
return false;
}
// Version check
if (complexObject[_jsonVersionKey].toInt() != 1) {
errorString = tr("QGroundControl does not support this version of survey items");
_clear();
return false;
}
QString complexType = complexObject[_jsonTypeKey].toString();
if (complexType != _complexType) {
errorString = tr("QGroundControl does not support loading this complex mission item type: %1").arg(complexType);
_clear();
return false;
}
setSequenceNumber(complexObject[_jsonIdKey].toInt());
_cameraTrigger = complexObject[_jsonCameraTriggerKey].toBool();
_gridAltitudeRelative = complexObject[_jsonGridAltitudeRelativeKey].toBool();
_gridAltitudeFact.setRawValue (complexObject[_jsonGridAltitudeKey].toDouble());
_gridAngleFact.setRawValue (complexObject[_jsonGridAngleKey].toDouble());
_gridSpacingFact.setRawValue (complexObject[_jsonGridSpacingKey].toDouble());
_cameraTriggerDistanceFact.setRawValue (complexObject[_jsonCameraTriggerDistanceKey].toDouble());
// Polygon shape
QJsonArray polygonArray(complexObject[_jsonPolygonKey].toArray());
for (int i=0; i<polygonArray.count(); i++) {
const QJsonValue& pointValue = polygonArray[i];
QGeoCoordinate pointCoord;
if (!JsonHelper::toQGeoCoordinate(pointValue, pointCoord, false /* altitudeRequired */, errorString)) {
_clear();
return false;
}
_polygonPath << QVariant::fromValue(pointCoord);
}
_generateGrid();
return true;
}
double SurveyMissionItem::greatestDistanceTo(const QGeoCoordinate &other) const
{
double greatestDistance = 0.0;
for (int i=0; i<_gridPoints.count(); i++) {
QGeoCoordinate currentCoord = _gridPoints[i].value<QGeoCoordinate>();
double distance = currentCoord.distanceTo(other);
if (distance > greatestDistance) {
greatestDistance = distance;
}
}
return greatestDistance;
}
void SurveyMissionItem::_setExitCoordinate(const QGeoCoordinate& coordinate)
{
if (_exitCoordinate != coordinate) {
_exitCoordinate = coordinate;
emit exitCoordinateChanged(coordinate);
}
}
bool SurveyMissionItem::specifiesCoordinate(void) const
{
return _polygonPath.count() > 2;
}
void SurveyMissionItem::_clearGrid(void)
{
// Bug workaround
while (_gridPoints.count() > 1) {
_gridPoints.takeLast();
}
emit gridPointsChanged();
_gridPoints.clear();
}
void SurveyMissionItem::_generateGrid(void)
{
if (_polygonPath.count() < 3) {
_clearGrid();
return;
}
_gridPoints.clear();
QList<QPointF> polygonPoints;
QList<QPointF> gridPoints;
// Convert polygon to Qt coordinate system (y positive is down)
qCDebug(SurveyMissionItemLog) << "Convert polygon";
QGeoCoordinate tangentOrigin = _polygonPath[0].value<QGeoCoordinate>();
for (int i=0; i<_polygonPath.count(); i++) {
double y, x, down;
convertGeoToNed(_polygonPath[i].value<QGeoCoordinate>(), tangentOrigin, &y, &x, &down);
polygonPoints += QPointF(x, -y);
qCDebug(SurveyMissionItemLog) << _polygonPath[i].value<QGeoCoordinate>() << polygonPoints.last().x() << polygonPoints.last().y();
}
double coveredArea = 0.0;
for (int i=0; i<polygonPoints.count(); i++) {
if (i != 0) {
coveredArea += polygonPoints[i - 1].x() * polygonPoints[i].y() - polygonPoints[i].x() * polygonPoints[i -1].y();
} else {
coveredArea += polygonPoints.last().x() * polygonPoints[i].y() - polygonPoints[i].x() * polygonPoints.last().y();
}
}
_setCoveredArea(0.5 * fabs(coveredArea));
// Generate grid
_gridGenerator(polygonPoints, gridPoints);
double surveyDistance = 0.0;
// Convert to Geo and set altitude
for (int i=0; i<gridPoints.count(); i++) {
QPointF& point = gridPoints[i];
if (i != 0) {
surveyDistance += sqrt(pow((gridPoints[i] - gridPoints[i - 1]).x(),2.0) + pow((gridPoints[i] - gridPoints[i - 1]).y(),2.0));
}
QGeoCoordinate geoCoord;
convertNedToGeo(-point.y(), point.x(), 0, tangentOrigin, &geoCoord);
_gridPoints += QVariant::fromValue(geoCoord);
}
_setSurveyDistance(surveyDistance);
_setCameraShots((int)floor(surveyDistance / _cameraTriggerDistanceFact.rawValue().toDouble()));
emit gridPointsChanged();
emit lastSequenceNumberChanged(lastSequenceNumber());
if (_gridPoints.count()) {
setCoordinate(_gridPoints.first().value<QGeoCoordinate>());
_setExitCoordinate(_gridPoints.last().value<QGeoCoordinate>());
}
}
QPointF SurveyMissionItem::_rotatePoint(const QPointF& point, const QPointF& origin, double angle)
{
QPointF rotated;
double radians = (M_PI / 180.0) * angle;
rotated.setX(((point.x() - origin.x()) * cos(radians)) - ((point.y() - origin.y()) * sin(radians)) + origin.x());
rotated.setY(((point.x() - origin.x()) * sin(radians)) + ((point.y() - origin.y()) * cos(radians)) + origin.y());
return rotated;
}
void SurveyMissionItem::_intersectLinesWithRect(const QList<QLineF>& lineList, const QRectF& boundRect, QList<QLineF>& resultLines)
{
QLineF topLine (boundRect.topLeft(), boundRect.topRight());
QLineF bottomLine (boundRect.bottomLeft(), boundRect.bottomRight());
QLineF leftLine (boundRect.topLeft(), boundRect.bottomLeft());
QLineF rightLine (boundRect.topRight(), boundRect.bottomRight());
for (int i=0; i<lineList.count(); i++) {
QPointF intersectPoint;
QLineF intersectLine;
const QLineF& line = lineList[i];
int foundCount = 0;
if (line.intersect(topLine, &intersectPoint) == QLineF::BoundedIntersection) {
intersectLine.setP1(intersectPoint);
foundCount++;
}
if (line.intersect(rightLine, &intersectPoint) == QLineF::BoundedIntersection) {
if (foundCount == 0) {
intersectLine.setP1(intersectPoint);
} else {
if (foundCount != 1) {
qWarning() << "Found more than two intersecting points";
}
intersectLine.setP2(intersectPoint);
}
foundCount++;
}
if (line.intersect(bottomLine, &intersectPoint) == QLineF::BoundedIntersection) {
if (foundCount == 0) {
intersectLine.setP1(intersectPoint);
} else {
if (foundCount != 1) {
qWarning() << "Found more than two intersecting points";
}
intersectLine.setP2(intersectPoint);
}
foundCount++;
}
if (line.intersect(leftLine, &intersectPoint) == QLineF::BoundedIntersection) {
if (foundCount == 0) {
intersectLine.setP1(intersectPoint);
} else {
if (foundCount != 1) {
qWarning() << "Found more than two intersecting points";
}
intersectLine.setP2(intersectPoint);
}
foundCount++;
}
if (foundCount == 2) {
resultLines += intersectLine;
}
}
}
void SurveyMissionItem::_intersectLinesWithPolygon(const QList<QLineF>& lineList, const QPolygonF& polygon, QList<QLineF>& resultLines)
{
for (int i=0; i<lineList.count(); i++) {
int foundCount = 0;
QLineF intersectLine;
const QLineF& line = lineList[i];
for (int j=0; j<polygon.count()-1; j++) {
QPointF intersectPoint;
QLineF polygonLine = QLineF(polygon[j], polygon[j+1]);
if (line.intersect(polygonLine, &intersectPoint) == QLineF::BoundedIntersection) {
if (foundCount == 0) {
foundCount++;
intersectLine.setP1(intersectPoint);
} else {
foundCount++;
intersectLine.setP2(intersectPoint);
break;
}
}
}
if (foundCount == 2) {
resultLines += intersectLine;
}
}
}
/// Adjust the line segments such that they are all going the same direction with respect to going from P1->P2
void SurveyMissionItem::_adjustLineDirection(const QList<QLineF>& lineList, QList<QLineF>& resultLines)
{
for (int i=0; i<lineList.count(); i++) {
const QLineF& line = lineList[i];
QLineF adjustedLine;
if (line.angle() > 180.0) {
adjustedLine.setP1(line.p2());
adjustedLine.setP2(line.p1());
} else {
adjustedLine = line;
}
resultLines += adjustedLine;
}
}
void SurveyMissionItem::_gridGenerator(const QList<QPointF>& polygonPoints, QList<QPointF>& gridPoints)
{
double gridAngle = _gridAngleFact.rawValue().toDouble();
gridPoints.clear();
// Convert polygon to bounding rect
qCDebug(SurveyMissionItemLog) << "Polygon";
QPolygonF polygon;
for (int i=0; i<polygonPoints.count(); i++) {
qCDebug(SurveyMissionItemLog) << polygonPoints[i];
polygon << polygonPoints[i];
}
polygon << polygonPoints[0];
QRectF smallBoundRect = polygon.boundingRect();
QPointF center = smallBoundRect.center();
qCDebug(SurveyMissionItemLog) << "Bounding rect" << smallBoundRect.topLeft().x() << smallBoundRect.topLeft().y() << smallBoundRect.bottomRight().x() << smallBoundRect.bottomRight().y();
// Rotate the bounding rect around it's center to generate the larger bounding rect
QPolygonF boundPolygon;
boundPolygon << _rotatePoint(smallBoundRect.topLeft(), center, gridAngle);
boundPolygon << _rotatePoint(smallBoundRect.topRight(), center, gridAngle);
boundPolygon << _rotatePoint(smallBoundRect.bottomRight(), center, gridAngle);
boundPolygon << _rotatePoint(smallBoundRect.bottomLeft(), center, gridAngle);
boundPolygon << boundPolygon[0];
QRectF largeBoundRect = boundPolygon.boundingRect();
qCDebug(SurveyMissionItemLog) << "Rotated bounding rect" << largeBoundRect.topLeft().x() << largeBoundRect.topLeft().y() << largeBoundRect.bottomRight().x() << largeBoundRect.bottomRight().y();
// Create set of rotated parallel lines within the expanded bounding rect. Make the lines larger than the
// bounding box to guarantee intersection.
QList<QLineF> lineList;
float x = largeBoundRect.topLeft().x();
float gridSpacing = _gridSpacingFact.rawValue().toDouble();
while (x < largeBoundRect.bottomRight().x()) {
float yTop = largeBoundRect.topLeft().y() - 100.0;
float yBottom = largeBoundRect.bottomRight().y() + 100.0;
lineList += QLineF(_rotatePoint(QPointF(x, yTop), center, gridAngle), _rotatePoint(QPointF(x, yBottom), center, gridAngle));
qCDebug(SurveyMissionItemLog) << "line(" << lineList.last().x1() << ", " << lineList.last().y1() << ")-(" << lineList.last().x2() <<", " << lineList.last().y2() << ")";
x += gridSpacing;
}
// Now intersect the lines with the polygon
QList<QLineF> intersectLines;
#if 1
_intersectLinesWithPolygon(lineList, polygon, intersectLines);
#else
// This is handy for debugging grid problems, not for release
intersectLines = lineList;
#endif
// Make sure all lines are going to same direction. Polygon intersection leads to line which
// can be in varied directions depending on the order of the intesecting sides.
QList<QLineF> resultLines;
_adjustLineDirection(intersectLines, resultLines);
// Turn into a path
for (int i=0; i<resultLines.count(); i++) {
const QLineF& line = resultLines[i];
if (i & 1) {
gridPoints << line.p2() << line.p1();
} else {
gridPoints << line.p1() << line.p2();
}
}
}
QmlObjectListModel* SurveyMissionItem::getMissionItems(void) const
{
QmlObjectListModel* pMissionItems = new QmlObjectListModel;
int seqNum = _sequenceNumber;
for (int i=0; i<_gridPoints.count(); i++) {
QGeoCoordinate coord = _gridPoints[i].value<QGeoCoordinate>();
double altitude = _gridAltitudeFact.rawValue().toDouble();
MissionItem* item = new MissionItem(seqNum++, // sequence number
MAV_CMD_NAV_WAYPOINT, // MAV_CMD
_gridAltitudeRelative ? MAV_FRAME_GLOBAL_RELATIVE_ALT : MAV_FRAME_GLOBAL, // MAV_FRAME
0.0, 0.0, 0.0, 0.0, // param 1-4
coord.latitude(),
coord.longitude(),
altitude,
true, // autoContinue
false, // isCurrentItem
pMissionItems); // parent - allow delete on pMissionItems to delete everthing
pMissionItems->append(item);
if (_cameraTrigger && i == 0) {
MissionItem* item = new MissionItem(seqNum++, // sequence number
MAV_CMD_DO_SET_CAM_TRIGG_DIST, // MAV_CMD
MAV_FRAME_MISSION, // MAV_FRAME
_cameraTriggerDistanceFact.rawValue().toDouble(), // trigger distance
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, // param 2-7
true, // autoContinue
false, // isCurrentItem
pMissionItems); // parent - allow delete on pMissionItems to delete everthing
pMissionItems->append(item);
}
}
if (_cameraTrigger) {
MissionItem* item = new MissionItem(seqNum++, // sequence number
MAV_CMD_DO_SET_CAM_TRIGG_DIST, // MAV_CMD
MAV_FRAME_MISSION, // MAV_FRAME
0.0, // trigger distance
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, // param 2-7
true, // autoContinue
false, // isCurrentItem
pMissionItems); // parent - allow delete on pMissionItems to delete everthing
pMissionItems->append(item);
}
return pMissionItems;
}
void SurveyMissionItem::_cameraTriggerChanged(void)
{
setDirty(true);
if (_gridPoints.count()) {
// If we have grid turn on/off camera trigger will add/remove two camera trigger mission items
emit lastSequenceNumberChanged(lastSequenceNumber());
}
}
src/MissionManager/SurveyMissionItem.h 0 → 100644
View file @ 957a4b1b
/****************************************************************************
*
* (c) 2009-2016 QGROUNDCONTROL PROJECT <http://www.qgroundcontrol.org>
*
* QGroundControl is licensed according to the terms in the file
* COPYING.md in the root of the source code directory.
*
****************************************************************************/
#ifndef SurveyMissionItem_H
#define SurveyMissionItem_H
#include "ComplexMissionItem.h"
#include "MissionItem.h"
#include "Fact.h"
#include "QGCLoggingCategory.h"
Q_DECLARE_LOGGING_CATEGORY(SurveyMissionItemLog)
class SurveyMissionItem : public ComplexMissionItem
{
Q_OBJECT
public:
SurveyMissionItem(Vehicle* vehicle, QObject* parent = NULL);
const SurveyMissionItem& operator=(const SurveyMissionItem& other);
Q_PROPERTY(Fact* gridAltitude READ gridAltitude CONSTANT)
Q_PROPERTY(bool gridAltitudeRelative MEMBER _gridAltitudeRelative NOTIFY gridAltitudeRelativeChanged)
Q_PROPERTY(Fact* gridAngle READ gridAngle CONSTANT)
Q_PROPERTY(Fact* gridSpacing READ gridSpacing CONSTANT)
Q_PROPERTY(bool cameraTrigger MEMBER _cameraTrigger NOTIFY cameraTriggerChanged)
Q_PROPERTY(Fact* cameraTriggerDistance READ cameraTriggerDistance CONSTANT)
Q_PROPERTY(QVariantList polygonPath READ polygonPath NOTIFY polygonPathChanged)
Q_PROPERTY(QVariantList gridPoints READ gridPoints NOTIFY gridPointsChanged)
Q_PROPERTY(int cameraShots READ cameraShots NOTIFY cameraShotsChanged)
Q_PROPERTY(double coveredArea READ coveredArea NOTIFY coveredAreaChanged)
Q_INVOKABLE void clearPolygon(void);
Q_INVOKABLE void addPolygonCoordinate(const QGeoCoordinate coordinate);
Q_INVOKABLE void adjustPolygonCoordinate(int vertexIndex, const QGeoCoordinate coordinate);
QVariantList polygonPath(void) { return _polygonPath; }
QVariantList gridPoints (void) { return _gridPoints; }
Fact* gridAltitude(void) { return &_gridAltitudeFact; }
Fact* gridAngle(void) { return &_gridAngleFact; }
Fact* gridSpacing(void) { return &_gridSpacingFact; }
Fact* cameraTriggerDistance(void) { return &_cameraTriggerDistanceFact; }
int cameraShots (void) const { return _cameraShots; }
double coveredArea (void) const { return _coveredArea; }
// Overrides from ComplexMissionItem
double complexDistance (void) const final { return _surveyDistance; }
int lastSequenceNumber (void) const final;
QmlObjectListModel* getMissionItems (void) const final;
bool load (const QJsonObject& complexObject, QString& errorString) final;
double greatestDistanceTo (const QGeoCoordinate &other) const final;
// Overrides from VisualMissionItem
bool dirty (void) const final { return _dirty; }
bool isSimpleItem (void) const final { return false; }
bool isStandaloneCoordinate (void) const final { return false; }
bool specifiesCoordinate (void) const final;
QString commandDescription (void) const final { return "Survey"; }
QString commandName (void) const final { return "Survey"; }
QString abbreviation (void) const final { return "S"; }
QGeoCoordinate coordinate (void) const final { return _coordinate; }
QGeoCoordinate exitCoordinate (void) const final { return _exitCoordinate; }
int sequenceNumber (void) const final { return _sequenceNumber; }
bool coordinateHasRelativeAltitude (void) const final { return _gridAltitudeRelative; }
bool exitCoordinateHasRelativeAltitude (void) const final { return _gridAltitudeRelative; }
bool exitCoordinateSameAsEntry (void) const final { return false; }
void setDirty (bool dirty) final;
void setCoordinate (const QGeoCoordinate& coordinate) final;
void setSequenceNumber (int sequenceNumber) final;
void save (QJsonObject& saveObject) const final;
signals:
void polygonPathChanged (void);
void altitudeChanged (double altitude);
void gridAngleChanged (double gridAngle);
void gridPointsChanged (void);
void cameraTriggerChanged (bool cameraTrigger);
void gridAltitudeRelativeChanged (bool gridAltitudeRelative);
void cameraShotsChanged (int cameraShots);
void coveredAreaChanged (double coveredArea);
private slots:
void _cameraTriggerChanged(void);
private:
void _clear(void);
void _setExitCoordinate(const QGeoCoordinate& coordinate);
void _clearGrid(void);
void _generateGrid(void);
void _gridGenerator(const QList<QPointF>& polygonPoints, QList<QPointF>& gridPoints);
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);
void _adjustLineDirection(const QList<QLineF>& lineList, QList<QLineF>& resultLines);
void _setSurveyDistance(double surveyDistance);
void _setCameraShots(int cameraShots);
void _setCoveredArea(double coveredArea);
int _sequenceNumber;
bool _dirty;
QVariantList _polygonPath;
QVariantList _gridPoints;
QGeoCoordinate _coordinate;
QGeoCoordinate _exitCoordinate;
double _altitude;
double _gridAngle;
bool _cameraTrigger;
bool _gridAltitudeRelative;
double _surveyDistance;
int _cameraShots;
double _coveredArea;
Fact _gridAltitudeFact;
Fact _gridAngleFact;
Fact _gridSpacingFact;
Fact _cameraTriggerDistanceFact;
static const char* _jsonVersionKey;
static const char* _jsonTypeKey;
static const char* _jsonPolygonKey;
static const char* _jsonIdKey;
static const char* _jsonGridAltitudeKey;
static const char* _jsonGridAltitudeRelativeKey;
static const char* _jsonGridAngleKey;
static const char* _jsonGridSpacingKey;
static const char* _jsonCameraTriggerKey;
static const char* _jsonCameraTriggerDistanceKey;
static const char* _complexType;
};
#endif
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