|
|
|
@ -341,9 +341,15 @@ void MissionController::_recalcWaypointLines(void)
@@ -341,9 +341,15 @@ void MissionController::_recalcWaypointLines(void)
|
|
|
|
|
// both relative altitude.
|
|
|
|
|
|
|
|
|
|
// No values for first item
|
|
|
|
|
lastCoordinateItem->setAltDifference(0.0); |
|
|
|
|
lastCoordinateItem->setAzimuth(0.0); |
|
|
|
|
lastCoordinateItem->setDistance(-1.0); |
|
|
|
|
|
|
|
|
|
double minAltSeen = 0.0; |
|
|
|
|
double maxAltSeen = 0.0; |
|
|
|
|
double homePositionAltitude = homeItem->coordinate().altitude(); |
|
|
|
|
minAltSeen = maxAltSeen = homeItem->coordinate().altitude(); |
|
|
|
|
|
|
|
|
|
_waypointLines.clear(); |
|
|
|
|
|
|
|
|
|
for (int i=1; i<_missionItems->count(); i++) { |
|
|
|
@ -353,35 +359,62 @@ void MissionController::_recalcWaypointLines(void)
@@ -353,35 +359,62 @@ void MissionController::_recalcWaypointLines(void)
|
|
|
|
|
item->setAzimuth(0.0); |
|
|
|
|
item->setDistance(-1.0); |
|
|
|
|
|
|
|
|
|
if (item->specifiesCoordinate() && !item->standaloneCoordinate()) { |
|
|
|
|
if (firstCoordinateItem) { |
|
|
|
|
if (item->command() == MavlinkQmlSingleton::MAV_CMD_NAV_TAKEOFF) { |
|
|
|
|
// The first coordinate we hit is a takeoff command so link back to home position if valid
|
|
|
|
|
if (homePositionValid) { |
|
|
|
|
double azimuth, distance, altDifference; |
|
|
|
|
|
|
|
|
|
_waypointLines.append(new CoordinateVector(homeItem->coordinate(), item->coordinate())); |
|
|
|
|
_calcPrevWaypointValues(homePositionValid, homeAlt, item, homeItem, &azimuth, &distance, &altDifference); |
|
|
|
|
item->setAltDifference(altDifference); |
|
|
|
|
item->setAzimuth(azimuth); |
|
|
|
|
item->setDistance(distance); |
|
|
|
|
if (item->specifiesCoordinate()) { |
|
|
|
|
double absoluteAltitude = item->coordinate().altitude(); |
|
|
|
|
if (item->relativeAltitude() && homePositionValid) { |
|
|
|
|
absoluteAltitude += homePositionAltitude; |
|
|
|
|
} |
|
|
|
|
minAltSeen = std::min(minAltSeen, absoluteAltitude); |
|
|
|
|
maxAltSeen = std::max(maxAltSeen, absoluteAltitude); |
|
|
|
|
|
|
|
|
|
if (!item->standaloneCoordinate()) { |
|
|
|
|
if (firstCoordinateItem) { |
|
|
|
|
if (item->command() == MavlinkQmlSingleton::MAV_CMD_NAV_TAKEOFF) { |
|
|
|
|
// The first coordinate we hit is a takeoff command so link back to home position if valid
|
|
|
|
|
if (homePositionValid) { |
|
|
|
|
double azimuth, distance, altDifference; |
|
|
|
|
|
|
|
|
|
_waypointLines.append(new CoordinateVector(homeItem->coordinate(), item->coordinate())); |
|
|
|
|
_calcPrevWaypointValues(homePositionValid, homeAlt, item, homeItem, &azimuth, &distance, &altDifference); |
|
|
|
|
item->setAltDifference(altDifference); |
|
|
|
|
item->setAzimuth(azimuth); |
|
|
|
|
item->setDistance(distance); |
|
|
|
|
} |
|
|
|
|
} else { |
|
|
|
|
// First coordiante is not a takeoff command, it does not link backwards to anything
|
|
|
|
|
} |
|
|
|
|
} else { |
|
|
|
|
// First coordiante is not a takeoff command, it does not link backwards to anything
|
|
|
|
|
firstCoordinateItem = false; |
|
|
|
|
} else if (!lastCoordinateItem->homePosition() || lastCoordinateItem->homePositionValid()) { |
|
|
|
|
double azimuth, distance, altDifference; |
|
|
|
|
|
|
|
|
|
// Subsequent coordinate items link to last coordinate item. If the last coordinate item
|
|
|
|
|
// is an invalid home position we skip the line
|
|
|
|
|
_calcPrevWaypointValues(homePositionValid, homeAlt, item, lastCoordinateItem, &azimuth, &distance, &altDifference); |
|
|
|
|
item->setAltDifference(altDifference); |
|
|
|
|
item->setAzimuth(azimuth); |
|
|
|
|
item->setDistance(distance); |
|
|
|
|
_waypointLines.append(new CoordinateVector(lastCoordinateItem->coordinate(), item->coordinate())); |
|
|
|
|
} |
|
|
|
|
firstCoordinateItem = false; |
|
|
|
|
} else if (!lastCoordinateItem->homePosition() || lastCoordinateItem->homePositionValid()) { |
|
|
|
|
double azimuth, distance, altDifference; |
|
|
|
|
|
|
|
|
|
// Subsequent coordinate items link to last coordinate item. If the last coordinate item
|
|
|
|
|
// is an invalid home position we skip the line
|
|
|
|
|
_calcPrevWaypointValues(homePositionValid, homeAlt, item, lastCoordinateItem, &azimuth, &distance, &altDifference); |
|
|
|
|
item->setAltDifference(altDifference); |
|
|
|
|
item->setAzimuth(azimuth); |
|
|
|
|
item->setDistance(distance); |
|
|
|
|
_waypointLines.append(new CoordinateVector(lastCoordinateItem->coordinate(), item->coordinate())); |
|
|
|
|
lastCoordinateItem = item; |
|
|
|
|
} |
|
|
|
|
} |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
// Walk the list again calculating altitude percentages
|
|
|
|
|
double altRange = maxAltSeen - minAltSeen; |
|
|
|
|
for (int i=0; i<_missionItems->count(); i++) { |
|
|
|
|
MissionItem* item = qobject_cast<MissionItem*>(_missionItems->get(i)); |
|
|
|
|
|
|
|
|
|
if (item->specifiesCoordinate()) { |
|
|
|
|
double absoluteAltitude = item->coordinate().altitude(); |
|
|
|
|
if (item->relativeAltitude() && homePositionValid) { |
|
|
|
|
absoluteAltitude += homePositionAltitude; |
|
|
|
|
} |
|
|
|
|
if (altRange == 0.0) { |
|
|
|
|
item->setAltPercent(0.0); |
|
|
|
|
} else { |
|
|
|
|
item->setAltPercent((absoluteAltitude - minAltSeen) / altRange); |
|
|
|
|
} |
|
|
|
|
lastCoordinateItem = item; |
|
|
|
|
} |
|
|
|
|
} |
|
|
|
|
|
|
|
|
@ -448,6 +481,11 @@ void MissionController::_initAllMissionItems(void)
@@ -448,6 +481,11 @@ void MissionController::_initAllMissionItems(void)
|
|
|
|
|
} |
|
|
|
|
homeItem->setCommand(MAV_CMD_NAV_WAYPOINT); |
|
|
|
|
homeItem->setFrame(MAV_FRAME_GLOBAL); |
|
|
|
|
if (!homeItem->homePositionValid()) { |
|
|
|
|
QGeoCoordinate homeCoord = homeItem->coordinate(); |
|
|
|
|
homeCoord.setAltitude(0.0); |
|
|
|
|
homeItem->setCoordinate(homeCoord); |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
for (int i=0; i<_missionItems->count(); i++) { |
|
|
|
|
_initMissionItem(qobject_cast<MissionItem*>(_missionItems->get(i))); |
|
|
|
|