RTL now connects back to home

src/MissionManager/MissionController.cc

@@ -895,6 +895,28 @@ double MissionController::_calcDistanceToHome(VisualMissionItem* currentItem, Vi
     return distanceOk ? homeCoord.distanceTo(currentCoord) : 0.0;
 }
 
+void MissionController::_addWaypointLineSegment(CoordVectHashTable& prevItemPairHashTable, VisualItemPair& pair)
+{
+    if (prevItemPairHashTable.contains(pair)) {
+        // Pair already exists and connected, just re-use
+        _linesTable[pair] = prevItemPairHashTable.take(pair);
+    } else {
+        // Create a new segment and wire update notifiers
+        auto linevect       = new CoordinateVector(pair.first->isSimpleItem() ? pair.first->coordinate() : pair.first->exitCoordinate(), pair.second->coordinate(), this);
+        auto originNotifier = pair.first->isSimpleItem() ? &VisualMissionItem::coordinateChanged : &VisualMissionItem::exitCoordinateChanged;
+        auto endNotifier    = &VisualMissionItem::coordinateChanged;
+
+        // Use signals/slots to update the coordinate endpoints
+        connect(pair.first,     originNotifier, linevect, &CoordinateVector::setCoordinate1);
+        connect(pair.second,    endNotifier,    linevect, &CoordinateVector::setCoordinate2);
+
+        // FIXME: We should ideally have signals for 2D position change, alt change, and 3D position change
+        // Not optimal, but still pretty fast, do a full update of range/bearing/altitudes
+        connect(pair.second, &VisualMissionItem::coordinateChanged, this, &MissionController::_recalcMissionFlightStatus);
+        _linesTable[pair] = linevect;
+    }
+}
+
 void MissionController::_recalcWaypointLines(void)
 {
     bool                firstCoordinateItem =   true;
@@ -908,7 +930,15 @@ void MissionController::_recalcWaypointLines(void)
     _linesTable.clear();
     _waypointLines.clear();
 
-    bool linkBackToHome = false;
+    bool linkEndToHome;
+    SimpleMissionItem* lastItem = _visualItems->value<SimpleMissionItem*>(_visualItems->count() - 1);
+    if (lastItem && (int)lastItem->command() == MAV_CMD_NAV_RETURN_TO_LAUNCH) {
+        linkEndToHome = true;
+    } else {
+        linkEndToHome = _settingsItem->missionEndRTL();
+    }
+
+    bool linkStartToHome = false;
     for (int i=1; i<_visualItems->count(); i++) {
         VisualMissionItem* item = qobject_cast<VisualMissionItem*>(_visualItems->get(i));
 
@@ -918,36 +948,24 @@ void MissionController::_recalcWaypointLines(void)
                 item->isSimpleItem() &&
                 (qobject_cast<SimpleMissionItem*>(item)->command() == MavlinkQmlSingleton::MAV_CMD_NAV_TAKEOFF ||
                  qobject_cast<SimpleMissionItem*>(item)->command() == MavlinkQmlSingleton::MAV_CMD_NAV_VTOL_TAKEOFF)) {
-            linkBackToHome = true;
+            linkStartToHome = true;
         }
 
         if (item->specifiesCoordinate()) {
             if (!item->isStandaloneCoordinate()) {
                 firstCoordinateItem = false;
                 VisualItemPair pair(lastCoordinateItem, item);
-                if (lastCoordinateItem != _settingsItem || (showHomePosition && linkBackToHome)) {
-                    if (old_table.contains(pair)) {
-                        // Do nothing, this segment already exists and is wired up
-                        _linesTable[pair] = old_table.take(pair);
-                    } else {
-                        // Create a new segment and wire update notifiers
-                        auto linevect       = new CoordinateVector(lastCoordinateItem->isSimpleItem() ? lastCoordinateItem->coordinate() : lastCoordinateItem->exitCoordinate(), item->coordinate(), this);
-                        auto originNotifier = lastCoordinateItem->isSimpleItem() ? &VisualMissionItem::coordinateChanged : &VisualMissionItem::exitCoordinateChanged,
-                                endNotifier    = &VisualMissionItem::coordinateChanged;
-                        // Use signals/slots to update the coordinate endpoints
-                        connect(lastCoordinateItem, originNotifier, linevect, &CoordinateVector::setCoordinate1);
-                        connect(item,               endNotifier,    linevect, &CoordinateVector::setCoordinate2);
-
-                        // FIXME: We should ideally have signals for 2D position change, alt change, and 3D position change
-                        // Not optimal, but still pretty fast, do a full update of range/bearing/altitudes
-                        connect(item, &VisualMissionItem::coordinateChanged, this, &MissionController::_recalcMissionFlightStatus);
-                        _linesTable[pair] = linevect;
-                    }
+                if (lastCoordinateItem != _settingsItem || (showHomePosition && linkStartToHome)) {
+                    _addWaypointLineSegment(old_table, pair);
                 }
                 lastCoordinateItem = item;
             }
         }
     }
+    if (linkEndToHome && lastCoordinateItem != _settingsItem && showHomePosition) {
+        VisualItemPair pair(lastCoordinateItem, _settingsItem);
+        _addWaypointLineSegment(old_table, pair);
+    }
 
     {
         // Create a temporary QObjectList and replace the model data
@@ -1029,7 +1047,7 @@ void MissionController::_recalcMissionFlightStatus()
     _resetMissionFlightStatus();
 
     bool vtolInHover = true;
-    bool linkBackToHome = false;
+    bool linkStartToHome = false;
 
     for (int i=0; i<_visualItems->count(); i++) {
         VisualMissionItem* item = qobject_cast<VisualMissionItem*>(_visualItems->get(i));
@@ -1074,7 +1092,7 @@ void MissionController::_recalcMissionFlightStatus()
         // Link back to home if first item is takeoff and we have home position
         if (firstCoordinateItem && simpleItem && simpleItem->command() == MavlinkQmlSingleton::MAV_CMD_NAV_TAKEOFF) {
             if (showHomePosition) {
-                linkBackToHome = true;
+                linkStartToHome = true;
             }
         }
 
@@ -1129,7 +1147,7 @@ void MissionController::_recalcMissionFlightStatus()
 
             if (!item->isStandaloneCoordinate()) {
                 firstCoordinateItem = false;
-                if (lastCoordinateItem != _settingsItem || linkBackToHome) {
+                if (lastCoordinateItem != _settingsItem || linkStartToHome) {
                     // This is a subsequent waypoint or we are forcing the first waypoint back to home
                     double azimuth, distance, altDifference;
 
@@ -1261,7 +1279,7 @@ void MissionController::_setPlannedHomePositionFromFirstCoordinate(void)
         return;
     }
 
-    // Set the planned home position to be a deltae from first coordinate
+    // Set the planned home position to be a delta from first coordinate
     for (int i=1; i<_visualItems->count(); i++) {
         VisualMissionItem* item = _visualItems->value<VisualMissionItem*>(i);
 
@@ -1301,6 +1319,7 @@ void MissionController::_initAllVisualItems(void)
     }
 
     connect(_settingsItem, &MissionSettingsItem::coordinateChanged,     this, &MissionController::_recalcAll);
+    connect(_settingsItem, &MissionSettingsItem::missionEndRTLChanged,  this, &MissionController::_recalcAll);
     connect(_settingsItem, &MissionSettingsItem::coordinateChanged,     this, &MissionController::plannedHomePositionChanged);
 
     for (int i=0; i<_visualItems->count(); i++) {

src/MissionManager/MissionController.h

@@ -209,6 +209,7 @@ private:
     void _updateBatteryInfo(int waypointIndex);
     bool _loadItemsFromJson(const QJsonObject& json, QmlObjectListModel* visualItems, QString& errorString);
     void _initLoadedVisualItems(QmlObjectListModel* loadedVisualItems);
+    void _addWaypointLineSegment(CoordVectHashTable& prevItemPairHashTable, VisualItemPair& pair);
 
 private:
     MissionManager*         _missionManager;

src/MissionManager/MissionSettingsItem.cc

@@ -273,3 +273,11 @@ double MissionSettingsItem::specifiedFlightSpeed(void)
         return std::numeric_limits<double>::quiet_NaN();
     }
 }
+
+void MissionSettingsItem::setMissionEndRTL(bool missionEndRTL)
+{
+    if (missionEndRTL != _missionEndRTL) {
+        _missionEndRTL = missionEndRTL;
+        emit missionEndRTLChanged(missionEndRTL);
+    }
+}

src/MissionManager/MissionSettingsItem.h

@@ -27,14 +27,16 @@ public:
     MissionSettingsItem(Vehicle* vehicle, QObject* parent = NULL);
 
     Q_PROPERTY(Fact*    plannedHomePositionAltitude READ plannedHomePositionAltitude                            CONSTANT)
-    Q_PROPERTY(bool     missionEndRTL               MEMBER _missionEndRTL                                                   NOTIFY missionEndRTLChanged)
+    Q_PROPERTY(bool     missionEndRTL               READ missionEndRTL                  WRITE setMissionEndRTL  NOTIFY missionEndRTLChanged)
     Q_PROPERTY(QObject* cameraSection               READ cameraSection                                          CONSTANT)
     Q_PROPERTY(QObject* speedSection                READ speedSection                                           CONSTANT)
 
     Fact*           plannedHomePositionAltitude (void) { return &_plannedHomePositionAltitudeFact; }
+    bool            missionEndRTL               (void) const { return _missionEndRTL; }
+    CameraSection*  cameraSection               (void) { return &_cameraSection; }
+    SpeedSection*   speedSection                (void) { return &_speedSection; }
 
-    CameraSection* cameraSection(void) { return &_cameraSection; }
-    SpeedSection* speedSection(void) { return &_speedSection; }
+    void setMissionEndRTL(bool missionEndRTL);
 
     /// Scans the loaded items for settings items
     bool scanForMissionSettings(QmlObjectListModel* visualItems, int scanIndex);