diff --git a/src/MissionManager/ComplexMissionItem.h b/src/MissionManager/ComplexMissionItem.h index bc27fe7f80856a77f750925fb21b734bcc5fab0a..fc4c4a63de59baa3ffb5f650d2b0c7a373e9c3fa 100644 --- a/src/MissionManager/ComplexMissionItem.h +++ b/src/MissionManager/ComplexMissionItem.h @@ -22,11 +22,16 @@ public: const ComplexMissionItem& operator=(const ComplexMissionItem& other); Q_PROPERTY(double complexDistance READ complexDistance NOTIFY complexDistanceChanged) + Q_PROPERTY(QRectF boundingBox READ boundingBox NOTIFY boundingBoxChanged) /// @return The distance covered the complex mission item in meters. /// Signals complexDistanceChanged virtual double complexDistance(void) const = 0; + /// @return The item bounding box (QRectf(West, Nort, East, South)) + /// Signals boundingBoxChanged + virtual QRectF boundingBox(void) const { return QRectF(); } + /// @return Amount of additional time delay in seconds needed to fly the complex item virtual double additionalTimeDelay(void) const { return 0; } @@ -48,6 +53,7 @@ public: signals: void complexDistanceChanged (void); + void boundingBoxChanged (void); void greatestDistanceToChanged (void); void additionalTimeDelayChanged (void); }; diff --git a/src/MissionManager/MissionController.cc b/src/MissionManager/MissionController.cc index b093c3f58acc82e934cbb2104249a54ee7e7dcd9..b0c1e8e0ada9dc21695650e777dfb10ab1073d19 100644 --- a/src/MissionManager/MissionController.cc +++ b/src/MissionManager/MissionController.cc @@ -174,7 +174,7 @@ void MissionController::_newMissionItemsAvailableFromVehicle(bool removeAllReque i = 1; } - for (; iappend(new SimpleMissionItem(_controllerVehicle, _editMode, *missionItem, this)); } diff --git a/src/MissionManager/MissionItem.cc b/src/MissionManager/MissionItem.cc index 9eeacb1078bc17109dfe462c5d8a87a821bd3780..4ac2b0fae97fd2e393a4bb35948eb197213181ef 100644 --- a/src/MissionManager/MissionItem.cc +++ b/src/MissionManager/MissionItem.cc @@ -418,6 +418,10 @@ void MissionItem::setCoordinate(const QGeoCoordinate& coordinate) QGeoCoordinate MissionItem::coordinate(void) const { + if(!isfinite(param5()) || !isfinite(param6())) { + //-- If either of these are NAN, return an invalid (QGeoCoordinate::isValid() == false) coordinate + return QGeoCoordinate(); + } return QGeoCoordinate(param5(), param6(), param7()); } diff --git a/src/MissionManager/PlanManager.cc b/src/MissionManager/PlanManager.cc index 29fad41b1e6ba53ba937bfdb1d903d423d8b1840..b8940897061a32af7748f13de9b4e2adec0ddd76 100644 --- a/src/MissionManager/PlanManager.cc +++ b/src/MissionManager/PlanManager.cc @@ -404,8 +404,8 @@ void PlanManager::_handleMissionItem(const mavlink_message_t& message, bool miss mavlink_msg_mission_item_int_decode(&message, &missionItem); command = (MAV_CMD)missionItem.command, - frame = (MAV_FRAME)missionItem.frame, - param1 = missionItem.param1; + frame = (MAV_FRAME)missionItem.frame, + param1 = missionItem.param1; param2 = missionItem.param2; param3 = missionItem.param3; param4 = missionItem.param4; @@ -420,8 +420,8 @@ void PlanManager::_handleMissionItem(const mavlink_message_t& message, bool miss mavlink_msg_mission_item_decode(&message, &missionItem); command = (MAV_CMD)missionItem.command, - frame = (MAV_FRAME)missionItem.frame, - param1 = missionItem.param1; + frame = (MAV_FRAME)missionItem.frame, + param1 = missionItem.param1; param2 = missionItem.param2; param3 = missionItem.param3; param4 = missionItem.param4; @@ -439,7 +439,6 @@ void PlanManager::_handleMissionItem(const mavlink_message_t& message, bool miss } else if (frame == MAV_FRAME_GLOBAL_RELATIVE_ALT_INT) { frame = MAV_FRAME_GLOBAL_RELATIVE_ALT; } - bool ardupilotHomePositionUpdate = false; if (!_checkForExpectedAck(AckMissionItem)) { diff --git a/src/MissionManager/SurveyMissionItem.cc b/src/MissionManager/SurveyMissionItem.cc index 1e8b5e9f4590bde163effb055a5212090f76e4b0..e2d36efc72a463e5006ce927883062a50c7999aa 100644 --- a/src/MissionManager/SurveyMissionItem.cc +++ b/src/MissionManager/SurveyMissionItem.cc @@ -163,6 +163,14 @@ void SurveyMissionItem::_setSurveyDistance(double surveyDistance) } } +void SurveyMissionItem::_setBoundingBox(QRectF bb) +{ + if (bb != _boundingBox) { + _boundingBox = bb; + emit boundingBoxChanged(); + } +} + void SurveyMissionItem::_setCameraShots(int cameraShots) { if (_cameraShots != cameraShots) { @@ -732,6 +740,22 @@ void SurveyMissionItem::_generateGrid(void) } _setSurveyDistance(surveyDistance); + // Calc bounding box + double north = 0.0; + double south = 180.0; + double east = 360.0; + double west = 0.0; + for (int i = 0; i < _simpleGridPoints.count(); i++) { + QGeoCoordinate coord = _simpleGridPoints[i].value(); + double lat = coord.latitude() + 90.0; + double lon = coord.longitude() + 180.0; + north = fmax(north, lat); + south = fmin(south, lat); + east = fmax(east, lon); + west = fmin(west, lon); + } + _setBoundingBox(QRectF(east - 180.0, north - 90.0, west - 180.0, south - 90.0)); + if (cameraShots == 0 && _triggerCamera()) { cameraShots = (int)floor(surveyDistance / _triggerDistance()); // Take into account immediate camera trigger at waypoint entry diff --git a/src/MissionManager/SurveyMissionItem.h b/src/MissionManager/SurveyMissionItem.h index d2096ea1fe2d3a86b4bbacd0c3e575ca05d15492..09c2085626f261333160d01ac6a9199298f3a865 100644 --- a/src/MissionManager/SurveyMissionItem.h +++ b/src/MissionManager/SurveyMissionItem.h @@ -96,6 +96,7 @@ public: // Overrides from ComplexMissionItem double complexDistance (void) const final { return _surveyDistance; } + QRectF boundingBox (void) const final { return _boundingBox; } double additionalTimeDelay (void) const final { return _additionalFlightDelaySeconds; } int lastSequenceNumber (void) const final; bool load (const QJsonObject& complexObject, int sequenceNumber, QString& errorString) final; @@ -199,6 +200,7 @@ private: void _intersectLinesWithPolygon(const QList& lineList, const QPolygonF& polygon, QList& resultLines); void _adjustLineDirection(const QList& lineList, QList& resultLines); void _setSurveyDistance(double surveyDistance); + void _setBoundingBox(QRectF bb); void _setCameraShots(int cameraShots); void _setCoveredArea(double coveredArea); void _cameraValueChanged(void); @@ -241,6 +243,7 @@ private: bool _ignoreRecalc; double _surveyDistance; + QRectF _boundingBox; int _cameraShots; double _coveredArea; double _timeBetweenShots;