Merge pull request #5636 from DonLakeFlyer/APMGuided

Better Guided Action support for ArduPilot

src/FirmwarePlugin/APM/APMFirmwarePlugin.cc

@@ -7,10 +7,6 @@
  *
  ****************************************************************************/
 
-
-/// @file
-///     @author Don Gagne <don@thegagnes.com>
-
 #include "APMFirmwarePlugin.h"
 #include "APMAutoPilotPlugin.h"
 #include "QGCMAVLink.h"
@@ -18,6 +14,7 @@
 #include "APMFlightModesComponentController.h"
 #include "APMAirframeComponentController.h"
 #include "APMSensorsComponentController.h"
+#include "MissionManager.h"
 
 #include <QTcpSocket>
 
@@ -159,9 +156,13 @@ AutoPilotPlugin* APMFirmwarePlugin::autopilotPlugin(Vehicle* vehicle)
     return new APMAutoPilotPlugin(vehicle, vehicle);
 }
 
-bool APMFirmwarePlugin::isCapable(const Vehicle* /*vehicle*/, FirmwareCapabilities capabilities)
+bool APMFirmwarePlugin::isCapable(const Vehicle* vehicle, FirmwareCapabilities capabilities)
 {
-    return (capabilities & (SetFlightModeCapability | PauseVehicleCapability)) == capabilities;
+    Q_UNUSED(vehicle);
+
+    uint32_t vehicleCapabilities = SetFlightModeCapability | GuidedModeCapability | PauseVehicleCapability;
+
+    return (capabilities & vehicleCapabilities) == capabilities;
 }
 
 QList<VehicleComponent*> APMFirmwarePlugin::componentsForVehicle(AutoPilotPlugin* vehicle)
@@ -725,12 +726,6 @@ bool APMFirmwarePlugin::isGuidedMode(const Vehicle* vehicle) const
     return vehicle->flightMode() == "Guided";
 }
 
-void APMFirmwarePlugin::pauseVehicle(Vehicle* vehicle)
-{
-    // Best we can do in this case
-    vehicle->setFlightMode("Loiter");
-}
-
 void APMFirmwarePlugin::_soloVideoHandshake(Vehicle* vehicle)
 {
     Q_UNUSED(vehicle);
@@ -819,3 +814,69 @@ QString APMFirmwarePlugin::internalParameterMetaDataFile(Vehicle* vehicle)
         return QString();
     }
 }
+
+void APMFirmwarePlugin::setGuidedMode(Vehicle* vehicle, bool guidedMode)
+{
+    if (guidedMode) {
+        _setFlightModeAndValidate(vehicle, "Guided");
+    } else {
+        pauseVehicle(vehicle);
+    }
+}
+
+void APMFirmwarePlugin::pauseVehicle(Vehicle* vehicle)
+{
+    _setFlightModeAndValidate(vehicle, pauseFlightMode());
+}
+
+void APMFirmwarePlugin::guidedModeGotoLocation(Vehicle* vehicle, const QGeoCoordinate& gotoCoord)
+{
+    if (qIsNaN(vehicle->altitudeRelative()->rawValue().toDouble())) {
+        qgcApp()->showMessage(QStringLiteral("Unable to go to location, vehicle position not known."));
+        return;
+    }
+
+
+    setGuidedMode(vehicle, true);
+
+    QGeoCoordinate coordWithAltitude = gotoCoord;
+    coordWithAltitude.setAltitude(vehicle->altitudeRelative()->rawValue().toDouble());
+    vehicle->missionManager()->writeArduPilotGuidedMissionItem(coordWithAltitude, false /* altChangeOnly */);
+}
+
+void APMFirmwarePlugin::guidedModeRTL(Vehicle* vehicle)
+{
+    _setFlightModeAndValidate(vehicle, rtlFlightMode());
+}
+
+void APMFirmwarePlugin::guidedModeChangeAltitude(Vehicle* vehicle, double altitudeChange)
+{
+    if (qIsNaN(vehicle->altitudeRelative()->rawValue().toDouble())) {
+        qgcApp()->showMessage(QStringLiteral("Unable to change altitude, vehicle altitude not known."));
+        return;
+    }
+
+    setGuidedMode(vehicle, true);
+
+    mavlink_message_t msg;
+    mavlink_set_position_target_local_ned_t cmd;
+
+    memset(&cmd, 0, sizeof(cmd));
+
+    cmd.target_system = vehicle->id();
+    cmd.target_component = vehicle->defaultComponentId();
+    cmd.coordinate_frame = MAV_FRAME_LOCAL_OFFSET_NED;
+    cmd.type_mask = 0xFFF8; // Only x/y/z valid
+    cmd.x = 0.0f;
+    cmd.y = 0.0f;
+    cmd.z = -(altitudeChange);
+
+    MAVLinkProtocol* mavlink = qgcApp()->toolbox()->mavlinkProtocol();
+    mavlink_msg_set_position_target_local_ned_encode_chan(mavlink->getSystemId(),
+                                                          mavlink->getComponentId(),
+                                                          vehicle->priorityLink()->mavlinkChannel(),
+                                                          &msg,
+                                                          &cmd);
+
+    vehicle->sendMessageOnLink(vehicle->priorityLink(), msg);
+}

src/FirmwarePlugin/APM/APMFirmwarePlugin.h

@@ -74,12 +74,18 @@ public:
     QList<MAV_CMD> supportedMissionCommands(void) final;
 
     AutoPilotPlugin*    autopilotPlugin                 (Vehicle* vehicle) final;
-    bool                isCapable                       (const Vehicle *vehicle, FirmwareCapabilities capabilities) override;
+    bool                isCapable                       (const Vehicle *vehicle, FirmwareCapabilities capabilities) final;
+    void                setGuidedMode                   (Vehicle* vehicle, bool guidedMode) final;
+    void                guidedModeGotoLocation          (Vehicle* vehicle, const QGeoCoordinate& gotoCoord) final;
     QStringList         flightModes                     (Vehicle* vehicle) final;
     QString             flightMode                      (uint8_t base_mode, uint32_t custom_mode) const final;
     bool                setFlightMode                   (const QString& flightMode, uint8_t* base_mode, uint32_t* custom_mode) final;
     bool                isGuidedMode                    (const Vehicle* vehicle) const final;
-    void                pauseVehicle                    (Vehicle* vehicle) override;
+    QString             rtlFlightMode                   (void) const final { return QString("RTL"); }
+    QString             missionFlightMode               (void) const final { return QString("Auto"); }
+    void                pauseVehicle                    (Vehicle* vehicle) final;
+    void                guidedModeRTL                   (Vehicle* vehicle) final;
+    void                guidedModeChangeAltitude        (Vehicle* vehicle, double altitudeChange) override;
     int                 manualControlReservedButtonCount(void) override;
     bool                adjustIncomingMavlinkMessage    (Vehicle* vehicle, mavlink_message_t* message) override;
     void                adjustOutgoingMavlinkMessage    (Vehicle* vehicle, LinkInterface* outgoingLink, mavlink_message_t* message) final;

src/FirmwarePlugin/APM/ArduCopterFirmwarePlugin.cc

@@ -151,20 +151,6 @@ int ArduCopterFirmwarePlugin::remapParamNameHigestMinorVersionNumber(int majorVe
     return majorVersionNumber == 3 ? 5 : Vehicle::versionNotSetValue;
 }
 
-bool ArduCopterFirmwarePlugin::isCapable(const Vehicle* vehicle, FirmwareCapabilities capabilities)
-{
-    Q_UNUSED(vehicle);
-
-    uint32_t vehicleCapabilities = SetFlightModeCapability | GuidedModeCapability | PauseVehicleCapability;
-
-    return (capabilities & vehicleCapabilities) == capabilities;
-}
-
-void ArduCopterFirmwarePlugin::guidedModeRTL(Vehicle* vehicle)
-{
-    _setFlightModeAndValidate(vehicle, "RTL");
-}
-
 void ArduCopterFirmwarePlugin::guidedModeLand(Vehicle* vehicle)
 {
     _setFlightModeAndValidate(vehicle, "Land");
@@ -209,64 +195,6 @@ bool ArduCopterFirmwarePlugin::_guidedModeTakeoff(Vehicle* vehicle)
     return true;
 }
 
-void ArduCopterFirmwarePlugin::guidedModeGotoLocation(Vehicle* vehicle, const QGeoCoordinate& gotoCoord)
-{
-    if (qIsNaN(vehicle->altitudeRelative()->rawValue().toDouble())) {
-        qgcApp()->showMessage(QStringLiteral("Unable to go to location, vehicle position not known."));
-        return;
-    }
-
-    QGeoCoordinate coordWithAltitude = gotoCoord;
-    coordWithAltitude.setAltitude(vehicle->altitudeRelative()->rawValue().toDouble());
-    vehicle->missionManager()->writeArduPilotGuidedMissionItem(coordWithAltitude, false /* altChangeOnly */);
-}
-
-void ArduCopterFirmwarePlugin::guidedModeChangeAltitude(Vehicle* vehicle, double altitudeChange)
-{
-    if (qIsNaN(vehicle->altitudeRelative()->rawValue().toDouble())) {
-        qgcApp()->showMessage(QStringLiteral("Unable to change altitude, vehicle altitude not known."));
-        return;
-    }
-
-    setGuidedMode(vehicle, true);
-
-    mavlink_message_t msg;
-    mavlink_set_position_target_local_ned_t cmd;
-
-    memset(&cmd, 0, sizeof(cmd));
-
-    cmd.target_system = vehicle->id();
-    cmd.target_component = vehicle->defaultComponentId();
-    cmd.coordinate_frame = MAV_FRAME_LOCAL_OFFSET_NED;
-    cmd.type_mask = 0xFFF8; // Only x/y/z valid
-    cmd.x = 0.0f;
-    cmd.y = 0.0f;
-    cmd.z = -(altitudeChange);
-
-    MAVLinkProtocol* mavlink = qgcApp()->toolbox()->mavlinkProtocol();
-    mavlink_msg_set_position_target_local_ned_encode_chan(mavlink->getSystemId(),
-                                                          mavlink->getComponentId(),
-                                                          vehicle->priorityLink()->mavlinkChannel(),
-                                                          &msg,
-                                                          &cmd);
-
-    vehicle->sendMessageOnLink(vehicle->priorityLink(), msg);
-}
-
-void ArduCopterFirmwarePlugin::pauseVehicle(Vehicle* vehicle)
-{
-    _setFlightModeAndValidate(vehicle, "Brake");
-}
-
-void ArduCopterFirmwarePlugin::setGuidedMode(Vehicle* vehicle, bool guidedMode)
-{
-    if (guidedMode) {
-        _setFlightModeAndValidate(vehicle, "Guided");
-    } else {
-        pauseVehicle(vehicle);
-    }
-}
-
 bool ArduCopterFirmwarePlugin::multiRotorCoaxialMotors(Vehicle* vehicle)
 {
     Q_UNUSED(vehicle);

src/FirmwarePlugin/APM/ArduCopterFirmwarePlugin.h

@@ -55,22 +55,14 @@ public:
     ArduCopterFirmwarePlugin(void);
 
     // Overrides from FirmwarePlugin
-    bool    isCapable                           (const Vehicle *vehicle, FirmwareCapabilities capabilities) final;
-    void    setGuidedMode                       (Vehicle* vehicle, bool guidedMode) final;
-    void    pauseVehicle                        (Vehicle* vehicle) final;
-    void    guidedModeRTL                       (Vehicle* vehicle) final;
     void    guidedModeLand                      (Vehicle* vehicle) final;
     void    guidedModeTakeoff                   (Vehicle* vehicle) final;
-    void    guidedModeGotoLocation              (Vehicle* vehicle, const QGeoCoordinate& gotoCoord) final;
-    void    guidedModeChangeAltitude            (Vehicle* vehicle, double altitudeChange) final;
     const FirmwarePlugin::remapParamNameMajorVersionMap_t& paramNameRemapMajorVersionMap(void) const final { return _remapParamName; }
     int     remapParamNameHigestMinorVersionNumber(int majorVersionNumber) const final;
     bool    multiRotorCoaxialMotors             (Vehicle* vehicle) final;
     bool    multiRotorXConfig                   (Vehicle* vehicle) final;
     QString offlineEditingParamFile             (Vehicle* vehicle) final { Q_UNUSED(vehicle); return QStringLiteral(":/FirmwarePlugin/APM/Copter.OfflineEditing.params"); }
     QString pauseFlightMode                     (void) const override { return QString("Brake"); }
-    QString missionFlightMode                   (void) const override { return QString("Auto"); }
-    QString rtlFlightMode                       (void) const override { return QString("RTL"); }
     QString landFlightMode                      (void) const override { return QString("Land"); }
     QString takeControlFlightMode               (void) const override { return QString("Loiter"); }
     bool    vehicleYawsToNextWaypointInMission  (const Vehicle* vehicle) const final;

src/FirmwarePlugin/APM/ArduPlaneFirmwarePlugin.h

@@ -56,10 +56,11 @@ public:
     ArduPlaneFirmwarePlugin(void);
 
     // Overrides from FirmwarePlugin
-    QString offlineEditingParamFile(Vehicle* vehicle) final { Q_UNUSED(vehicle); return QStringLiteral(":/FirmwarePlugin/APM/Plane.OfflineEditing.params"); }
+    QString pauseFlightMode                         (void) const override { return QString("Loiter"); }
+    QString offlineEditingParamFile                 (Vehicle* vehicle) final { Q_UNUSED(vehicle); return QStringLiteral(":/FirmwarePlugin/APM/Plane.OfflineEditing.params"); }
+    QString autoDisarmParameter                     (Vehicle* vehicle) final { Q_UNUSED(vehicle); return QStringLiteral("LAND_DISARMDELAY"); }
+    int     remapParamNameHigestMinorVersionNumber  (int majorVersionNumber) const final;
     const FirmwarePlugin::remapParamNameMajorVersionMap_t& paramNameRemapMajorVersionMap(void) const final { return _remapParamName; }
-    int remapParamNameHigestMinorVersionNumber(int majorVersionNumber) const final;
-    QString autoDisarmParameter(Vehicle* vehicle) final { Q_UNUSED(vehicle); return QStringLiteral("LAND_DISARMDELAY"); }
 
 private:
     static bool _remapParamNameIntialized;

src/FirmwarePlugin/APM/ArduRoverFirmwarePlugin.cc

@@ -8,6 +8,7 @@
  ****************************************************************************/
 
 #include "ArduRoverFirmwarePlugin.h"
+#include "QGCApplication.h"
 
 bool ArduRoverFirmwarePlugin::_remapParamNameIntialized = false;
 FirmwarePlugin::remapParamNameMajorVersionMap_t ArduRoverFirmwarePlugin::_remapParamName;
@@ -98,3 +99,10 @@ int ArduRoverFirmwarePlugin::remapParamNameHigestMinorVersionNumber(int majorVer
     return majorVersionNumber == 3 ? 2 : Vehicle::versionNotSetValue;
 }
 
+void ArduRoverFirmwarePlugin::guidedModeChangeAltitude(Vehicle* vehicle, double altitudeChange)
+{
+    Q_UNUSED(vehicle);
+    Q_UNUSED(altitudeChange);
+
+    qgcApp()->showMessage(QStringLiteral("Change altitude not supported."));
+}

src/FirmwarePlugin/APM/ArduRoverFirmwarePlugin.h

@@ -51,8 +51,10 @@ public:
     ArduRoverFirmwarePlugin(void);
 
     // Overrides from FirmwarePlugin
+    QString pauseFlightMode                         (void) const override { return QString("Hold"); }
+    void    guidedModeChangeAltitude                (Vehicle* vehicle, double altitudeChange) final;
+    int     remapParamNameHigestMinorVersionNumber  (int majorVersionNumber) const final;
     const FirmwarePlugin::remapParamNameMajorVersionMap_t& paramNameRemapMajorVersionMap(void) const final { return _remapParamName; }
-    int remapParamNameHigestMinorVersionNumber(int majorVersionNumber) const final;
 
 private:
     static bool _remapParamNameIntialized;

src/FlightDisplay/FlightDisplayViewMap.qml

@@ -261,7 +261,7 @@ FlightMap {
     // GoTo here waypoint
     MapQuickItem {
         coordinate:     _gotoHereCoordinate
-        visible:        _activeVehicle && _activeVehicle.guidedMode && _gotoHereCoordinate.isValid
+        visible:        _activeVehicle && _activeVehicle.guidedModeSupported && _gotoHereCoordinate.isValid
         z:              QGroundControl.zOrderMapItems
         anchorPoint.x:  sourceItem.anchorPointX
         anchorPoint.y:  sourceItem.anchorPointY

src/FlightDisplay/GuidedActionsController.qml

@@ -101,7 +101,7 @@ Item {
     property bool showChangeAlt:        (_activeVehicle && _vehicleFlying) && _activeVehicle.guidedModeSupported && _vehicleArmed && !_missionActive
     property bool showOrbit:            !_hideOrbit && _activeVehicle && _vehicleFlying && _activeVehicle.orbitModeSupported && _vehicleArmed && !_missionActive
     property bool showLandAbort:        _activeVehicle && _vehicleFlying && _activeVehicle.fixedWing && _vehicleLanding
-    property bool showGotoLocation:     _activeVehicle && _activeVehicle.guidedMode && _vehicleFlying
+    property bool showGotoLocation:     _activeVehicle && _vehicleFlying
 
     property bool guidedUIVisible:      guidedActionConfirm.visible || guidedActionList.visible
 

src/MissionManager/MissionManager.cc

@@ -40,6 +40,8 @@ void MissionManager::writeArduPilotGuidedMissionItem(const QGeoCoordinate& gotoC
 
     _transactionInProgress = TransactionWrite;
 
+    _connectToMavlink();
+
     mavlink_message_t       messageOut;
     mavlink_mission_item_t  missionItem;
 

src/MissionManager/MissionSettingsItem.cc

@@ -233,7 +233,6 @@ void MissionSettingsItem::setCoordinate(const QGeoCoordinate& coordinate)
     if (_plannedHomePositionCoordinate != coordinate) {
         // ArduPilot tends to send crap home positions at initial vehicel boot, discard them
         if (coordinate.isValid() && (coordinate.latitude() != 0 || coordinate.longitude() != 0)) {
-            qDebug() << "Setting home position" << coordinate;
             _plannedHomePositionCoordinate = coordinate;
             emit coordinateChanged(coordinate);
             emit exitCoordinateChanged(coordinate);

src/Vehicle/Vehicle.cc

@@ -1733,8 +1733,6 @@ void Vehicle::setFlightMode(const QString& flightMode)
     uint8_t     base_mode;
     uint32_t    custom_mode;
 
-    qDebug() << flightMode;
-
     if (_firmwarePlugin->setFlightMode(flightMode, &base_mode, &custom_mode)) {
         // setFlightMode will only set MAV_MODE_FLAG_CUSTOM_MODE_ENABLED in base_mode, we need to move back in the existing
         // states.