Add Guided mode support

src/FirmwarePlugin/APM/APMFirmwarePlugin.h

@@ -84,12 +84,15 @@ public:
     QList<VehicleComponent*> componentsForVehicle(AutoPilotPlugin* vehicle) final;
     QList<MAV_CMD> supportedMissionCommands(void) final;
 
-    bool        isCapable(FirmwareCapabilities capabilities) final;
+    bool        isCapable(FirmwareCapabilities capabilities);
     QStringList flightModes(void) final;
-    QString     flightMode(uint8_t base_mode, uint32_t custom_mode) 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);
     int         manualControlReservedButtonCount(void) final;
-    void        adjustMavlinkMessage(Vehicle* vehicle, mavlink_message_t* message) final;
+    void        adjustIncomingMavlinkMessage(Vehicle* vehicle, mavlink_message_t* message) final;
+    void        adjustOutgoingMavlinkMessage(Vehicle* vehicle, mavlink_message_t* message) final;
     void        initializeVehicle(Vehicle* vehicle) final;
     bool        sendHomePositionToVehicle(void) final;
     void        addMetaDataToFact(QObject* parameterMetaData, Fact* fact, MAV_TYPE vehicleType) final;
@@ -111,6 +114,10 @@ private:
     static bool _isTextSeverityAdjustmentNeeded(const APMFirmwareVersion& firmwareVersion);
     void _setInfoSeverity(mavlink_message_t* message) const;
     QString _getMessageText(mavlink_message_t* message) const;
+    void _handleParamValue(Vehicle* vehicle, mavlink_message_t* message);
+    void _handleParamSet(Vehicle* vehicle, mavlink_message_t* message);
+    void _handleStatusText(Vehicle* vehicle, mavlink_message_t* message);
+    void _handleHeartbeat(Vehicle* vehicle, mavlink_message_t* message);
 
     APMFirmwareVersion      _firmwareVersion;
     bool                    _textSeverityAdjustmentNeeded;

src/FirmwarePlugin/APM/ArduCopterFirmwarePlugin.cc

@@ -25,6 +25,8 @@
 ///     @author Don Gagne <don@thegagnes.com>
 
 #include "ArduCopterFirmwarePlugin.h"
+#include "QGCApplication.h"
+#include "MissionManager.h"
 
 APMCopterMode::APMCopterMode(uint32_t mode, bool settable) :
     APMCustomMode(mode, settable)
@@ -73,3 +75,104 @@ ArduCopterFirmwarePlugin::ArduCopterFirmwarePlugin(void)
     supportedFlightModes << APMCopterMode(APMCopterMode::BRAKE     ,true);
     setSupportedModes(supportedFlightModes);
 }
+
+bool ArduCopterFirmwarePlugin::isCapable(FirmwareCapabilities capabilities)
+{
+    return (capabilities & (SetFlightModeCapability | GuidedModeCapability | PauseVehicleCapability)) == capabilities;
+}
+
+void ArduCopterFirmwarePlugin::guidedModeRTL(Vehicle* vehicle)
+{
+    vehicle->setFlightMode("RTL");
+}
+
+void ArduCopterFirmwarePlugin::guidedModeLand(Vehicle* vehicle)
+{
+    vehicle->setFlightMode("Land");
+}
+
+void ArduCopterFirmwarePlugin::guidedModeTakeoff(Vehicle* vehicle, double altitudeRel)
+{
+    if (qIsNaN(vehicle->altitudeAMSL()->rawValue().toDouble())) {
+        qgcApp()->showMessage(QStringLiteral("Unable to takeoff, vehicle position not known."));
+        return;
+    }
+
+    mavlink_message_t msg;
+    mavlink_command_long_t cmd;
+
+    cmd.command = (uint16_t)MAV_CMD_NAV_TAKEOFF;
+    cmd.confirmation = 0;
+    cmd.param1 = 0.0f;
+    cmd.param2 = 0.0f;
+    cmd.param3 = 0.0f;
+    cmd.param4 = 0.0f;
+    cmd.param5 = 0.0f;
+    cmd.param6 = 0.0f;
+    cmd.param7 = vehicle->altitudeAMSL()->rawValue().toFloat() +  altitudeRel; // AMSL meters
+    cmd.target_system = vehicle->id();
+    cmd.target_component = 0;
+
+    MAVLinkProtocol* mavlink = qgcApp()->toolbox()->mavlinkProtocol();
+    mavlink_msg_command_long_encode(mavlink->getSystemId(), mavlink->getComponentId(), &msg, &cmd);
+
+    vehicle->sendMessage(msg);
+}
+
+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;
+    }
+
+    vehicle->setFlightMode("Guided");
+    QGeoCoordinate coordWithAltitude = gotoCoord;
+    coordWithAltitude.setAltitude(vehicle->altitudeRelative()->rawValue().toDouble());
+    vehicle->missionManager()->writeArduPilotGuidedMissionItem(coordWithAltitude, false /* altChangeOnly */);
+}
+
+void ArduCopterFirmwarePlugin::guidedModeChangeAltitude(Vehicle* vehicle, double altitudeRel)
+{
+    if (qIsNaN(vehicle->altitudeRelative()->rawValue().toDouble())) {
+        qgcApp()->showMessage(QStringLiteral("Unable to change altitude, vehicle altitude not known."));
+        return;
+    }
+
+    mavlink_message_t msg;
+    mavlink_set_position_target_local_ned_t cmd;
+
+    memset(&cmd, 0, sizeof(mavlink_set_position_target_local_ned_t));
+
+    cmd.target_system = vehicle->id();
+    cmd.target_component = 0;
+    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 = -(altitudeRel - vehicle->altitudeRelative()->rawValue().toDouble());
+
+    MAVLinkProtocol* mavlink = qgcApp()->toolbox()->mavlinkProtocol();
+    mavlink_msg_set_position_target_local_ned_encode(mavlink->getSystemId(), mavlink->getComponentId(), &msg, &cmd);
+
+    vehicle->sendMessage(msg);
+}
+
+bool ArduCopterFirmwarePlugin::isPaused(const Vehicle* vehicle) const
+{
+    return vehicle->flightMode() == "Brake";
+}
+
+void ArduCopterFirmwarePlugin::pauseVehicle(Vehicle* vehicle)
+{
+    vehicle->setFlightMode("Brake");
+}
+
+void ArduCopterFirmwarePlugin::setGuidedMode(Vehicle* vehicle, bool guidedMode)
+{
+    if (guidedMode) {
+        vehicle->setFlightMode("Guided");
+    } else {
+        pauseVehicle(vehicle);
+    }
+}

src/FirmwarePlugin/APM/ArduCopterFirmwarePlugin.h

@@ -64,6 +64,17 @@ class ArduCopterFirmwarePlugin : public APMFirmwarePlugin
     
 public:
     ArduCopterFirmwarePlugin(void);
+
+    // Overrides from FirmwarePlugin
+    bool isCapable(FirmwareCapabilities capabilities) final;
+    bool isPaused(const Vehicle* vehicle) const 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, double altitudeRel) final;
+    void guidedModeGotoLocation(Vehicle* vehicle, const QGeoCoordinate& gotoCoord) final;
+    void guidedModeChangeAltitude(Vehicle* vehicle, double altitudeRel) final;
 };
 
 #endif

src/FirmwarePlugin/FirmwarePlugin.cc

@@ -22,9 +22,16 @@
  ======================================================================*/
 
 #include "FirmwarePlugin.h"
+#include "QGCApplication.h"
 
 #include <QDebug>
 
+bool FirmwarePlugin::isCapable(FirmwareCapabilities capabilities)
+{
+    Q_UNUSED(capabilities);
+    return false;
+}
+
 QList<VehicleComponent*> FirmwarePlugin::componentsForVehicle(AutoPilotPlugin* vehicle)
 {
     Q_UNUSED(vehicle);
@@ -32,7 +39,7 @@ QList<VehicleComponent*> FirmwarePlugin::componentsForVehicle(AutoPilotPlugin* v
     return QList<VehicleComponent*>();
 }
 
-QString FirmwarePlugin::flightMode(uint8_t base_mode, uint32_t custom_mode)
+QString FirmwarePlugin::flightMode(uint8_t base_mode, uint32_t custom_mode) const
 {
     QString flightMode;
     
@@ -86,11 +93,17 @@ int FirmwarePlugin::manualControlReservedButtonCount(void)
     return -1;
 }
 
-void FirmwarePlugin::adjustMavlinkMessage(Vehicle* vehicle, mavlink_message_t* message)
+void FirmwarePlugin::adjustIncomingMavlinkMessage(Vehicle* vehicle, mavlink_message_t* message)
+{
+    Q_UNUSED(vehicle);
+    Q_UNUSED(message);    
+    // Generic plugin does no message adjustment
+}
+
+void FirmwarePlugin::adjustOutgoingMavlinkMessage(Vehicle* vehicle, mavlink_message_t* message)
 {
     Q_UNUSED(vehicle);
     Q_UNUSED(message);
-    
     // Generic plugin does no message adjustment
 }
 
@@ -129,3 +142,69 @@ void FirmwarePlugin::getParameterMetaDataVersionInfo(const QString& metaDataFile
     majorVersion = -1;
     minorVersion = -1;
 }
+
+bool FirmwarePlugin::isGuidedMode(const Vehicle* vehicle) const
+{
+    // Not supported by generic vehicle
+    Q_UNUSED(vehicle);
+    return false;
+}
+
+void FirmwarePlugin::setGuidedMode(Vehicle* vehicle, bool guidedMode)
+{
+    Q_UNUSED(vehicle);
+    Q_UNUSED(guidedMode);
+    qgcApp()->showMessage(QStringLiteral("Guided mode not supported by Vehicle."));
+}
+
+bool FirmwarePlugin::isPaused(const Vehicle* vehicle) const
+{
+    // Not supported by generic vehicle
+    Q_UNUSED(vehicle);
+    return false;
+}
+
+void FirmwarePlugin::pauseVehicle(Vehicle* vehicle)
+{
+    // Not supported by generic vehicle
+    Q_UNUSED(vehicle);
+    qgcApp()->showMessage(QStringLiteral("Guided mode not supported by Vehicle."));
+}
+
+void FirmwarePlugin::guidedModeRTL(Vehicle* vehicle)
+{
+    // Not supported by generic vehicle
+    Q_UNUSED(vehicle);
+    qgcApp()->showMessage(QStringLiteral("Guided mode not supported by Vehicle."));
+}
+
+void FirmwarePlugin::guidedModeLand(Vehicle* vehicle)
+{
+    // Not supported by generic vehicle
+    Q_UNUSED(vehicle);
+    qgcApp()->showMessage(QStringLiteral("Guided mode not supported by Vehicle."));
+}
+
+void FirmwarePlugin::guidedModeTakeoff(Vehicle* vehicle, double altitudeRel)
+{
+    // Not supported by generic vehicle
+    Q_UNUSED(vehicle);
+    Q_UNUSED(altitudeRel);
+    qgcApp()->showMessage(QStringLiteral("Guided mode not supported by Vehicle."));
+}
+
+void FirmwarePlugin::guidedModeGotoLocation(Vehicle* vehicle, const QGeoCoordinate& gotoCoord)
+{
+    // Not supported by generic vehicle
+    Q_UNUSED(vehicle);
+    Q_UNUSED(gotoCoord);
+    qgcApp()->showMessage(QStringLiteral("Guided mode not supported by Vehicle."));
+}
+
+void FirmwarePlugin::guidedModeChangeAltitude(Vehicle* vehicle, double altitudeRel)
+{
+    // Not supported by generic vehicle
+    Q_UNUSED(vehicle);
+    Q_UNUSED(altitudeRel);
+    qgcApp()->showMessage(QStringLiteral("Guided mode not supported by Vehicle."));
+}

src/FirmwarePlugin/FirmwarePlugin.h

@@ -50,12 +50,14 @@ class FirmwarePlugin : public QObject
 public:
     /// Set of optional capabilites which firmware may support
     typedef enum {
-        SetFlightModeCapability,            ///< FirmwarePlugin::setFlightMode method is supported
-        MavCmdPreflightStorageCapability,   ///< MAV_CMD_PREFLIGHT_STORAGE is supported        
+        SetFlightModeCapability =           1 << 0, ///< FirmwarePlugin::setFlightMode method is supported
+        MavCmdPreflightStorageCapability =  1 << 1, ///< MAV_CMD_PREFLIGHT_STORAGE is supported
+        PauseVehicleCapability =            1 << 2, ///< Vehicle supports pausing at current location
+        GuidedModeCapability =              1 << 3, ///< Vehicle Support guided mode commands
     } FirmwareCapabilities;
     
     /// @return true: Firmware supports all specified capabilites
-    virtual bool isCapable(FirmwareCapabilities capabilities) { Q_UNUSED(capabilities); return false; }
+    virtual bool isCapable(FirmwareCapabilities capabilities);
 
     /// Returns VehicleComponents for specified Vehicle
     ///     @param vehicle Vehicle  to associate with components
@@ -69,13 +71,42 @@ public:
     /// Returns the name for this flight mode. Flight mode names must be human readable as well as audio speakable.
     ///     @param base_mode Base mode from mavlink HEARTBEAT message
     ///     @param custom_mode Custom mode from mavlink HEARTBEAT message
-    virtual QString flightMode(uint8_t base_mode, uint32_t custom_mode);
+    virtual QString flightMode(uint8_t base_mode, uint32_t custom_mode) const;
     
     /// Sets base_mode and custom_mode to specified flight mode.
     ///     @param[out] base_mode Base mode for SET_MODE mavlink message
     ///     @param[out] custom_mode Custom mode for SET_MODE mavlink message
     virtual bool setFlightMode(const QString& flightMode, uint8_t* base_mode, uint32_t* custom_mode);
-    
+
+    /// Returns whether the vehicle is in guided mode or not.
+    virtual bool isGuidedMode(const Vehicle* vehicle) const;
+
+    /// Set guided flight mode
+    virtual void setGuidedMode(Vehicle* vehicle, bool guidedMode);
+
+    /// Returns whether the vehicle is paused or not.
+    virtual bool isPaused(const Vehicle* vehicle) const;
+
+    /// Causes the vehicle to stop at current position. If guide mode is supported, vehicle will be let in guide mode.
+    /// If not, vehicle will be left in Loiter.
+    virtual void pauseVehicle(Vehicle* vehicle);
+
+    /// Command vehicle to return to launch
+    virtual void guidedModeRTL(Vehicle* vehicle);
+
+    /// Command vehicle to land at current location
+    virtual void guidedModeLand(Vehicle* vehicle);
+
+    /// Command vehicle to takeoff from current location
+    ///     @param altitudeRel Relative altitude to takeoff to
+    virtual void guidedModeTakeoff(Vehicle* vehicle, double altitudeRel);
+
+    /// Command vehicle to move to specified location (altitude is included and relative)
+    virtual void guidedModeGotoLocation(Vehicle* vehicle, const QGeoCoordinate& gotoCoord);
+
+    /// Command vehicle to change to the specified relatice altitude
+    virtual void guidedModeChangeAltitude(Vehicle* vehicle, double altitudeRel);
+
     /// FIXME: This isn't quite correct being here. All code for Joystick suvehicleTypepport is currently firmware specific
     /// not just this. I'm going to try to change that. If not, this will need to be removed.
     /// Returns the number of buttons which are reserved for firmware use in the MANUAL_CONTROL mavlink
@@ -89,8 +120,15 @@ public:
     /// spec implementations such that the base code can remain mavlink generic.
     ///     @param vehicle Vehicle message came from
     ///     @param message[in,out] Mavlink message to adjust if needed.
-    virtual void adjustMavlinkMessage(Vehicle* vehicle, mavlink_message_t* message);
+    virtual void adjustIncomingMavlinkMessage(Vehicle* vehicle, mavlink_message_t* message);
     
+    /// Called before any mavlink message is sent to the Vehicle so plugin can adjust any message characteristics.
+    /// This is handy to adjust or differences in mavlink spec implementations such that the base code can remain
+    /// mavlink generic.
+    ///     @param vehicle Vehicle message came from
+    ///     @param message[in,out] Mavlink message to adjust if needed.
+    virtual void adjustOutgoingMavlinkMessage(Vehicle* vehicle, mavlink_message_t* message);
+
     /// Called when Vehicle is first created to send any necessary mavlink messages to the firmware.
     virtual void initializeVehicle(Vehicle* vehicle);
 

src/FirmwarePlugin/PX4/PX4FirmwarePlugin.cc

@@ -111,7 +111,7 @@ QStringList PX4FirmwarePlugin::flightModes(void)
     return flightModes;
 }
 
-QString PX4FirmwarePlugin::flightMode(uint8_t base_mode, uint32_t custom_mode)
+QString PX4FirmwarePlugin::flightMode(uint8_t base_mode, uint32_t custom_mode) const
 {
     QString flightMode = "Unknown";
 
@@ -178,17 +178,10 @@ int PX4FirmwarePlugin::manualControlReservedButtonCount(void)
     return 0;   // 0 buttons reserved for rc switch simulation
 }
 
-void PX4FirmwarePlugin::adjustMavlinkMessage(Vehicle* vehicle, mavlink_message_t* message)
-{
-    Q_UNUSED(vehicle);
-    Q_UNUSED(message);
-
-    // PX4 Flight Stack plugin does no message adjustment
-}
-
 bool PX4FirmwarePlugin::isCapable(FirmwareCapabilities capabilities)
 {
-    return (capabilities & (MavCmdPreflightStorageCapability | SetFlightModeCapability)) == capabilities;
+    qDebug() << (capabilities & (MavCmdPreflightStorageCapability | SetFlightModeCapability | PauseVehicleCapability)) << capabilities;
+    return (capabilities & (MavCmdPreflightStorageCapability | SetFlightModeCapability | PauseVehicleCapability)) == capabilities;
 }
 
 void PX4FirmwarePlugin::initializeVehicle(Vehicle* vehicle)
@@ -247,3 +240,8 @@ QObject* PX4FirmwarePlugin::loadParameterMetaData(const QString& metaDataFile)
     metaData->loadParameterFactMetaDataFile(metaDataFile);
     return metaData;
 }
+
+void PX4FirmwarePlugin::pauseVehicle(Vehicle* vehicle)
+{
+    vehicle->setFlightMode("Auto: Pause");
+}

src/FirmwarePlugin/PX4/PX4FirmwarePlugin.h

@@ -43,10 +43,10 @@ public:
 
     bool        isCapable                       (FirmwareCapabilities capabilities) final;
     QStringList flightModes                     (void) final;
-    QString     flightMode                      (uint8_t base_mode, uint32_t custom_mode) 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;
+    void        pauseVehicle                    (Vehicle* vehicle) final;
     int         manualControlReservedButtonCount(void) final;
-    void        adjustMavlinkMessage            (Vehicle* vehicle, mavlink_message_t* message) final;
     void        initializeVehicle               (Vehicle* vehicle) final;
     bool        sendHomePositionToVehicle       (void) final;
     void        addMetaDataToFact               (QObject* parameterMetaData, Fact* fact, MAV_TYPE vehicleType) final;

src/FlightDisplay/FlightDisplayView.qml

@@ -47,22 +47,8 @@ QGCView {
     QGCPalette { id: qgcPal; colorGroupEnabled: enabled }
 
     property real availableHeight: parent.height
-
-    readonly property bool isBackgroundDark: _mainIsMap ? (_flightMap ? _flightMap.isSatelliteMap : true) : true
-
     property var _activeVehicle:    multiVehicleManager.activeVehicle
 
-    readonly property real _defaultRoll:                0
-    readonly property real _defaultPitch:               0
-    readonly property real _defaultHeading:             0
-    readonly property real _defaultAltitudeAMSL:        0
-    readonly property real _defaultGroundSpeed:         0
-    readonly property real _defaultAirSpeed:            0
-
-    readonly property string _mapName:                  "FlightDisplayView"
-    readonly property string _showMapBackgroundKey:     "/showMapBackground"
-    readonly property string _mainIsMapKey:             "MainFlyWindowIsMap"
-    readonly property string _PIPVisibleKey:            "IsPIPVisible"
 
     property bool _mainIsMap:           QGroundControl.loadBoolGlobalSetting(_mainIsMapKey,  true)
     property bool _isPipVisible:        QGroundControl.loadBoolGlobalSetting(_PIPVisibleKey, true)
@@ -82,6 +68,18 @@ QGCView {
 
     property real pipSize:              mainWindow.width * 0.2
 
+    readonly property bool      isBackgroundDark:       _mainIsMap ? (_flightMap ? _flightMap.isSatelliteMap : true) : true
+    readonly property real      _defaultRoll:           0
+    readonly property real      _defaultPitch:          0
+    readonly property real      _defaultHeading:        0
+    readonly property real      _defaultAltitudeAMSL:   0
+    readonly property real      _defaultGroundSpeed:    0
+    readonly property real      _defaultAirSpeed:       0
+    readonly property string    _mapName:               "FlightDisplayView"
+    readonly property string    _showMapBackgroundKey:  "/showMapBackground"
+    readonly property string    _mainIsMapKey:          "MainFlyWindowIsMap"
+    readonly property string    _PIPVisibleKey:         "IsPIPVisible"
+
     FlightDisplayViewController { id: _controller }
 
     function setStates() {
@@ -174,6 +172,7 @@ QGCView {
             FlightDisplayViewMap {
                 id:             _flightMap
                 anchors.fill:   parent
+                flightWidgets:  widgetsLoader.item
             }
         }
 

src/FlightDisplay/FlightDisplayViewMap.qml

@@ -40,9 +40,14 @@ FlightMap {
     anchors.fill:   parent
     mapName:        _mapName
 
+    property alias  missionController: _missionController
+    property var    flightWidgets
+
     property bool   _followVehicle:                 true
-    property bool   _activeVehicleCoordinateValid:  multiVehicleManager.activeVehicle ? multiVehicleManager.activeVehicle.coordinateValid : false
-    property var    activeVehicleCoordinate:        multiVehicleManager.activeVehicle ? multiVehicleManager.activeVehicle.coordinate : QtPositioning.coordinate()
+    property var    _activeVehicle:                 QGroundControl.multiVehicleManager.activeVehicle
+    property bool   _activeVehicleCoordinateValid:  _activeVehicle ? _activeVehicle.coordinateValid : false
+    property var    activeVehicleCoordinate:        _activeVehicle ? _activeVehicle.coordinate : QtPositioning.coordinate()
+    property var    _gotoHereCoordinate:            QtPositioning.coordinate()
 
     Component.onCompleted: {
         QGroundControl.flightMapPosition = center
@@ -58,6 +63,8 @@ FlightMap {
         }
     }
 
+    QGCPalette { id: qgcPal; colorGroupEnabled: true }
+
     MissionController {
         id: _missionController
         Component.onCompleted: start(false /* editMode */)
@@ -68,14 +75,14 @@ FlightMap {
         model: _mainIsMap ? multiVehicleManager.activeVehicle ? multiVehicleManager.activeVehicle.trajectoryPoints : 0 : 0
         delegate:
             MapPolyline {
-                line.width: 3
-                line.color: "red"
-                z:          QGroundControl.zOrderMapItems - 1
-                path: [
-                    { latitude: object.coordinate1.latitude, longitude: object.coordinate1.longitude },
-                    { latitude: object.coordinate2.latitude, longitude: object.coordinate2.longitude },
-                ]
-            }
+            line.width: 3
+            line.color: "red"
+            z:          QGroundControl.zOrderMapItems - 1
+            path: [
+                { latitude: object.coordinate1.latitude, longitude: object.coordinate1.longitude },
+                { latitude: object.coordinate2.latitude, longitude: object.coordinate2.longitude },
+            ]
+        }
     }
 
     // Add the vehicles to the map
@@ -83,12 +90,12 @@ FlightMap {
         model: multiVehicleManager.vehicles
         delegate:
             VehicleMapItem {
-                    vehicle:        object
-                    coordinate:     object.coordinate
-                    isSatellite:    flightMap.isSatelliteMap
-                    size:           _mainIsMap ? ScreenTools.defaultFontPixelHeight * 5 : ScreenTools.defaultFontPixelHeight * 2
-                    z:              QGroundControl.zOrderMapItems
-            }
+            vehicle:        object
+            coordinate:     object.coordinate
+            isSatellite:    flightMap.isSatelliteMap
+            size:           _mainIsMap ? ScreenTools.defaultFontPixelHeight * 5 : ScreenTools.defaultFontPixelHeight * 2
+            z:              QGroundControl.zOrderMapItems
+        }
     }
 
     // Add the mission items to the map
@@ -101,8 +108,29 @@ FlightMap {
         model: _mainIsMap ? _missionController.waypointLines : 0
     }
 
-    // Used to make pinch zoom work
+    // GoTo here waypoint
+    MapQuickItem {
+        coordinate:     _gotoHereCoordinate
+        visible:        _vehicle.guidedMode && _gotoHereCoordinate.isValid
+        z:              QGroundControl.zOrderMapItems
+        anchorPoint.x:  sourceItem.width  / 2
+        anchorPoint.y:  sourceItem.height / 2
+
+        sourceItem: MissionItemIndexLabel {
+            isCurrentItem:  true
+            label:          "G"
+        }
+    }
+
+    // Handle guided mode clicks
     MouseArea {
         anchors.fill: parent
+
+        onClicked: {
+            if (_activeVehicle && _activeVehicle.guidedMode) {
+                _gotoHereCoordinate = flightMap.toCoordinate(Qt.point(mouse.x, mouse.y))
+                flightWidgets.guidedModeBar.confirmAction(flightWidgets.guidedModeBar.confirmGoTo)
+            }
+        }
     }
 }

src/MissionManager/MissionManager.cc

@@ -107,6 +107,41 @@ void MissionManager::writeMissionItems(const QList<MissionItem*>& missionItems)
     emit inProgressChanged(true);
 }
 
+void MissionManager::writeArduPilotGuidedMissionItem(const QGeoCoordinate& gotoCoord, bool altChangeOnly)
+{
+    if (inProgress()) {
+        qCDebug(MissionManagerLog) << "writeArduPilotGuidedMissionItem called while transaction in progress";
+        return;
+    }
+
+    _writeTransactionInProgress = true;
+
+    mavlink_message_t       messageOut;
+    mavlink_mission_item_t  missionItem;
+
+    missionItem.target_system =     _vehicle->id();
+    missionItem.target_component =  0;
+    missionItem.seq =               0;
+    missionItem.command =           MAV_CMD_NAV_WAYPOINT;
+    missionItem.param1 =            0;
+    missionItem.param2 =            0;
+    missionItem.param3 =            0;
+    missionItem.param4 =            0;
+    missionItem.x =                 gotoCoord.latitude();
+    missionItem.y =                 gotoCoord.longitude();
+    missionItem.z =                 gotoCoord.altitude();
+    missionItem.frame =             MAV_FRAME_GLOBAL_RELATIVE_ALT;
+    missionItem.current =           altChangeOnly ? 3 : 2;
+    missionItem.autocontinue =      true;
+
+    mavlink_msg_mission_item_encode(qgcApp()->toolbox()->mavlinkProtocol()->getSystemId(), qgcApp()->toolbox()->mavlinkProtocol()->getComponentId(), &messageOut, &missionItem);
+
+    _dedicatedLink = _vehicle->priorityLink();
+    _vehicle->sendMessageOnLink(_dedicatedLink, messageOut);
+    _startAckTimeout(AckGuidedItem);
+    emit inProgressChanged(true);
+}
+
 void MissionManager::requestMissionItems(void)
 {
     qCDebug(MissionManagerLog) << "requestMissionItems read sequence";
@@ -211,8 +246,6 @@ void MissionManager::_handleMissionCount(const mavlink_message_t& message)
         }
         _requestNextMissionItem();
     }
-
-    
 }
 
 void MissionManager::_requestNextMissionItem(void)
@@ -394,6 +427,16 @@ void MissionManager::_handleMissionAck(const mavlink_message_t& message)
                 _finishTransaction(false);
             }
             break;
+        case AckGuidedItem:
+            // MISSION_REQUEST is expected, or MISSION_ACK to end sequence
+            if (missionAck.type == MAV_MISSION_ACCEPTED) {
+                qCDebug(MissionManagerLog) << "_handleMissionAck guide mode item accepted";
+                _finishTransaction(true);
+            } else {
+                _sendError(VehicleError, QString("Vehicle returned error: %1. Vehicle did not accept guided item.").arg(_missionResultToString((MAV_MISSION_RESULT)missionAck.type)));
+                _finishTransaction(false);
+            }
+            break;
     }
 }
 
@@ -437,14 +480,16 @@ void MissionManager::_sendError(ErrorCode_t errorCode, const QString& errorMsg)
 QString MissionManager::_ackTypeToString(AckType_t ackType)
 {
     switch (ackType) {
-        case AckNone:   // State machine is idle
+        case AckNone:
             return QString("No Ack");
-        case AckMissionCount:   // MISSION_COUNT message expected
+        case AckMissionCount:
             return QString("MISSION_COUNT");
-        case AckMissionItem:  ///< MISSION_ITEM expected
+        case AckMissionItem:
             return QString("MISSION_ITEM");
-        case AckMissionRequest: ///< MISSION_REQUEST is expected, or MISSION_ACK to end sequence
+        case AckMissionRequest:
             return QString("MISSION_REQUEST");
+        case AckGuidedItem:
+            return QString("Guided Mode Item");
         default:
             qWarning(MissionManagerLog) << "Fell off end of switch statement";
             return QString("QGC Internal Error");

src/MissionManager/MissionManager.h

@@ -58,6 +58,11 @@ public:
     ///     @param missionItems Items to send to vehicle
     void writeMissionItems(const QList<MissionItem*>& missionItems);
     
+    /// Writes the specified set mission items to the vehicle as an ArduPilot guided mode mission item.
+    ///     @param gotoCoord Coordinate to move to
+    ///     @param altChangeOnly true: only altitude change, false: lat/lon/alt change
+    void writeArduPilotGuidedMissionItem(const QGeoCoordinate& gotoCoord, bool altChangeOnly);
+
     /// Error codes returned in error signal
     typedef enum {
         InternalError,
@@ -90,6 +95,7 @@ private:
         AckMissionCount,    ///< MISSION_COUNT message expected
         AckMissionItem,     ///< MISSION_ITEM expected
         AckMissionRequest,  ///< MISSION_REQUEST is expected, or MISSION_ACK to end sequence
+        AckGuidedItem,      ///< MISSION_ACK expected in reponse to ArduPilot guided mode single item send
     } AckType_t;
     
     void _startAckTimeout(AckType_t ack);

src/Vehicle/Vehicle.cc

@@ -99,6 +99,7 @@ Vehicle::Vehicle(LinkInterface*             link,
     , _rcRSSI(0)
     , _rcRSSIstore(100.0)
     , _autoDisconnect(false)
+    , _flying(false)
     , _connectionLost(false)
     , _connectionLostEnabled(true)
     , _missionManager(NULL)
@@ -140,7 +141,7 @@ Vehicle::Vehicle(LinkInterface*             link,
 
     connect(this, &Vehicle::_sendMessageOnThread,       this, &Vehicle::_sendMessage, Qt::QueuedConnection);
     connect(this, &Vehicle::_sendMessageOnLinkOnThread, this, &Vehicle::_sendMessageOnLink, Qt::QueuedConnection);
-    connect(this, &Vehicle::flightModeChanged,          this, &Vehicle::_announceflightModeChanged);
+    connect(this, &Vehicle::flightModeChanged,          this, &Vehicle::_handleFlightModeChanged);
     connect(this, &Vehicle::armedChanged,               this, &Vehicle::_announceArmedChanged);
 
     _uas = new UAS(_mavlink, this, _firmwarePluginManager);
@@ -376,7 +377,7 @@ void Vehicle::_mavlinkMessageReceived(LinkInterface* link, mavlink_message_t mes
     }
 
     // Give the plugin a change to adjust the message contents
-    _firmwarePlugin->adjustMavlinkMessage(this, &message);
+    _firmwarePlugin->adjustIncomingMavlinkMessage(this, &message);
 
     switch (message.msgid) {
     case MAVLINK_MSG_ID_HOME_POSITION:
@@ -412,6 +413,9 @@ void Vehicle::_mavlinkMessageReceived(LinkInterface* link, mavlink_message_t mes
     case MAVLINK_MSG_ID_VIBRATION:
         _handleVibration(message);
         break;
+    case MAVLINK_MSG_ID_EXTENDED_SYS_STATE:
+        _handleExtendedSysState(message);
+        break;
 
     // Following are ArduPilot dialect messages
 
@@ -425,6 +429,23 @@ void Vehicle::_mavlinkMessageReceived(LinkInterface* link, mavlink_message_t mes
     _uas->receiveMessage(message);
 }
 
+void Vehicle::_handleExtendedSysState(mavlink_message_t& message)
+{
+    mavlink_extended_sys_state_t extendedState;
+    mavlink_msg_extended_sys_state_decode(&message, &extendedState);
+
+    switch (extendedState.landed_state) {
+        case MAV_LANDED_STATE_UNDEFINED:
+        break;
+    case MAV_LANDED_STATE_ON_GROUND:
+        setFlying(false);
+        break;
+    case MAV_LANDED_STATE_IN_AIR:
+        setFlying(true);
+        return;
+    }
+}
+
 void Vehicle::_handleVibration(mavlink_message_t& message)
 {
     mavlink_vibration_t vibration;
@@ -703,7 +724,7 @@ void Vehicle::_sendMessageOnLink(LinkInterface* link, mavlink_message_t message)
     }
 
     // Give the plugin a chance to adjust
-    _firmwarePlugin->adjustMavlinkMessage(this, &message);
+    _firmwarePlugin->adjustOutgoingMavlinkMessage(this, &message);
 
     static const uint8_t messageKeys[256] = MAVLINK_MESSAGE_CRCS;
     mavlink_finalize_message_chan(&message, _mavlink->getSystemId(), _mavlink->getComponentId(), link->getMavlinkChannel(), message.len, messageKeys[message.msgid]);
@@ -1091,7 +1112,7 @@ QStringList Vehicle::flightModes(void)
     return _firmwarePlugin->flightModes();
 }
 
-QString Vehicle::flightMode(void)
+QString Vehicle::flightMode(void) const
 {
     return _firmwarePlugin->flightMode(_base_mode, _custom_mode);
 }
@@ -1302,7 +1323,7 @@ void Vehicle::_connectionActive(void)
     if (_connectionLost) {
         _connectionLost = false;
         emit connectionLostChanged(false);
-        _say(QString("% 1 communication regained").arg(_vehicleIdSpeech()));
+        _say(QString("%1 communication regained").arg(_vehicleIdSpeech()));
     }
 }
 
@@ -1349,9 +1370,10 @@ QString Vehicle::_vehicleIdSpeech(void)
     }
 }
 
-void Vehicle::_announceflightModeChanged(const QString& flightMode)
+void Vehicle::_handleFlightModeChanged(const QString& flightMode)
 {
     _say(QString("%1 %2 flight mode").arg(_vehicleIdSpeech()).arg(flightMode));
+    emit guidedModeChanged(_firmwarePlugin->isGuidedMode(this));
 }
 
 void Vehicle::_announceArmedChanged(bool armed)
@@ -1364,6 +1386,111 @@ void Vehicle::clearTrajectoryPoints(void)
     _mapTrajectoryList.clearAndDeleteContents();
 }
 
+void Vehicle::setFlying(bool flying)
+{
+    if (armed() && _flying != flying) {
+        _flying = flying;
+        emit flyingChanged(flying);
+    }
+}
+
+bool Vehicle::guidedModeSupported(void) const
+{
+    return _firmwarePlugin->isCapable(FirmwarePlugin::GuidedModeCapability);
+}
+
+bool Vehicle::pauseVehicleSupported(void) const
+{
+    return _firmwarePlugin->isCapable(FirmwarePlugin::PauseVehicleCapability);
+}
+
+void Vehicle::guidedModeRTL(void)
+{
+    if (!guidedModeSupported()) {
+        qgcApp()->showMessage(QStringLiteral("Guided mode not supported by vehicle."));
+        return;
+    }
+    _firmwarePlugin->guidedModeRTL(this);
+}
+
+void Vehicle::guidedModeLand(void)
+{
+    if (!guidedModeSupported()) {
+        qgcApp()->showMessage(QStringLiteral("Guided mode not supported by vehicle."));
+        return;
+    }
+    _firmwarePlugin->guidedModeLand(this);
+}
+
+void Vehicle::guidedModeTakeoff(double altitudeRel)
+{
+    if (!guidedModeSupported()) {
+        qgcApp()->showMessage(QStringLiteral("Guided mode not supported by vehicle."));
+        return;
+    }
+    setGuidedMode(true);
+    _firmwarePlugin->guidedModeTakeoff(this, altitudeRel);
+}
+
+void Vehicle::guidedModeGotoLocation(const QGeoCoordinate& gotoCoord)
+{
+    if (!guidedModeSupported()) {
+        qgcApp()->showMessage(QStringLiteral("Guided mode not supported by vehicle."));
+        return;
+    }
+    _firmwarePlugin->guidedModeGotoLocation(this, gotoCoord);
+}
+
+void Vehicle::guidedModeChangeAltitude(double altitudeRel)
+{
+    if (!guidedModeSupported()) {
+        qgcApp()->showMessage(QStringLiteral("Guided mode not supported by vehicle."));
+        return;
+    }
+    _firmwarePlugin->guidedModeChangeAltitude(this, altitudeRel);
+}
+
+void Vehicle::pauseVehicle(void)
+{
+    if (!pauseVehicleSupported()) {
+        qgcApp()->showMessage(QStringLiteral("Pause not supported by vehicle."));
+        return;
+    }
+    _firmwarePlugin->pauseVehicle(this);
+}
+
+bool Vehicle::guidedMode(void) const
+{
+    return _firmwarePlugin->isGuidedMode(this);
+}
+
+void Vehicle::setGuidedMode(bool guidedMode)
+{
+    return _firmwarePlugin->setGuidedMode(this, guidedMode);
+}
+
+void Vehicle::emergencyStop(void)
+{
+    mavlink_message_t msg;
+    mavlink_command_long_t cmd;
+
+    cmd.command = (uint16_t)MAV_CMD_COMPONENT_ARM_DISARM;
+    cmd.confirmation = 0;
+    cmd.param1 = 0.0f;
+    cmd.param2 = 21196.0f;  // Magic number for emergency stop
+    cmd.param3 = 0.0f;
+    cmd.param4 = 0.0f;
+    cmd.param5 = 0.0f;
+    cmd.param6 = 0.0f;
+    cmd.param7 = 0.0f;
+    cmd.target_system = id();
+    cmd.target_component = 0;
+
+    mavlink_msg_command_long_encode(_mavlink->getSystemId(), _mavlink->getComponentId(), &msg, &cmd);
+
+    sendMessage(msg);
+}
+
 
 const char* VehicleGPSFactGroup::_hdopFactName =                "hdop";
 const char* VehicleGPSFactGroup::_vdopFactName =                "vdop";

src/Vehicle/Vehicle.h

@@ -287,6 +287,18 @@ public:
     Q_PROPERTY(bool                 multiRotor              READ multiRotor                             CONSTANT)
     Q_PROPERTY(bool                 autoDisconnect          MEMBER _autoDisconnect                      NOTIFY autoDisconnectChanged)
 
+    /// true: Vehicle is flying, false: Vehicle is on ground
+    Q_PROPERTY(bool flying      READ flying     WRITE setFlying     NOTIFY flyingChanged)
+
+    /// true: Vehicle is in Guided mode and can respond to guided commands, false: vehicle cannot respond to direct control commands
+    Q_PROPERTY(bool guidedMode  READ guidedMode WRITE setGuidedMode NOTIFY guidedModeChanged)
+
+    /// true: Guided mode commands are supported by this vehicle
+    Q_PROPERTY(bool guidedModeSupported READ guidedModeSupported CONSTANT)
+
+    /// true: pauseVehicle command is supported
+    Q_PROPERTY(bool pauseVehicleSupported READ pauseVehicleSupported CONSTANT)
+
     // FactGroup object model properties
 
     Q_PROPERTY(Fact* roll               READ roll               CONSTANT)
@@ -322,6 +334,31 @@ public:
 
     Q_INVOKABLE void clearTrajectoryPoints(void);
 
+    /// Command vehicle to return to launch
+    Q_INVOKABLE void guidedModeRTL(void);
+
+    /// Command vehicle to land at current location
+    Q_INVOKABLE void guidedModeLand(void);
+
+    /// Command vehicle to takeoff from current location
+    Q_INVOKABLE void guidedModeTakeoff(double altitudeRel);
+
+    /// Command vehicle to move to specified location (altitude is included and relative)
+    Q_INVOKABLE void guidedModeGotoLocation(const QGeoCoordinate& gotoCoord);
+
+    /// Command vehicle to change to the specified relatice altitude
+    Q_INVOKABLE void guidedModeChangeAltitude(double altitudeRel);
+
+    /// Command vehicle to pause at current location. If vehicle supports guide mode, vehicle will be left
+    /// in guided mode after pause.
+    Q_INVOKABLE void pauseVehicle(void);
+
+    /// Command vehicle to kill all motors no matter what state
+    Q_INVOKABLE void emergencyStop(void);
+
+    bool guidedModeSupported(void) const;
+    bool pauseVehicleSupported(void) const;
+
     // Property accessors
 
     QGeoCoordinate coordinate(void) { return _coordinate; }
@@ -390,7 +427,7 @@ public:
 
     bool flightModeSetAvailable(void);
     QStringList flightModes(void);
-    QString flightMode(void);
+    QString flightMode(void) const;
     void setFlightMode(const QString& flightMode);
 
     bool hilMode(void);
@@ -399,6 +436,9 @@ public:
     bool fixedWing(void) const;
     bool multiRotor(void) const;
 
+    void setFlying(bool flying);
+    void setGuidedMode(bool guidedMode);
+
     QmlObjectListModel* trajectoryPoints(void) { return &_mapTrajectoryList; }
 
     int  flowImageIndex() { return _flowImageIndex; }
@@ -432,14 +472,16 @@ public:
     bool            mavPresent          () { return _mav != NULL; }
     QString         currentState        () { return _currentState; }
     int             rcRSSI              () { return _rcRSSI; }
-    bool            px4Firmware         () { return _firmwareType == MAV_AUTOPILOT_PX4; }
-    bool            apmFirmware         () { return _firmwareType == MAV_AUTOPILOT_ARDUPILOTMEGA; }
-    bool            genericFirmware     () { return !px4Firmware() && !apmFirmware(); }
+    bool            px4Firmware         () const { return _firmwareType == MAV_AUTOPILOT_PX4; }
+    bool            apmFirmware         () const { return _firmwareType == MAV_AUTOPILOT_ARDUPILOTMEGA; }
+    bool            genericFirmware     () const { return !px4Firmware() && !apmFirmware(); }
     bool            connectionLost      () const { return _connectionLost; }
     bool            connectionLostEnabled() const { return _connectionLostEnabled; }
     uint            messagesReceived    () { return _messagesReceived; }
     uint            messagesSent        () { return _messagesSent; }
     uint            messagesLost        () { return _messagesLost; }
+    bool            flying              () const { return _flying; }
+    bool            guidedMode          () const;
 
     Fact* roll              (void) { return &_rollFact; }
     Fact* heading           (void) { return &_headingFact; }
@@ -484,6 +526,8 @@ signals:
     void connectionLostChanged(bool connectionLost);
     void connectionLostEnabledChanged(bool connectionLostEnabled);
     void autoDisconnectChanged(bool autoDisconnectChanged);
+    void flyingChanged(bool flying);
+    void guidedModeChanged(bool guidedMode);
 
     void messagesReceivedChanged    ();
     void messagesSentChanged        ();
@@ -527,7 +571,7 @@ private slots:
     void _addNewMapTrajectoryPoint(void);
     void _parametersReady(bool parametersReady);
     void _remoteControlRSSIChanged(uint8_t rssi);
-    void _announceflightModeChanged(const QString& flightMode);
+    void _handleFlightModeChanged(const QString& flightMode);
     void _announceArmedChanged(bool armed);
 
     void _handleTextMessage                 (int newCount);
@@ -560,6 +604,7 @@ private:
     void _handleSysStatus(mavlink_message_t& message);
     void _handleWind(mavlink_message_t& message);
     void _handleVibration(mavlink_message_t& message);
+    void _handleExtendedSysState(mavlink_message_t& message);
     void _missionManagerError(int errorCode, const QString& errorMsg);
     void _mapTrajectoryStart(void);
     void _mapTrajectoryStop(void);
@@ -610,6 +655,7 @@ private:
     int             _rcRSSI;
     double          _rcRSSIstore;
     bool            _autoDisconnect;    ///< true: Automatically disconnect vehicle when last connection goes away or lost heartbeat
+    bool            _flying;
 
     // Lost connection handling
     bool                _connectionLost;

src/ui/toolbar/MainToolBarIndicators.qml

@@ -435,53 +435,6 @@ Row {
         }
     }
 
-    //-------------------------------------------------------------------------
-    //-- Arm/Disarm
-
-    Item {
-        width:  armCol.width * 1.1
-        height: mainWindow.tbCellHeight
-        anchors.verticalCenter: parent.verticalCenter
-        Row {
-            id:                 armCol
-            spacing:            tbSpacing * 0.5
-            anchors.verticalCenter: parent.verticalCenter
-            Image {
-                width:          mainWindow.tbCellHeight * 0.5
-                height:         mainWindow.tbCellHeight * 0.5
-                fillMode:       Image.PreserveAspectFit
-                mipmap:         true
-                smooth:         true
-                source:         activeVehicle ? (activeVehicle.armed ? "/qmlimages/Disarmed.svg" : "/qmlimages/Armed.svg") : "/qmlimages/Disarmed.svg"
-                anchors.verticalCenter: parent.verticalCenter
-            }
-            QGCLabel {
-                text:           activeVehicle ? (activeVehicle.armed ? "Armed" : "Disarmed") : ""
-                font.pixelSize: tbFontLarge
-                color:          colorWhite
-                anchors.verticalCenter: parent.verticalCenter
-            }
-        }
-        MouseArea {
-            anchors.fill:   parent
-            onClicked: {
-                armDialog.visible = true
-            }
-        }
-        MessageDialog {
-            id:         armDialog
-            visible:    false
-            icon:       StandardIcon.Warning
-            standardButtons: StandardButton.Yes | StandardButton.No
-            title:      activeVehicle ? (activeVehicle.armed ? "Disarming Vehicle" : "Arming Vehicle") : ""
-            text:       activeVehicle ? (activeVehicle.armed ? "Do you want to disarm? This will cut power to all motors." : "Do you want to arm? This will enable all motors.") : ""
-            onYes: {
-                activeVehicle.armed = !activeVehicle.armed
-                armDialog.visible = false
-            }
-        }
-    }
-
 /*
     property var colorOrangeText: (qgcPal.globalTheme === QGCPalette.Light) ? "#b75711" : "#ea8225"
     property var colorRedText:    (qgcPal.globalTheme === QGCPalette.Light) ? "#ee1112" : "#ef2526"