wp display in flight view mod

src/Wima/WimaController.cc

@@ -12,8 +12,8 @@ WimaController::WimaController(QObject *parent)
     , _serviceArea          (this)
     , _corridor             (this)
     , _localPlanDataValid   (false)
-    , _startWaypointIndex   (0)
-    , _endWaypointIndex     (0)
+    , _startWaypointIndex   (1)
+    , _endWaypointIndex     (1)
 {
 
 }
@@ -97,6 +97,12 @@ void WimaController::setDataContainer(WimaDataContainer *container)
     }
 }
 
+void WimaController::nextPhase()
+{
+    updateCurrentMissionItems();
+    updateCurrentPath();
+}
+
 void WimaController::startMission()
 {
 
@@ -293,9 +299,18 @@ void WimaController::containerDataValidChanged(bool valid)
         // create mission items
         _missionController->removeAll();
         QmlObjectListModel* missionControllerVisualItems = _missionController->visualItems();
-        for ( int i = 1; i < tempMissionItems.size(); i++) {
+        bool copyON = false;
+        for ( int i = 0; i < tempMissionItems.size(); i++) {
             const MissionItem *missionItem = tempMissionItems[i];
-            _missionController->insertSimpleMissionItem(*missionItem, missionControllerVisualItems->count());
+            if (copyON || missionItem->command() == MAV_CMD_NAV_VTOL_TAKEOFF
+                       || missionItem->command() == MAV_CMD_NAV_TAKEOFF) {
+                _missionController->insertSimpleMissionItem(*missionItem, missionControllerVisualItems->count());
+                copyON = true;
+            }
+
+            if (    missionItem->command() == MAV_CMD_NAV_VTOL_LAND
+                 || missionItem->command() == MAV_CMD_NAV_LAND)
+                break;
         }
 
 
@@ -318,6 +333,8 @@ void WimaController::containerDataValidChanged(bool valid)
         }
 
         updateWaypointPath();
+
+        _startWaypointIndex = 1;
         updateCurrentMissionItems();
         updateCurrentPath();
 
@@ -342,28 +359,30 @@ void WimaController::containerDataValidChanged(bool valid)
 void WimaController::updateCurrentMissionItems()
 {
     int numberWaypoints = 30; // the number of waypoints currentMissionItems must not exceed
+    int overlapping = 2; // number of overlapping waypoints of consecutive mission phases
 
     SimpleMissionItem *homeItem = _missionItems.value<SimpleMissionItem *>(0);
     if (homeItem == nullptr) {
         qWarning("WimaController::updateCurrentMissionItems(): nullptr");
+        _currentMissionItems.clear();
         return;
     }
     QGeoCoordinate homeCoordinate(homeItem->coordinate());
 
     QList<QGeoCoordinate> geoCoordinateList; // list with potential waypoints (from _missionItems), for _currentMissionItems
-    if (!extractCoordinateList(_missionItems, geoCoordinateList, _startWaypointIndex,
-                               std::min(_startWaypointIndex + numberWaypoints - 1, _missionItems.count()-2))) // -2 -> last item is land item
-    {
+    _endWaypointIndex = std::min(_startWaypointIndex + numberWaypoints - 1, _missionItems.count()-2); // -2 -> last item is land item
+    if (!extractCoordinateList(_missionItems, geoCoordinateList, _startWaypointIndex, _endWaypointIndex)) {
         qWarning("WimaController::updateCurrentMissionItems(): error on waypoint extraction.");
+        _currentMissionItems.clear();
         return;
     }
-
-    _currentMissionItems.clear();
+    _startWaypointIndex = _endWaypointIndex + 1 - overlapping;
 
     // calculate path from home to first waypoint
     QList<QGeoCoordinate> path;
     if ( !calcShortestPath(homeCoordinate, geoCoordinateList[0], path) ) {
         qWarning("WimaController::updateCurrentMissionItems(): Not able to calc path from home to first waypoint.");
+        _currentMissionItems.clear();
         return;
     }
     // prepend to geoCoordinateList
@@ -374,6 +393,7 @@ void WimaController::updateCurrentMissionItems()
     path.clear();
     if ( !calcShortestPath(geoCoordinateList.last(), homeCoordinate, path) ) {
         qWarning("WimaController::updateCurrentMissionItems(): Not able to calc path from home to first waypoint.");
+        _currentMissionItems.clear();
         return;
     }
     path.removeFirst(); // first coordinate already in geoCoordinateList
@@ -389,6 +409,7 @@ void WimaController::updateCurrentMissionItems()
     SimpleMissionItem *landItem = missionControllerVisuals->value<SimpleMissionItem*>(missionControllerVisuals->count()-1);
     if (landItem == nullptr) {
         qWarning("WimaController::updateCurrentMissionItems(): nullptr");
+        _currentMissionItems.clear();
         return;
     }
     // check vehicle type, before setting land command
@@ -396,8 +417,10 @@ void WimaController::updateCurrentMissionItems()
     MAV_CMD landCmd = controllerVehicle->vtol() ? MAV_CMD_NAV_VTOL_LAND : MAV_CMD_NAV_LAND;
     if (controllerVehicle->firmwarePlugin()->supportedMissionCommands().contains(landCmd)) {
         landItem->setCommand(landCmd);
-    } else
+    } else {
+        _currentMissionItems.clear();
         return;
+    }
 
     // copy mission items to _currentMissionItems
 //    MissionSettingsItem *settingsItem     = qobject_cast<MissionSettingsItem *>((*missionControllerVisuals)[0]);
@@ -407,10 +430,12 @@ void WimaController::updateCurrentMissionItems()
 //    }
 //    _missionItems.append(settingsItem);
 
+    _currentMissionItems.clear();
     for ( int i = 1; i < missionControllerVisuals->count(); i++) {
         SimpleMissionItem *visualItem     = missionControllerVisuals->value<SimpleMissionItem*>(i);
         if (visualItem == nullptr) {
-            qWarning("WimaController::updateCurrentMissionItems(): Nullptr at SimpleMissionItem!");
+            qWarning("WimaController::updateCurrentMissionItems(): Nullptr at SimpleMissionItem!");            
+            _currentMissionItems.clear();
             return;
         }
         SimpleMissionItem *visualItemCopy = new SimpleMissionItem(*visualItem, true, this);
@@ -423,14 +448,16 @@ void WimaController::updateCurrentMissionItems()
 void WimaController::updateWaypointPath()
 {
     _waypointPath.clear();
-    extractCoordinateList(_missionItems, _waypointPath, 0, _missionItems.count()-1);
+    if (!extractCoordinateList(_missionItems, _waypointPath, 0, _missionItems.count()-1))
+        return;
 
     emit waypointPathChanged();
 }
 void WimaController::updateCurrentPath()
 {
     _currentWaypointPath.clear();
-    extractCoordinateList(_currentMissionItems, _currentWaypointPath, 0, _currentMissionItems.count()-1);
+    if (!extractCoordinateList(_currentMissionItems, _currentWaypointPath, 0, _currentMissionItems.count()-1))
+        return;
 
     emit currentWaypointPathChanged();
 }

src/Wima/WimaController.h

@@ -68,11 +68,11 @@ public:
     void setDataContainer           (WimaDataContainer* container);
 
     // Member Methodes
+    Q_INVOKABLE void nextPhase();
     Q_INVOKABLE void startMission();
     Q_INVOKABLE void abortMission();
     Q_INVOKABLE void pauseMission();
     Q_INVOKABLE void resumeMission();
-    Q_INVOKABLE bool updateMission();
 
     Q_INVOKABLE void saveToCurrent  ();
     Q_INVOKABLE void saveToFile     (const QString& filename);