|
|
|
@ -18,6 +18,7 @@
@@ -18,6 +18,7 @@
|
|
|
|
|
#include "ComplexMissionItem.h" |
|
|
|
|
#include "JsonHelper.h" |
|
|
|
|
#include "ParameterLoader.h" |
|
|
|
|
#include "QGroundControlQmlGlobal.h" |
|
|
|
|
|
|
|
|
|
#ifndef __mobile__ |
|
|
|
|
#include "QGCFileDialog.h" |
|
|
|
@ -44,6 +45,10 @@ MissionController::MissionController(QObject *parent)
@@ -44,6 +45,10 @@ MissionController::MissionController(QObject *parent)
|
|
|
|
|
, _firstItemsFromVehicle(false) |
|
|
|
|
, _missionItemsRequested(false) |
|
|
|
|
, _queuedSend(false) |
|
|
|
|
, _missionDistance(0.0) |
|
|
|
|
, _missionMaxTelemetry(0.0) |
|
|
|
|
, _cruiseDistance(0.0) |
|
|
|
|
, _hoverDistance(0.0) |
|
|
|
|
{ |
|
|
|
|
|
|
|
|
|
} |
|
|
|
@ -602,6 +607,23 @@ void MissionController::_calcPrevWaypointValues(double homeAlt, VisualMissionIte
@@ -602,6 +607,23 @@ void MissionController::_calcPrevWaypointValues(double homeAlt, VisualMissionIte
|
|
|
|
|
} |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
void MissionController::_calcHomeDist(VisualMissionItem* currentItem, VisualMissionItem* homeItem, double* distance) |
|
|
|
|
{ |
|
|
|
|
QGeoCoordinate currentCoord = currentItem->coordinate(); |
|
|
|
|
QGeoCoordinate homeCoord = homeItem->exitCoordinate(); |
|
|
|
|
bool distanceOk = false; |
|
|
|
|
|
|
|
|
|
distanceOk = true; |
|
|
|
|
|
|
|
|
|
qCDebug(MissionControllerLog) << "distanceOk" << distanceOk; |
|
|
|
|
|
|
|
|
|
if (distanceOk) { |
|
|
|
|
*distance = homeCoord.distanceTo(currentCoord); |
|
|
|
|
} else { |
|
|
|
|
*distance = 0.0; |
|
|
|
|
} |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
void MissionController::_recalcWaypointLines(void) |
|
|
|
|
{ |
|
|
|
|
bool firstCoordinateItem = true; |
|
|
|
@ -692,7 +714,6 @@ void MissionController::_recalcAltitudeRangeBearing()
@@ -692,7 +714,6 @@ void MissionController::_recalcAltitudeRangeBearing()
|
|
|
|
|
|
|
|
|
|
bool firstCoordinateItem = true; |
|
|
|
|
VisualMissionItem* lastCoordinateItem = qobject_cast<VisualMissionItem*>(_visualItems->get(0)); |
|
|
|
|
|
|
|
|
|
SimpleMissionItem* homeItem = qobject_cast<SimpleMissionItem*>(lastCoordinateItem); |
|
|
|
|
|
|
|
|
|
if (!homeItem) { |
|
|
|
@ -717,19 +738,50 @@ void MissionController::_recalcAltitudeRangeBearing()
@@ -717,19 +738,50 @@ void MissionController::_recalcAltitudeRangeBearing()
|
|
|
|
|
const double homePositionAltitude = homeItem->coordinate().altitude(); |
|
|
|
|
minAltSeen = maxAltSeen = homeItem->coordinate().altitude(); |
|
|
|
|
|
|
|
|
|
double missionDistance = 0.0; |
|
|
|
|
double missionMaxTelemetry = 0.0; |
|
|
|
|
|
|
|
|
|
bool vtolCalc = (QGroundControlQmlGlobal::offlineEditingVehicleType()->enumStringValue() == "VTOL" || (_activeVehicle && _activeVehicle->vtol())) ? true : false ; |
|
|
|
|
double cruiseDistance = 0.0; |
|
|
|
|
double hoverDistance = 0.0; |
|
|
|
|
bool hoverDistanceCalc = false; |
|
|
|
|
bool hoverTransition = false; |
|
|
|
|
bool cruiseTransition = false; |
|
|
|
|
bool hoverDistanceReset = false; |
|
|
|
|
|
|
|
|
|
bool linkBackToHome = false; |
|
|
|
|
|
|
|
|
|
|
|
|
|
|
for (int i=1; i<_visualItems->count(); i++) { |
|
|
|
|
VisualMissionItem* item = qobject_cast<VisualMissionItem*>(_visualItems->get(i)); |
|
|
|
|
|
|
|
|
|
// Assume the worst
|
|
|
|
|
item->setAzimuth(0.0); |
|
|
|
|
item->setDistance(0.0); |
|
|
|
|
|
|
|
|
|
// If we still haven't found the first coordinate item and we hit a a takeoff command link back to home
|
|
|
|
|
// If we still haven't found the first coordinate item and we hit a takeoff command link back to home
|
|
|
|
|
if (firstCoordinateItem && |
|
|
|
|
item->isSimpleItem() && |
|
|
|
|
qobject_cast<SimpleMissionItem*>(item)->command() == MavlinkQmlSingleton::MAV_CMD_NAV_TAKEOFF) { |
|
|
|
|
linkBackToHome = true; |
|
|
|
|
hoverDistanceCalc = true; |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
if (item->isSimpleItem() && vtolCalc) { |
|
|
|
|
SimpleMissionItem* simpleItem = qobject_cast<SimpleMissionItem*>(item); |
|
|
|
|
if (simpleItem->command() == MavlinkQmlSingleton::MAV_CMD_DO_VTOL_TRANSITION){ //transition waypoint value
|
|
|
|
|
if (simpleItem->missionItem().param1() == 3){ //hover mode value
|
|
|
|
|
hoverDistanceCalc = true; |
|
|
|
|
hoverTransition = true; |
|
|
|
|
} |
|
|
|
|
else if (simpleItem->missionItem().param1() == 4){ |
|
|
|
|
hoverDistanceCalc = false; |
|
|
|
|
cruiseTransition = true; |
|
|
|
|
} |
|
|
|
|
} |
|
|
|
|
if(!hoverTransition && cruiseTransition && !hoverDistanceReset && !linkBackToHome){ |
|
|
|
|
hoverDistance = missionDistance; |
|
|
|
|
hoverDistanceReset = true; |
|
|
|
|
} |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
if (item->specifiesCoordinate()) { |
|
|
|
@ -754,7 +806,7 @@ void MissionController::_recalcAltitudeRangeBearing()
@@ -754,7 +806,7 @@ void MissionController::_recalcAltitudeRangeBearing()
|
|
|
|
|
if (!item->isStandaloneCoordinate()) { |
|
|
|
|
firstCoordinateItem = false; |
|
|
|
|
if (lastCoordinateItem != homeItem || (showHomePosition && linkBackToHome)) { |
|
|
|
|
double azimuth, distance, altDifference; |
|
|
|
|
double azimuth, distance, altDifference, telemetryDistance; |
|
|
|
|
|
|
|
|
|
// Subsequent coordinate items link to last coordinate item. If the last coordinate item
|
|
|
|
|
// is an invalid home position we skip the line
|
|
|
|
@ -762,12 +814,46 @@ void MissionController::_recalcAltitudeRangeBearing()
@@ -762,12 +814,46 @@ void MissionController::_recalcAltitudeRangeBearing()
|
|
|
|
|
item->setAltDifference(altDifference); |
|
|
|
|
item->setAzimuth(azimuth); |
|
|
|
|
item->setDistance(distance); |
|
|
|
|
|
|
|
|
|
missionDistance += distance; |
|
|
|
|
|
|
|
|
|
if (item->isSimpleItem()) { |
|
|
|
|
_calcHomeDist(item, homeItem, &telemetryDistance); |
|
|
|
|
|
|
|
|
|
if (vtolCalc) { |
|
|
|
|
SimpleMissionItem* simpleItem = qobject_cast<SimpleMissionItem*>(item); |
|
|
|
|
if (simpleItem->command() == MavlinkQmlSingleton::MAV_CMD_NAV_TAKEOFF || hoverDistanceCalc){ |
|
|
|
|
hoverDistance += distance; |
|
|
|
|
} |
|
|
|
|
cruiseDistance = missionDistance - hoverDistance; |
|
|
|
|
if(simpleItem->command() == MavlinkQmlSingleton::MAV_CMD_NAV_LAND && !linkBackToHome && !cruiseTransition && !hoverTransition){ |
|
|
|
|
hoverDistance = cruiseDistance; |
|
|
|
|
cruiseDistance = missionDistance - hoverDistance; |
|
|
|
|
} |
|
|
|
|
} |
|
|
|
|
} else { |
|
|
|
|
missionDistance += qobject_cast<ComplexMissionItem*>(item)->surveyDistance(); |
|
|
|
|
telemetryDistance = qobject_cast<ComplexMissionItem*>(item)->greatestDistanceTo(homeItem->exitCoordinate()); |
|
|
|
|
|
|
|
|
|
if (vtolCalc){ |
|
|
|
|
cruiseDistance += qobject_cast<ComplexMissionItem*>(item)->surveyDistance(); //assume all survey missions undertaken in cruise
|
|
|
|
|
} |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
if (telemetryDistance > missionMaxTelemetry) { |
|
|
|
|
missionMaxTelemetry = telemetryDistance; |
|
|
|
|
} |
|
|
|
|
} |
|
|
|
|
lastCoordinateItem = item; |
|
|
|
|
} |
|
|
|
|
} |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
setMissionDistance(missionDistance); |
|
|
|
|
setMissionMaxTelemetry(missionMaxTelemetry); |
|
|
|
|
setCruiseDistance(cruiseDistance); |
|
|
|
|
setHoverDistance(hoverDistance); |
|
|
|
|
|
|
|
|
|
// Walk the list again calculating altitude percentages
|
|
|
|
|
double altRange = maxAltSeen - minAltSeen; |
|
|
|
|
for (int i=0; i<_visualItems->count(); i++) { |
|
|
|
@ -921,6 +1007,7 @@ void MissionController::_initVisualItem(VisualMissionItem* visualItem)
@@ -921,6 +1007,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); |
|
|
|
|
} |
|
|
|
|
} |
|
|
|
|
|
|
|
|
@ -1017,6 +1104,38 @@ void MissionController::setAutoSync(bool autoSync)
@@ -1017,6 +1104,38 @@ void MissionController::setAutoSync(bool autoSync)
|
|
|
|
|
#endif |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
void MissionController::setMissionDistance(double missionDistance) |
|
|
|
|
{ |
|
|
|
|
if (!qFuzzyCompare(_missionDistance, missionDistance)) { |
|
|
|
|
_missionDistance = missionDistance; |
|
|
|
|
emit missionDistanceChanged(_missionDistance); |
|
|
|
|
} |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
void MissionController::setMissionMaxTelemetry(double missionMaxTelemetry) |
|
|
|
|
{ |
|
|
|
|
if (!qFuzzyCompare(_missionMaxTelemetry, missionMaxTelemetry)) { |
|
|
|
|
_missionMaxTelemetry = missionMaxTelemetry; |
|
|
|
|
emit missionMaxTelemetryChanged(_missionMaxTelemetry); |
|
|
|
|
} |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
void MissionController::setCruiseDistance(double cruiseDistance) |
|
|
|
|
{ |
|
|
|
|
if (!qFuzzyCompare(_cruiseDistance, cruiseDistance)) { |
|
|
|
|
_cruiseDistance = cruiseDistance; |
|
|
|
|
emit cruiseDistanceChanged(_cruiseDistance); |
|
|
|
|
} |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
void MissionController::setHoverDistance(double hoverDistance) |
|
|
|
|
{ |
|
|
|
|
if (!qFuzzyCompare(_hoverDistance, hoverDistance)) { |
|
|
|
|
_hoverDistance = hoverDistance; |
|
|
|
|
emit hoverDistanceChanged(_hoverDistance); |
|
|
|
|
} |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
void MissionController::_dirtyChanged(bool dirty) |
|
|
|
|
{ |
|
|
|
|
if (dirty && _autoSync) { |
|
|
|
@ -1051,7 +1170,7 @@ void MissionController::_inProgressChanged(bool inProgress)
@@ -1051,7 +1170,7 @@ void MissionController::_inProgressChanged(bool inProgress)
|
|
|
|
|
|
|
|
|
|
bool MissionController::_findLastAltitude(double* lastAltitude, MAV_FRAME* frame) |
|
|
|
|
{ |
|
|
|
|
bool found = false;
|
|
|
|
|
bool found = false; |
|
|
|
|
double foundAltitude; |
|
|
|
|
MAV_FRAME foundFrame; |
|
|
|
|
|
|
|
|
|