Commit 2cc0a608 authored by Gus Grubba's avatar Gus Grubba

ComplexMissionItem (SurveyMissionItem) bounding box

parent ff21304c
......@@ -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);
};
......
......@@ -174,7 +174,7 @@ void MissionController::_newMissionItemsAvailableFromVehicle(bool removeAllReque
i = 1;
}
for (; i<newMissionItems.count(); i++) {
for (; i < newMissionItems.count(); i++) {
const MissionItem* missionItem = newMissionItems[i];
newControllerMissionItems->append(new SimpleMissionItem(_controllerVehicle, _editMode, *missionItem, this));
}
......
......@@ -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());
}
......
......@@ -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)) {
......
......@@ -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<QGeoCoordinate>();
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
......
......@@ -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<QLineF>& lineList, const QPolygonF& polygon, QList<QLineF>& resultLines);
void _adjustLineDirection(const QList<QLineF>& lineList, QList<QLineF>& 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;
......
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