Merge pull request #5055 from DonLakeFlyer/Fixes

Fix initial plan loading from vehicle

src/MissionManager/GeoFenceController.cc

@@ -186,6 +186,7 @@ void GeoFenceController::sendToVehicle(void)
     } else if (syncInProgress()) {
         qCWarning(GeoFenceControllerLog) << "GeoFenceController::sendToVehicle called while syncInProgress";
     } else {
+        qCDebug(GeoFenceControllerLog) << "GeoFenceController::sendToVehicle";
         _geoFenceManager->sendToVehicle(_breachReturnPoint, _mapPolygon.pathModel());
         _mapPolygon.setDirty(false);
         setDirty(false);
@@ -316,14 +317,14 @@ void GeoFenceController::_updateContainsItems(void)
 
 bool GeoFenceController::showPlanFromManagerVehicle(void)
 {
-    qCDebug(GeoFenceControllerLog) << "showPlanFromManagerVehicle";
+    qCDebug(GeoFenceControllerLog) << "showPlanFromManagerVehicle" << _editMode;
     if (_masterController->offline()) {
         qCWarning(GeoFenceControllerLog) << "GeoFenceController::showPlanFromManagerVehicle called while offline";
         return true;    // stops further propogation of showPlanFromManagerVehicle due to error
     } else {
         _itemsRequested = true;
         if (!_managerVehicle->initialPlanRequestComplete()) {
-            // The vehicle hasn't completed initial load, we can just wait for newMissionItemsAvailable to be signalled automatically
+            // The vehicle hasn't completed initial load, we can just wait for loadComplete to be signalled automatically
             qCDebug(GeoFenceControllerLog) << "showPlanFromManagerVehicle: !initialPlanRequestComplete, wait for signal";
             return true;
         } else if (syncInProgress()) {

src/MissionManager/MissionController.cc

@@ -205,6 +205,7 @@ void MissionController::sendToVehicle(void)
     } else if (syncInProgress()) {
         qCWarning(MissionControllerLog) << "MissionControllerLog::sendToVehicle called while syncInProgress";
     } else {
+        qCDebug(MissionControllerLog) << "MissionControllerLog::sendToVehicle";
         if (_visualItems->count() == 1) {
             // This prevents us from sending a possibly bogus home position to the vehicle
             QmlObjectListModel emptyModel;
@@ -1678,7 +1679,7 @@ void MissionController::_visualItemsDirtyChanged(bool dirty)
 
 bool MissionController::showPlanFromManagerVehicle (void)
 {
-    qCDebug(MissionControllerLog) << "showPlanFromManagerVehicle";
+    qCDebug(MissionControllerLog) << "showPlanFromManagerVehicle" << _editMode;
     if (_masterController->offline()) {
         qCWarning(MissionControllerLog) << "MissionController::showPlanFromManagerVehicle called while offline";
         return true;    // stops further propogation of showPlanFromManagerVehicle due to error

src/MissionManager/MissionManager.cc

@@ -409,11 +409,6 @@ void MissionManager::_requestNextMissionItem(void)
 
 void MissionManager::_handleMissionItem(const mavlink_message_t& message, bool missionItemInt)
 {
-    
-    if (!_checkForExpectedAck(AckMissionItem)) {
-        return;
-    }
-
     MAV_CMD     command;
     MAV_FRAME   frame;
     double      param1;
@@ -468,7 +463,24 @@ void MissionManager::_handleMissionItem(const mavlink_message_t& message, bool m
         frame = MAV_FRAME_GLOBAL_RELATIVE_ALT;
     }
     
-    qCDebug(MissionManagerLog) << "_handleMissionItem seq:command:current" << seq << command << isCurrentItem;
+
+    bool ardupilotHomePositionUpdate = false;
+    if (!_checkForExpectedAck(AckMissionItem)) {
+        if (_vehicle->apmFirmware() && seq ==  0) {
+            ardupilotHomePositionUpdate = true;
+        } else {
+            qCDebug(MissionManagerLog) << "_handleMissionItem dropping spurious item seq:command:current" << seq << command << isCurrentItem;
+            return;
+        }
+    }
+
+    qCDebug(MissionManagerLog) << "_handleMissionItem seq:command:current:ardupilotHomePositionUpdate" << seq << command << isCurrentItem << ardupilotHomePositionUpdate;
+
+    if (ardupilotHomePositionUpdate) {
+        QGeoCoordinate newHomePosition(param5, param6, param7);
+        _vehicle->_setHomePosition(newHomePosition);
+        return;
+    }
     
     if (_itemIndicesToRead.contains(seq)) {
         _itemIndicesToRead.removeOne(seq);

src/MissionManager/RallyPointController.cc

@@ -157,6 +157,7 @@ void RallyPointController::sendToVehicle(void)
     } else if (syncInProgress()) {
         qCWarning(RallyPointControllerLog) << "RallyPointController::sendToVehicle called while syncInProgress";
     } else {
+        qCDebug(RallyPointControllerLog) << "RallyPointController::sendToVehicle";
         setDirty(false);
         QList<QGeoCoordinate> rgPoints;
         for (int i=0; i<_points.count(); i++) {
@@ -280,13 +281,13 @@ void RallyPointController::_updateContainsItems(void)
 
 bool RallyPointController::showPlanFromManagerVehicle (void)
 {
-    qCDebug(RallyPointControllerLog) << "showPlanFromManagerVehicle";
+    qCDebug(RallyPointControllerLog) << "showPlanFromManagerVehicle _editMode" << _editMode;
     if (_masterController->offline()) {
         qCWarning(RallyPointControllerLog) << "RallyPointController::showPlanFromManagerVehicle called while offline";
         return true;    // stops further propogation of showPlanFromManagerVehicle due to error
     } else {
         if (!_managerVehicle->initialPlanRequestComplete()) {
-            // The vehicle hasn't completed initial load, we can just wait for newMissionItemsAvailable to be signalled automatically
+            // The vehicle hasn't completed initial load, we can just wait for loadComplete to be signalled automatically
             qCDebug(RallyPointControllerLog) << "showPlanFromManagerVehicle: !initialPlanRequestComplete, wait for signal";
             return true;
         } else if (syncInProgress()) {

src/Vehicle/Vehicle.cc

@@ -669,6 +669,16 @@ void Vehicle::_handleAltitude(mavlink_message_t& message)
     }
 }
 
+void Vehicle::_setCapabilities(uint64_t capabilityBits)
+{
+    if (capabilityBits & MAV_PROTOCOL_CAPABILITY_MISSION_INT) {
+        _supportsMissionItemInt = true;
+    }
+    _vehicleCapabilitiesKnown = true;
+
+    qCDebug(VehicleLog) << QString("Vehicle %1 MISSION_ITEM_INT").arg(_supportsMissionItemInt ? QStringLiteral("supports") : QStringLiteral("does not support"));
+}
+
 void Vehicle::_handleAutopilotVersion(LinkInterface *link, mavlink_message_t& message)
 {
     Q_UNUSED(link);
@@ -703,12 +713,8 @@ void Vehicle::_handleAutopilotVersion(LinkInterface *link, mavlink_message_t& me
         emit gitHashChanged(_gitHash);
     }
 
-    if (autopilotVersion.capabilities & MAV_PROTOCOL_CAPABILITY_MISSION_INT) {
-        qCDebug(VehicleLog) << "Vehicle supports MISSION_ITEM_INT";
-        _supportsMissionItemInt = true;
-        _vehicleCapabilitiesKnown = true;
-        _startPlanRequest();
-    }
+    _setCapabilities(autopilotVersion.capabilities);
+    _startPlanRequest();
 }
 
 void Vehicle::_handleHilActuatorControls(mavlink_message_t &message)
@@ -745,6 +751,7 @@ void Vehicle::_handleCommandAck(mavlink_message_t& message)
     if (ack.command == MAV_CMD_REQUEST_AUTOPILOT_CAPABILITIES && ack.result != MAV_RESULT_ACCEPTED) {
         // We aren't going to get a response back for capabilities, so stop waiting for it before we ask for mission items
         qCDebug(VehicleLog) << "Vehicle failed to responded to MAV_CMD_REQUEST_AUTOPILOT_CAPABILITIES with error. Starting mission request.";
+        _setCapabilities(0);
         _startPlanRequest();
     }
 
@@ -899,6 +906,14 @@ void Vehicle::_handleBatteryStatus(mavlink_message_t& message)
     _batteryFactGroup.cellCount()->setRawValue(cellCount);
 }
 
+void Vehicle::_setHomePosition(QGeoCoordinate& homeCoord)
+{
+    if (homeCoord != _homePosition) {
+        _homePosition = homeCoord;
+        emit homePositionChanged(_homePosition);
+    }
+}
+
 void Vehicle::_handleHomePosition(mavlink_message_t& message)
 {
     mavlink_home_position_t homePos;
@@ -908,10 +923,7 @@ void Vehicle::_handleHomePosition(mavlink_message_t& message)
     QGeoCoordinate newHomePosition (homePos.latitude / 10000000.0,
                                     homePos.longitude / 10000000.0,
                                     homePos.altitude / 1000.0);
-    if (newHomePosition != _homePosition) {
-        _homePosition = newHomePosition;
-        emit homePositionChanged(_homePosition);
-    }
+    _setHomePosition(newHomePosition);
 }
 
 void Vehicle::_handleHeartbeat(mavlink_message_t& message)
@@ -1625,7 +1637,11 @@ void Vehicle::_mapTrajectoryStop()
 
 void Vehicle::_startPlanRequest(void)
 {
-    if (!_missionManagerInitialRequestSent && _parameterManager->parametersReady() && _vehicleCapabilitiesKnown) {
+    if (_missionManagerInitialRequestSent) {
+        return;
+    }
+
+    if (_parameterManager->parametersReady() && _vehicleCapabilitiesKnown) {
         qCDebug(VehicleLog) << "_startPlanRequest";
         _missionManagerInitialRequestSent = true;
         if (_settingsManager->appSettings()->autoLoadMissions()->rawValue().toBool()) {
@@ -1644,7 +1660,7 @@ void Vehicle::_startPlanRequest(void)
         if (!_parameterManager->parametersReady()) {
             qCDebug(VehicleLog) << "Delaying _startPlanRequest due to parameters not ready";
         } else if (!_vehicleCapabilitiesKnown) {
-            qCDebug(VehicleLog) << "Delaying _startPlanRequest due to vehicle capabilities not know";
+            qCDebug(VehicleLog) << "Delaying _startPlanRequest due to vehicle capabilities not known";
         }
     }
 }
@@ -2067,6 +2083,7 @@ void Vehicle::_sendMavCommandAgain(void)
     if (_mavCommandRetryCount++ > _mavCommandMaxRetryCount) {
         if (queuedCommand.command == MAV_CMD_REQUEST_AUTOPILOT_CAPABILITIES) {
             // We aren't going to get a response back for capabilities, so stop waiting for it before we ask for mission items
+            _setCapabilities(0);
             _startPlanRequest();
         }
 
@@ -2100,7 +2117,7 @@ void Vehicle::_sendMavCommandAgain(void)
                 }
             }
         }
-        qDebug() << "Vehicle::_sendMavCommandAgain retrying command:_mavCommandRetryCount" << queuedCommand.command << _mavCommandRetryCount;
+        qCDebug(VehicleLog) << "Vehicle::_sendMavCommandAgain retrying command:_mavCommandRetryCount" << queuedCommand.command << _mavCommandRetryCount;
     }
 
     _mavCommandAckTimer.start();

src/Vehicle/Vehicle.h

@@ -673,6 +673,8 @@ public:
     /// @return: true: initial request is complete, false: initial request is still in progress;
     bool initialPlanRequestComplete(void) const { return _initialPlanRequestComplete; }
 
+    void _setHomePosition(QGeoCoordinate& homeCoord);
+
 signals:
     void allLinksInactive(Vehicle* vehicle);
     void coordinateChanged(QGeoCoordinate coordinate);
@@ -834,6 +836,7 @@ private:
     void _commonInit(void);
     void _startPlanRequest(void);
     void _setupAutoDisarmSignalling(void);
+    void _setCapabilities(uint64_t capabilityBits);
 
     int     _id;                    ///< Mavlink system id
     int     _defaultComponentId;

src/ui/toolbar/ArmedIndicator.qml

@@ -26,11 +26,8 @@ QGCLabel {
     text:               _armed ? qsTr("Armed") : qsTr("Disarmed")
     font.pointSize:     ScreenTools.mediumFontPointSize
     color:              qgcPal.buttonText
-    visible:            !_autoDisarm || _fixedWing
 
     property var    _activeVehicle: QGroundControl.multiVehicleManager.activeVehicle
-    property bool   _autoDisarm:    _activeVehicle ? _activeVehicle.autoDisarm : false
-    property bool   _fixedWing:     _activeVehicle ? _activeVehicle.fixedWing : false
     property bool   _armed:         _activeVehicle ? _activeVehicle.armed : false
 
     QGCPalette { id: qgcPal }