From b750c08c9b6176eaecb624915af3ba3ab33f49e7 Mon Sep 17 00:00:00 2001
From: Valentin Platzgummer <valentin.platzgummer@nt.tuwien.ac.at>
Date: Fri, 21 Feb 2020 13:40:18 +0100
Subject: [PATCH] rem. dist time issues fixed

---
 src/MissionManager/MissionController.cc | 34 +++++++++++++++++--------
 1 file changed, 23 insertions(+), 11 deletions(-)

diff --git a/src/MissionManager/MissionController.cc b/src/MissionManager/MissionController.cc
index 4a6dc13c3..31a3dd9b6 100644
--- a/src/MissionManager/MissionController.cc
+++ b/src/MissionManager/MissionController.cc
@@ -1676,7 +1676,6 @@ void MissionController::_recalcAll(void)
 
 void MissionController::_enableDisableRemainingDistTimeCalculation(bool flying)
 {
-    qWarning() << "MissionController::_enableDisableRemainingDistTimeCalculation(): current index: " << _missionManager->currentIndex();
     if (flying) {
         _remainingDistanceTimeTimer.start();
     } else {
@@ -2188,8 +2187,6 @@ bool MissionController::distanceTimeToMissionEnd(double &remainingDistance, doub
         || missionIndex > _visualItems->count()) {
         return false;
     }
-    qWarning("\n\n");
-    qWarning() << "MissionController::distanceTimeToMissionEnd: missionIndex" << missionIndex;
     // check if vehicle is flying and fetch vehicle coordinate
     QGeoCoordinate vehiclePosition;
     if (_managerVehicle && _managerVehicle->flying() && useVehiclePosition) {
@@ -2233,10 +2230,10 @@ bool MissionController::distanceTimeToMissionEnd(double &remainingDistance, doub
         }
     }
 
-    // determine speed at waypoint with index currentMissionIdx-1
+    // determine speed at waypoint with index missionIndex
     double currentSpeed = _settingsItem->specifiedFlightSpeed();
     currentSpeed = !qIsNaN(currentSpeed) ? currentSpeed : 5;
-    for (int i = 1; i < missionIndex-1; ++i) {
+    for (int i = 1; i < missionIndex; ++i) {
         SimpleMissionItem* simpleItem = _visualItems->value<SimpleMissionItem*>(i);
 
         if (simpleItem != nullptr) {
@@ -2246,6 +2243,7 @@ bool MissionController::distanceTimeToMissionEnd(double &remainingDistance, doub
         }
     }
 
+    // calculate dist and time from current vehicle pos. to mission item with index missionIndex
     if (useVehiclePosition) {
         // find valid coordinate
         for (int i = missionIndex; i < _visualItems->count(); ++i) {
@@ -2265,8 +2263,6 @@ bool MissionController::distanceTimeToMissionEnd(double &remainingDistance, doub
     for (int i=missionIndex; i<_visualItems->count(); i++) {
         VisualMissionItem* item = qobject_cast<VisualMissionItem*>(_visualItems->get(i));
         SimpleMissionItem* simpleItem = qobject_cast<SimpleMissionItem*>(item);
-        qWarning() << "MissionController::distanceTimeToMissionEnd: i:" << i;
-        qWarning() << "MissionController::distanceTimeToMissionEnd: coordinate:" << item->coordinate();
 
         // Assume the worst
         item->setAzimuth(0.0);
@@ -2306,7 +2302,6 @@ bool MissionController::distanceTimeToMissionEnd(double &remainingDistance, doub
 
             if (lastCoordinateItem != _settingsItem || linkStartToHome) {
                 // This is a subsequent waypoint or we are forcing the first waypoint back to home
-                qWarning() << "MissionController::distanceTimeToMissionEnd: entered";
 
                 QGeoCoordinate  currentCoord =  item->coordinate();
                 QGeoCoordinate  prevCoord =     lastCoordinateItem->exitCoordinate();
@@ -2328,8 +2323,27 @@ bool MissionController::distanceTimeToMissionEnd(double &remainingDistance, doub
             lastCoordinateItem = item;
         }
 
+        if (    simpleItem != nullptr
+            && (   simpleItem->command() == MAV_CMD_NAV_LAND
+                || simpleItem->command() == MAV_CMD_NAV_LAND_LOCAL
+                || simpleItem->command() == MAV_CMD_NAV_VTOL_LAND)) {
 
-        qWarning() << "MissionController::distanceTimeToMissionEnd: dist: " << remainingDistance;
+            double alt = 0;
+            if (i == missionIndex) {
+                alt = _managerVehicle->altitudeRelative()->rawValue().toDouble();
+            } else {
+                QGeoCoordinate  currentCoord =  simpleItem->coordinate();
+
+                if (simpleItem->coordinateHasRelativeAltitude()) {
+                    currentCoord.setAltitude(homePositionAltitude + currentCoord.altitude());
+                }
+
+                alt = currentCoord.altitude() - homePositionAltitude;
+            }
+            double landTime = qAbs(alt) / _appSettings->offlineEditingDescentSpeed()->rawValue().toDouble();
+
+            remainingTime       += landTime;
+        }
     }
 
     if (linkEndToHome && lastCoordinateItem != _settingsItem) {
@@ -2350,8 +2364,6 @@ bool MissionController::distanceTimeToMissionEnd(double &remainingDistance, doub
         remainingDistance   += distance;
         remainingTime       += time + landTime;
     }
-
-    qWarning() << "MissionController::distanceTimeToMissionEnd: end dist: " << remainingDistance;
     return true;
 }
 
-- 
2.22.0