diff --git a/qgroundcontrol.pro b/qgroundcontrol.pro index d213b17bf05e174a0f0ac7f9d4a2edd11157bd88..80e4987b33a54ba3fa51758dc7211d9e6c0cf7f0 100644 --- a/qgroundcontrol.pro +++ b/qgroundcontrol.pro @@ -267,13 +267,14 @@ HEADERS += \ src/JsonHelper.h \ src/LogCompressor.h \ src/MG.h \ + src/MissionManager/ComplexMissionItem.h \ src/MissionManager/MissionCommandList.h \ src/MissionManager/MissionCommands.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 \ @@ -426,13 +427,14 @@ SOURCES += \ src/FollowMe/FollowMe.cc \ src/LogCompressor.cc \ src/main.cc \ + src/MissionManager/ComplexMissionItem.cc \ src/MissionManager/MissionCommandList.cc \ src/MissionManager/MissionCommands.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 \ diff --git a/src/MissionManager/ComplexMissionItem.cc b/src/MissionManager/ComplexMissionItem.cc index a2b58a5becb3696daac17535c990ccde201b39cc..04f76ecc5dbd67c55051aeb12ffeda9b5d66829c 100644 --- a/src/MissionManager/ComplexMissionItem.cc +++ b/src/MissionManager/ComplexMissionItem.cc @@ -9,612 +9,16 @@ #include "ComplexMissionItem.h" -#include "JsonHelper.h" -#include "MissionController.h" -#include "QGCGeo.h" - -#include - -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(), 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 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(); - 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 polygonPoints; - QList gridPoints; - - // Convert polygon to Qt coordinate system (y positive is down) - qCDebug(ComplexMissionItemLog) << "Convert polygon"; - QGeoCoordinate tangentOrigin = _polygonPath[0].value(); - for (int i=0; i<_polygonPath.count(); i++) { - double y, x, down; - convertGeoToNed(_polygonPath[i].value(), tangentOrigin, &y, &x, &down); - polygonPoints += QPointF(x, -y); - qCDebug(ComplexMissionItemLog) << _polygonPath[i].value() << polygonPoints.last().x() << polygonPoints.last().y(); - } - - double coveredArea = 0.0; - for (int i=0; i()); - _setExitCoordinate(_gridPoints.last().value()); - } -} - -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& lineList, const QRectF& boundRect, QList& 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, const QPolygonF& polygon, QList& resultLines) -{ - for (int i=0; iP2 -void ComplexMissionItem::_adjustLineDirection(const QList& lineList, QList& resultLines) -{ - for (int i=0; i 180.0) { - adjustedLine.setP1(line.p2()); - adjustedLine.setP2(line.p1()); - } else { - adjustedLine = line; - } - - resultLines += adjustedLine; - } -} - -void ComplexMissionItem::_gridGenerator(const QList& polygonPoints, QList& gridPoints) -{ - double gridAngle = _gridAngleFact.rawValue().toDouble(); - - gridPoints.clear(); - - // Convert polygon to bounding rect - - qCDebug(ComplexMissionItemLog) << "Polygon"; - QPolygonF polygon; - for (int i=0; i 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 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 resultLines; - _adjustLineDirection(intersectLines, resultLines); - - // Turn into a path - for (int i=0; i(); - 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()); - } -} diff --git a/src/MissionManager/ComplexMissionItem.h b/src/MissionManager/ComplexMissionItem.h index 7396bf18973a0ef76faacaaa6f561fb345532918..d3e7e327c6e2fa5bac609b57e4a344353df1d086 100644 --- a/src/MissionManager/ComplexMissionItem.h +++ b/src/MissionManager/ComplexMissionItem.h @@ -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& polygonPoints, QList& gridPoints); - QPointF _rotatePoint(const QPointF& point, const QPointF& origin, double angle); - void _intersectLinesWithRect(const QList& lineList, const QRectF& boundRect, QList& resultLines); - void _intersectLinesWithPolygon(const QList& lineList, const QPolygonF& polygon, QList& resultLines); - void _adjustLineDirection(const QList& lineList, QList& 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 diff --git a/src/MissionManager/ComplexMissionItemTest.cc b/src/MissionManager/ComplexMissionItemTest.cc index 73491e73286d970dd68172f61d4c0cfd0bfcca48..5d7497822c68a81c7158833ca21fc37e24457a9a 100644 --- a/src/MissionManager/ComplexMissionItemTest.cc +++ b/src/MissionManager/ComplexMissionItemTest.cc @@ -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 diff --git a/src/MissionManager/ComplexMissionItemTest.h b/src/MissionManager/ComplexMissionItemTest.h index 76574868fad6166a537f4dc0e8a1d09ae0ec54ac..a2524399cfcb38f88fac7c3dfe02845ccb70a8cb 100644 --- a/src/MissionManager/ComplexMissionItemTest.h +++ b/src/MissionManager/ComplexMissionItemTest.h @@ -14,7 +14,7 @@ #include "UnitTest.h" #include "TCPLink.h" #include "MultiSignalSpy.h" -#include "ComplexMissionItem.h" +#include "SurveyMissionItem.h" #include @@ -95,7 +95,7 @@ private: const char* _rgComplexMissionItemSignals[_cComplexMissionItemSignals]; MultiSignalSpy* _multiSpy; - ComplexMissionItem* _complexItem; + SurveyMissionItem* _complexItem; QList _polyPoints; }; diff --git a/src/MissionManager/MissionController.cc b/src/MissionManager/MissionController.cc index 402975e315386a4f81197bd9021bd37f0f52d849..864d05ff87546d4ee8c20fbbc42c390473460b77 100644 --- a/src/MissionManager/MissionController.cc +++ b/src/MissionManager/MissionController.cc @@ -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(_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(complexItems->get(nextComplexItemIndex)); + SurveyMissionItem* complexItem = qobject_cast(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(item)->surveyDistance(); + missionDistance += qobject_cast(item)->complexDistance(); telemetryDistance = qobject_cast(item)->greatestDistanceTo(homeItem->exitCoordinate()); if (vtolCalc){ - cruiseDistance += qobject_cast(item)->surveyDistance(); //assume all survey missions undertaken in cruise + cruiseDistance += qobject_cast(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(item)->surveyDistance(); + missionDistance += qobject_cast(item)->complexDistance(); missionMaxTelemetry = qobject_cast(item)->greatestDistanceTo(homeItem->exitCoordinate()); if (vtolCalc){ - cruiseDistance += qobject_cast(item)->surveyDistance(); //assume all survey missions undertaken in cruise + cruiseDistance += qobject_cast(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(visualItem); connect(complexItem, &ComplexMissionItem::lastSequenceNumberChanged, this, &MissionController::_recalcSequence); - connect(complexItem, &ComplexMissionItem::surveyDistanceChanged, this, &MissionController::_recalcAltitudeRangeBearing); + connect(complexItem, &ComplexMissionItem::complexDistanceChanged, this, &MissionController::_recalcAltitudeRangeBearing); } } diff --git a/src/MissionManager/MissionItem.h b/src/MissionManager/MissionItem.h index e0ad7cf7130b87f182a3e980ed907cfe7a7a3c80..fa58ab58da3c8f4055821fc5f1b47060f1eb865a 100644 --- a/src/MissionManager/MissionItem.h +++ b/src/MissionManager/MissionItem.h @@ -27,7 +27,7 @@ #include "QmlObjectListModel.h" #include "MissionCommands.h" -class ComplexMissionItem; +class SurveyMissionItem; class SimpleMissionItem; class MissionController; #ifdef UNITTEST_BUILD @@ -128,7 +128,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 diff --git a/src/MissionManager/SimpleMissionItem.cc b/src/MissionManager/SimpleMissionItem.cc index 7f0e78e09b8dc74029b7a50b58802d28b409e74d..69bccbaae25b3c5e04a8db3274163b4473e6552c 100644 --- a/src/MissionManager/SimpleMissionItem.cc +++ b/src/MissionManager/SimpleMissionItem.cc @@ -124,7 +124,7 @@ SimpleMissionItem::SimpleMissionItem(const SimpleMissionItem& other, QObject* pa const SimpleMissionItem& SimpleMissionItem::operator=(const SimpleMissionItem& other) { - static_cast(*this) = other; + VisualMissionItem::operator=(other); setRawEdit(other._rawEdit); setDirty(other._dirty); diff --git a/src/MissionManager/SurveyMissionItem.cc b/src/MissionManager/SurveyMissionItem.cc new file mode 100644 index 0000000000000000000000000000000000000000..70f00e418cce63f74b9143d16a23b033131be0bd --- /dev/null +++ b/src/MissionManager/SurveyMissionItem.cc @@ -0,0 +1,614 @@ +/**************************************************************************** + * + * (c) 2009-2016 QGROUNDCONTROL PROJECT + * + * 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 + +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(), 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 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(); + 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 polygonPoints; + QList gridPoints; + + // Convert polygon to Qt coordinate system (y positive is down) + qCDebug(SurveyMissionItemLog) << "Convert polygon"; + QGeoCoordinate tangentOrigin = _polygonPath[0].value(); + for (int i=0; i<_polygonPath.count(); i++) { + double y, x, down; + convertGeoToNed(_polygonPath[i].value(), tangentOrigin, &y, &x, &down); + polygonPoints += QPointF(x, -y); + qCDebug(SurveyMissionItemLog) << _polygonPath[i].value() << polygonPoints.last().x() << polygonPoints.last().y(); + } + + double coveredArea = 0.0; + for (int i=0; i()); + _setExitCoordinate(_gridPoints.last().value()); + } +} + +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& lineList, const QRectF& boundRect, QList& 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, const QPolygonF& polygon, QList& resultLines) +{ + for (int i=0; iP2 +void SurveyMissionItem::_adjustLineDirection(const QList& lineList, QList& resultLines) +{ + for (int i=0; i 180.0) { + adjustedLine.setP1(line.p2()); + adjustedLine.setP2(line.p1()); + } else { + adjustedLine = line; + } + + resultLines += adjustedLine; + } +} + +void SurveyMissionItem::_gridGenerator(const QList& polygonPoints, QList& gridPoints) +{ + double gridAngle = _gridAngleFact.rawValue().toDouble(); + + gridPoints.clear(); + + // Convert polygon to bounding rect + + qCDebug(SurveyMissionItemLog) << "Polygon"; + QPolygonF polygon; + for (int i=0; i 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 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 resultLines; + _adjustLineDirection(intersectLines, resultLines); + + // Turn into a path + for (int i=0; i(); + 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()); + } +} diff --git a/src/MissionManager/SurveyMissionItem.h b/src/MissionManager/SurveyMissionItem.h new file mode 100644 index 0000000000000000000000000000000000000000..d82bd9dfce0a6e7b747473a08b7c78378aceaf59 --- /dev/null +++ b/src/MissionManager/SurveyMissionItem.h @@ -0,0 +1,147 @@ +/**************************************************************************** + * + * (c) 2009-2016 QGROUNDCONTROL PROJECT + * + * 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& polygonPoints, QList& gridPoints); + QPointF _rotatePoint(const QPointF& point, const QPointF& origin, double angle); + void _intersectLinesWithRect(const QList& lineList, const QRectF& boundRect, QList& resultLines); + void _intersectLinesWithPolygon(const QList& lineList, const QPolygonF& polygon, QList& resultLines); + void _adjustLineDirection(const QList& lineList, QList& 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