Merge pull request #6900 from DonLakeFlyer/AddTakeoffItem

ArduPilot: Fixed planned home position creation when not connected to vehicle

ChangeLog.md

@@ -4,6 +4,9 @@ Note: This file only contains high level features or important fixes.
 
 ## 3.4
 
+### 3.4.5 - Not yet released
+* ArduPilot: Fix location of planned home position when not connected to vehicle. Issue #6840.
+
 ### 3.4.4 - Stable
 * Stable desktop versions now inform user at boot if newer version is available.
 * Multi-Vehicle Start Mission and Pause now work correctly. Issue #6864.

src/MissionManager/MissionController.cc

@@ -371,7 +371,8 @@ int MissionController::insertSimpleMissionItem(QGeoCoordinate coordinate, int i)
     newItem->setMissionFlightStatus(_missionFlightStatus);
     _visualItems->insert(i, newItem);
 
-    _recalcAll();
+    // We send the click coordinate through here to be able to set the planned home position from the user click location if needed
+    _recalcAllWithClickCoordinate(coordinate);
 
     return newItem->sequenceNumber();
 }
@@ -1517,8 +1518,10 @@ void MissionController::_recalcChildItems(void)
     }
 }
 
-void MissionController::_setPlannedHomePositionFromFirstCoordinate(void)
+void MissionController::_setPlannedHomePositionFromFirstCoordinate(const QGeoCoordinate& clickCoordinate)
 {
+    QGeoCoordinate firstCoordinate;
+
     if (_settingsItem->coordinate().isValid()) {
         return;
     }
@@ -1528,24 +1531,40 @@ void MissionController::_setPlannedHomePositionFromFirstCoordinate(void)
         VisualMissionItem* item = _visualItems->value<VisualMissionItem*>(i);
 
         if (item->specifiesCoordinate()) {
-            QGeoCoordinate plannedHomeCoord = item->coordinate().atDistanceAndAzimuth(30, 0);
-            plannedHomeCoord.setAltitude(0);
-            _settingsItem->setCoordinate(plannedHomeCoord);
+            firstCoordinate = item->coordinate();
+            break;
         }
     }
-}
 
+    // No item specifying a coordinate was found, in this case it we have a clickCoordinate use that
+    if (!firstCoordinate.isValid()) {
+        firstCoordinate = clickCoordinate;
+    }
 
-void MissionController::_recalcAll(void)
+    if (firstCoordinate.isValid()) {
+        QGeoCoordinate plannedHomeCoord = firstCoordinate.atDistanceAndAzimuth(30, 0);
+        plannedHomeCoord.setAltitude(0);
+        _settingsItem->setCoordinate(plannedHomeCoord);
+    }
+}
+
+/// @param clickCoordinate The location of the user click when inserting a new item
+void MissionController::_recalcAllWithClickCoordinate(QGeoCoordinate& clickCoordinate)
 {
     if (!_flyView) {
-        _setPlannedHomePositionFromFirstCoordinate();
+        _setPlannedHomePositionFromFirstCoordinate(clickCoordinate);
     }
     _recalcSequence();
     _recalcChildItems();
     _recalcWaypointLines();
 }
 
+void MissionController::_recalcAll(void)
+{
+    QGeoCoordinate emptyCoord;
+    _recalcAllWithClickCoordinate(emptyCoord);
+}
+
 /// Initializes a new set of mission items
 void MissionController::_initAllVisualItems(void)
 {

src/MissionManager/MissionController.h

@@ -225,12 +225,13 @@ private slots:
     void _visualItemsDirtyChanged(bool dirty);
     void _managerSendComplete(bool error);
     void _managerRemoveAllComplete(bool error);
+    void _recalcAll(void);
 
 private:
     void _init(void);
     void _recalcSequence(void);
     void _recalcChildItems(void);
-    void _recalcAll(void);
+    void _recalcAllWithClickCoordinate(QGeoCoordinate& clickCoordinate);
     void _initAllVisualItems(void);
     void _deinitAllVisualItems(void);
     void _initVisualItem(VisualMissionItem* item);
@@ -249,7 +250,7 @@ private:
     int _nextSequenceNumber(void);
     void _scanForAdditionalSettings(QmlObjectListModel* visualItems, Vehicle* vehicle);
     static bool _convertToMissionItems(QmlObjectListModel* visualMissionItems, QList<MissionItem*>& rgMissionItems, QObject* missionItemParent);
-    void _setPlannedHomePositionFromFirstCoordinate(void);
+    void _setPlannedHomePositionFromFirstCoordinate(const QGeoCoordinate& clickCoordinate);
     void _resetMissionFlightStatus(void);
     void _addHoverTime(double hoverTime, double hoverDistance, int waypointIndex);
     void _addCruiseTime(double cruiseTime, double cruiseDistance, int wayPointIndex);

src/MissionManager/MissionControllerTest.cc

@@ -135,9 +135,8 @@ void MissionControllerTest::_testAddWaypointWorker(MAV_AUTOPILOT firmwareType)
     QCOMPARE((MAV_CMD)simpleItem->command(), MAV_CMD_NAV_TAKEOFF);
     QCOMPARE(simpleItem->childItems()->count(), 0);
 
-    // If the first item added specifies a coordinate, then planned home position will be set
-    bool plannedHomePositionValue = firmwareType == MAV_AUTOPILOT_ARDUPILOTMEGA ? false : true;
-    QCOMPARE(settingsItem->coordinate().isValid(), plannedHomePositionValue);
+    // Planned home position should always be set after first item
+    QVERIFY(settingsItem->coordinate().isValid());
 
     // ArduPilot takeoff command has no coordinate, so should be child item
     QCOMPARE(settingsItem->childItems()->count(), firmwareType == MAV_AUTOPILOT_ARDUPILOTMEGA ? 1 : 0);