diff --git a/src/FirmwarePlugin/APM/APMFirmwarePlugin.cc b/src/FirmwarePlugin/APM/APMFirmwarePlugin.cc
index 812993b0d6cc8a5bd9d913654f2fcd8c2420a668..a979ac78957cf6b96ed38734d4d5c3576bf24bd7 100644
--- a/src/FirmwarePlugin/APM/APMFirmwarePlugin.cc
+++ b/src/FirmwarePlugin/APM/APMFirmwarePlugin.cc
@@ -1,5 +1,5 @@
 /*=====================================================================
- 
+
  QGroundControl Open Source Ground Control Station
  
  (c) 2009 - 2015 QGROUNDCONTROL PROJECT <http://www.qgroundcontrol.org>
@@ -144,12 +144,12 @@ QString APMCustomMode::modeString() const
 
 APMFirmwarePlugin::APMFirmwarePlugin(void)
 {
-     _textSeverityAdjustmentNeeded = false;
+    _textSeverityAdjustmentNeeded = false;
 }
 
 bool APMFirmwarePlugin::isCapable(FirmwareCapabilities capabilities)
 {
-    return (capabilities & SetFlightModeCapability) == capabilities;
+    return (capabilities & (SetFlightModeCapability | PauseVehicleCapability)) == capabilities;
 }
 
 QList<VehicleComponent*> APMFirmwarePlugin::componentsForVehicle(AutoPilotPlugin* vehicle)
@@ -170,7 +170,7 @@ QStringList APMFirmwarePlugin::flightModes(void)
     return flightModesList;
 }
 
-QString APMFirmwarePlugin::flightMode(uint8_t base_mode, uint32_t custom_mode)
+QString APMFirmwarePlugin::flightMode(uint8_t base_mode, uint32_t custom_mode) const
 {
     QString flightMode = "Unknown";
 
@@ -214,166 +214,220 @@ int APMFirmwarePlugin::manualControlReservedButtonCount(void)
     return -1;
 }
 
-void APMFirmwarePlugin::adjustMavlinkMessage(Vehicle* vehicle, mavlink_message_t* message)
+void APMFirmwarePlugin::_handleParamValue(Vehicle* vehicle, mavlink_message_t* message)
 {
-    //-- Don't process messages to/from UDP Bridge. It doesn't suffer from these issues
-    if (message->compid == MAV_COMP_ID_UDP_BRIDGE) {
-        return;
+    Q_UNUSED(vehicle);
+
+    mavlink_param_value_t paramValue;
+    mavlink_param_union_t paramUnion;
+
+    // APM stack passes all parameter values in mavlink_param_union_t.param_float no matter what
+    // type they are. Fix that up to correct usage.
+
+    mavlink_msg_param_value_decode(message, &paramValue);
+
+    switch (paramValue.param_type) {
+    case MAV_PARAM_TYPE_UINT8:
+        paramUnion.param_uint8 = (uint8_t)paramValue.param_value;
+        break;
+    case MAV_PARAM_TYPE_INT8:
+        paramUnion.param_int8 = (int8_t)paramValue.param_value;
+        break;
+    case MAV_PARAM_TYPE_UINT16:
+        paramUnion.param_uint16 = (uint16_t)paramValue.param_value;
+        break;
+    case MAV_PARAM_TYPE_INT16:
+        paramUnion.param_int16 = (int16_t)paramValue.param_value;
+        break;
+    case MAV_PARAM_TYPE_UINT32:
+        paramUnion.param_uint32 = (uint32_t)paramValue.param_value;
+        break;
+    case MAV_PARAM_TYPE_INT32:
+        paramUnion.param_int32 = (int32_t)paramValue.param_value;
+        break;
+    case MAV_PARAM_TYPE_REAL32:
+        paramUnion.param_float = paramValue.param_value;
+        break;
+    default:
+        qCCritical(APMFirmwarePluginLog) << "Invalid/Unsupported data type used in parameter:" << paramValue.param_type;
     }
 
-    if (message->msgid == MAVLINK_MSG_ID_PARAM_VALUE) {
-        mavlink_param_value_t paramValue;
-        mavlink_param_union_t paramUnion;
-        
-        // APM stack passes all parameter values in mavlink_param_union_t.param_float no matter what
-        // type they are. Fix that up to correct usage.
-        
-        mavlink_msg_param_value_decode(message, &paramValue);
-        
-        switch (paramValue.param_type) {
-            case MAV_PARAM_TYPE_UINT8:
-                paramUnion.param_uint8 = (uint8_t)paramValue.param_value;
-                break;
-            case MAV_PARAM_TYPE_INT8:
-                paramUnion.param_int8 = (int8_t)paramValue.param_value;
-                break;
-            case MAV_PARAM_TYPE_UINT16:
-                paramUnion.param_uint16 = (uint16_t)paramValue.param_value;
-                break;
-            case MAV_PARAM_TYPE_INT16:
-                paramUnion.param_int16 = (int16_t)paramValue.param_value;
-                break;
-            case MAV_PARAM_TYPE_UINT32:
-                paramUnion.param_uint32 = (uint32_t)paramValue.param_value;
-                break;
-            case MAV_PARAM_TYPE_INT32:
-                paramUnion.param_int32 = (int32_t)paramValue.param_value;
-                break;
-            case MAV_PARAM_TYPE_REAL32:
-                paramUnion.param_float = paramValue.param_value;
-                break;
-            default:
-                qCCritical(APMFirmwarePluginLog) << "Invalid/Unsupported data type used in parameter:" << paramValue.param_type;
-        }
-        
-        paramValue.param_value = paramUnion.param_float;
-        
-        mavlink_msg_param_value_encode(message->sysid, message->compid, message, &paramValue);
-        
-    } else if (message->msgid == MAVLINK_MSG_ID_PARAM_SET) {
-        mavlink_param_set_t     paramSet;
-        mavlink_param_union_t   paramUnion;
-        
-        // APM stack passes all parameter values in mavlink_param_union_t.param_float no matter what
-        // type they are. Fix it back to the wrong way on the way out.
-        
-        mavlink_msg_param_set_decode(message, &paramSet);
-        
-        paramUnion.param_float = paramSet.param_value;
-
-        switch (paramSet.param_type) {
-            case MAV_PARAM_TYPE_UINT8:
-                paramSet.param_value = paramUnion.param_uint8;
-                break;
-            case MAV_PARAM_TYPE_INT8:
-                paramSet.param_value = paramUnion.param_int8;
-                break;
-            case MAV_PARAM_TYPE_UINT16:
-                paramSet.param_value = paramUnion.param_uint16;
-                break;
-            case MAV_PARAM_TYPE_INT16:
-                paramSet.param_value = paramUnion.param_int16;
-                break;
-            case MAV_PARAM_TYPE_UINT32:
-                paramSet.param_value = paramUnion.param_uint32;
-                break;
-            case MAV_PARAM_TYPE_INT32:
-                paramSet.param_value = paramUnion.param_int32;
-                break;
-            case MAV_PARAM_TYPE_REAL32:
-                // Already in param_float
-                break;
-            default:
-                qCCritical(APMFirmwarePluginLog) << "Invalid/Unsupported data type used in parameter:" << paramSet.param_type;
-        }
-        
-        mavlink_msg_param_set_encode(message->sysid, message->compid, message, &paramSet);
+    paramValue.param_value = paramUnion.param_float;
+
+    mavlink_msg_param_value_encode(message->sysid, message->compid, message, &paramValue);
+}
+
+void APMFirmwarePlugin::_handleParamSet(Vehicle* vehicle, mavlink_message_t* message)
+{
+    Q_UNUSED(vehicle);
+
+    mavlink_param_set_t     paramSet;
+    mavlink_param_union_t   paramUnion;
+
+    // APM stack passes all parameter values in mavlink_param_union_t.param_float no matter what
+    // type they are. Fix it back to the wrong way on the way out.
+
+    mavlink_msg_param_set_decode(message, &paramSet);
+
+    paramUnion.param_float = paramSet.param_value;
+
+    switch (paramSet.param_type) {
+    case MAV_PARAM_TYPE_UINT8:
+        paramSet.param_value = paramUnion.param_uint8;
+        break;
+    case MAV_PARAM_TYPE_INT8:
+        paramSet.param_value = paramUnion.param_int8;
+        break;
+    case MAV_PARAM_TYPE_UINT16:
+        paramSet.param_value = paramUnion.param_uint16;
+        break;
+    case MAV_PARAM_TYPE_INT16:
+        paramSet.param_value = paramUnion.param_int16;
+        break;
+    case MAV_PARAM_TYPE_UINT32:
+        paramSet.param_value = paramUnion.param_uint32;
+        break;
+    case MAV_PARAM_TYPE_INT32:
+        paramSet.param_value = paramUnion.param_int32;
+        break;
+    case MAV_PARAM_TYPE_REAL32:
+        // Already in param_float
+        break;
+    default:
+        qCCritical(APMFirmwarePluginLog) << "Invalid/Unsupported data type used in parameter:" << paramSet.param_type;
     }
 
-    if (message->msgid == MAVLINK_MSG_ID_STATUSTEXT) {
-        QString messageText;
-
-        mavlink_statustext_t statusText;
-        mavlink_msg_statustext_decode(message, &statusText);
-
-        if (!_firmwareVersion.isValid() || statusText.severity < MAV_SEVERITY_NOTICE) {
-            messageText = _getMessageText(message);
-            qCDebug(APMFirmwarePluginLog) << messageText;
-
-            if (!_firmwareVersion.isValid()) {
-                // if don't know firmwareVersion yet, try and see if this message contains it
-                if (messageText.contains(APM_COPTER_REXP) || messageText.contains(APM_PLANE_REXP) || messageText.contains(APM_ROVER_REXP)) {
-                    // found version string
-                    _firmwareVersion = APMFirmwareVersion(messageText);
-                    _textSeverityAdjustmentNeeded = _isTextSeverityAdjustmentNeeded(_firmwareVersion);
-
-                    if (!_firmwareVersion.isBeta() && !_firmwareVersion.isDev()) {
-                        int supportedMajorNumber = -1;
-                        int supportedMinorNumber = -1;
-
-                        switch (vehicle->vehicleType()) {
-                        case MAV_TYPE_FIXED_WING:
-                            supportedMajorNumber = 3;
-                            supportedMinorNumber = 2;
-                            break;
-                        case MAV_TYPE_QUADROTOR:
-                        case MAV_TYPE_COAXIAL:
-                        case MAV_TYPE_HELICOPTER:
-                        case MAV_TYPE_SUBMARINE:
-                        case MAV_TYPE_HEXAROTOR:
-                        case MAV_TYPE_OCTOROTOR:
-                        case MAV_TYPE_TRICOPTER:
-                            supportedMajorNumber = 3;
-                            supportedMinorNumber = 2;
-                            break;
-                        default:
-                            break;
-                        }
+    mavlink_msg_param_set_encode(message->sysid, message->compid, message, &paramSet);
+}
+
+void APMFirmwarePlugin::_handleStatusText(Vehicle* vehicle, mavlink_message_t* message)
+{
+    QString messageText;
 
-                        if (supportedMajorNumber != -1) {
-                            if (_firmwareVersion.majorNumber() < supportedMajorNumber || _firmwareVersion.minorNumber() < supportedMinorNumber) {
-                                qgcApp()->showMessage(QString("QGroundControl fully supports Version %1.%2 and above. You are using a version prior to that. This combination is untested, you may run into unpredictable results.").arg(supportedMajorNumber).arg(supportedMinorNumber));
-                            }
+    mavlink_statustext_t statusText;
+    mavlink_msg_statustext_decode(message, &statusText);
+
+    if (!_firmwareVersion.isValid() || statusText.severity < MAV_SEVERITY_NOTICE) {
+        messageText = _getMessageText(message);
+        qCDebug(APMFirmwarePluginLog) << messageText;
+
+        if (!_firmwareVersion.isValid()) {
+            // if don't know firmwareVersion yet, try and see if this message contains it
+            if (messageText.contains(APM_COPTER_REXP) || messageText.contains(APM_PLANE_REXP) || messageText.contains(APM_ROVER_REXP)) {
+                // found version string
+                _firmwareVersion = APMFirmwareVersion(messageText);
+                _textSeverityAdjustmentNeeded = _isTextSeverityAdjustmentNeeded(_firmwareVersion);
+
+                if (!_firmwareVersion.isBeta() && !_firmwareVersion.isDev()) {
+                    int supportedMajorNumber = -1;
+                    int supportedMinorNumber = -1;
+
+                    switch (vehicle->vehicleType()) {
+                    case MAV_TYPE_FIXED_WING:
+                        supportedMajorNumber = 3;
+                        supportedMinorNumber = 2;
+                        break;
+                    case MAV_TYPE_QUADROTOR:
+                    case MAV_TYPE_COAXIAL:
+                    case MAV_TYPE_HELICOPTER:
+                    case MAV_TYPE_SUBMARINE:
+                    case MAV_TYPE_HEXAROTOR:
+                    case MAV_TYPE_OCTOROTOR:
+                    case MAV_TYPE_TRICOPTER:
+                        supportedMajorNumber = 3;
+                        supportedMinorNumber = 2;
+                        break;
+                    default:
+                        break;
+                    }
+
+                    if (supportedMajorNumber != -1) {
+                        if (_firmwareVersion.majorNumber() < supportedMajorNumber || _firmwareVersion.minorNumber() < supportedMinorNumber) {
+                            qgcApp()->showMessage(QString("QGroundControl fully supports Version %1.%2 and above. You are using a version prior to that. This combination is untested, you may run into unpredictable results.").arg(supportedMajorNumber).arg(supportedMinorNumber));
                         }
                     }
                 }
             }
+        }
 
-            // APM user facing calibration messages come through as high severity, we need to parse them out
-            // and lower the severity on them so that they don't pop in the users face.
+        // APM user facing calibration messages come through as high severity, we need to parse them out
+        // and lower the severity on them so that they don't pop in the users face.
 
-            if (messageText.contains("Place vehicle") || messageText.contains("Calibration successful")) {
-                _adjustCalibrationMessageSeverity(message);
-                return;
-            }
+        if (messageText.contains("Place vehicle") || messageText.contains("Calibration successful")) {
+            _adjustCalibrationMessageSeverity(message);
+            return;
         }
+    }
 
-        // adjust mesasge if needed
-        if (_textSeverityAdjustmentNeeded) {
-            _adjustSeverity(message);
-        }
+    // adjust mesasge if needed
+    if (_textSeverityAdjustmentNeeded) {
+        _adjustSeverity(message);
+    }
 
-        if (messageText.isEmpty()) {
-            messageText = _getMessageText(message);
-        }
+    if (messageText.isEmpty()) {
+        messageText = _getMessageText(message);
+    }
+
+    // The following messages are incorrectly labeled as warning message.
+    // Fixed in newer firmware (unreleased at this point), but still in older firmware.
+    if (messageText.contains(APM_COPTER_REXP) || messageText.contains(APM_PLANE_REXP) || messageText.contains(APM_ROVER_REXP) ||
+            messageText.contains(APM_PX4NUTTX_REXP) || messageText.contains(APM_FRAME_REXP) || messageText.contains(APM_SYSID_REXP)) {
+        _setInfoSeverity(message);
+    }
+}
+
+void APMFirmwarePlugin::_handleHeartbeat(Vehicle* vehicle, mavlink_message_t* message)
+{
+    bool flying = false;
 
-        // The following messages are incorrectly labeled as warning message.
-        // Fixed in newer firmware (unreleased at this point), but still in older firmware.
-        if (messageText.contains(APM_COPTER_REXP) || messageText.contains(APM_PLANE_REXP) || messageText.contains(APM_ROVER_REXP) ||
-                messageText.contains(APM_PX4NUTTX_REXP) || messageText.contains(APM_FRAME_REXP) || messageText.contains(APM_SYSID_REXP)) {
-            _setInfoSeverity(message);
+    // We pull Vehicle::flying state from HEARTBEAT on ArduPilot. This is a firmware specific test.
+
+    if (vehicle->armed()) {
+        mavlink_heartbeat_t heartbeat;
+        mavlink_msg_heartbeat_decode(message, &heartbeat);
+
+        flying = heartbeat.system_status == MAV_STATE_ACTIVE;
+        if (!flying && vehicle->flying()) {
+            // If we were previously flying, and we go into critical or emergency assume we are still flying
+            flying = heartbeat.system_status == MAV_STATE_CRITICAL || heartbeat.system_status == MAV_STATE_EMERGENCY;
         }
     }
+
+    vehicle->setFlying(flying);
+}
+
+void APMFirmwarePlugin::adjustIncomingMavlinkMessage(Vehicle* vehicle, mavlink_message_t* message)
+{
+    //-- Don't process messages to/from UDP Bridge. It doesn't suffer from these issues
+    if (message->compid == MAV_COMP_ID_UDP_BRIDGE) {
+        return;
+    }
+
+    switch (message->msgid) {
+    case MAVLINK_MSG_ID_PARAM_VALUE:
+        _handleParamValue(vehicle, message);
+        break;
+    case MAVLINK_MSG_ID_STATUSTEXT:
+        _handleStatusText(vehicle, message);
+        break;
+    case MAVLINK_MSG_ID_HEARTBEAT:
+        _handleHeartbeat(vehicle, message);
+        break;
+    }
+}
+
+void APMFirmwarePlugin::adjustOutgoingMavlinkMessage(Vehicle* vehicle, mavlink_message_t* message)
+{
+    //-- Don't process messages to/from UDP Bridge. It doesn't suffer from these issues
+    if (message->compid == MAV_COMP_ID_UDP_BRIDGE) {
+        return;
+    }
+
+    switch (message->msgid) {
+    case MAVLINK_MSG_ID_PARAM_SET:
+        _handleParamSet(vehicle, message);
+        break;
+    }
 }
 
 QString APMFirmwarePlugin::_getMessageText(mavlink_message_t* message) const
@@ -390,26 +444,26 @@ QString APMFirmwarePlugin::_getMessageText(mavlink_message_t* message) const
 
 bool APMFirmwarePlugin::_isTextSeverityAdjustmentNeeded(const APMFirmwareVersion& firmwareVersion)
 {
-   if (!firmwareVersion.isValid()) {
-       return false;
-   }
+    if (!firmwareVersion.isValid()) {
+        return false;
+    }
 
-   bool adjustmentNeeded = false;
-   if (firmwareVersion.vehicleType().contains(APM_COPTER_REXP)) {
-       if (firmwareVersion < APMFirmwareVersion(MIN_COPTER_VERSION_WITH_CORRECT_SEVERITY_MSGS)) {
-           adjustmentNeeded = true;
-       }
-   } else if (firmwareVersion.vehicleType().contains(APM_PLANE_REXP)) {
-       if (firmwareVersion < APMFirmwareVersion(MIN_PLANE_VERSION_WITH_CORRECT_SEVERITY_MSGS)) {
-           adjustmentNeeded = true;
-       }
-   } else if (firmwareVersion.vehicleType().contains(APM_ROVER_REXP)) {
-       if (firmwareVersion < APMFirmwareVersion(MIN_ROVER_VERSION_WITH_CORRECT_SEVERITY_MSGS)) {
-           adjustmentNeeded = true;
-       }
-   }
+    bool adjustmentNeeded = false;
+    if (firmwareVersion.vehicleType().contains(APM_COPTER_REXP)) {
+        if (firmwareVersion < APMFirmwareVersion(MIN_COPTER_VERSION_WITH_CORRECT_SEVERITY_MSGS)) {
+            adjustmentNeeded = true;
+        }
+    } else if (firmwareVersion.vehicleType().contains(APM_PLANE_REXP)) {
+        if (firmwareVersion < APMFirmwareVersion(MIN_PLANE_VERSION_WITH_CORRECT_SEVERITY_MSGS)) {
+            adjustmentNeeded = true;
+        }
+    } else if (firmwareVersion.vehicleType().contains(APM_ROVER_REXP)) {
+        if (firmwareVersion < APMFirmwareVersion(MIN_ROVER_VERSION_WITH_CORRECT_SEVERITY_MSGS)) {
+            adjustmentNeeded = true;
+        }
+    }
 
-   return adjustmentNeeded;
+    return adjustmentNeeded;
 }
 
 void APMFirmwarePlugin::_adjustSeverity(mavlink_message_t* message) const
@@ -418,15 +472,15 @@ void APMFirmwarePlugin::_adjustSeverity(mavlink_message_t* message) const
     mavlink_statustext_t statusText;
     mavlink_msg_statustext_decode(message, &statusText);
     switch(statusText.severity) {
-        case MAV_SEVERITY_ALERT:    /* SEVERITY_LOW according to old codes */
-            statusText.severity = MAV_SEVERITY_WARNING;
-            break;
-        case MAV_SEVERITY_CRITICAL: /*SEVERITY_MEDIUM according to old codes  */
-            statusText.severity = MAV_SEVERITY_ALERT;
-            break;
-        case MAV_SEVERITY_ERROR:    /*SEVERITY_HIGH according to old codes */
-            statusText.severity = MAV_SEVERITY_CRITICAL;
-            break;
+    case MAV_SEVERITY_ALERT:    /* SEVERITY_LOW according to old codes */
+        statusText.severity = MAV_SEVERITY_WARNING;
+        break;
+    case MAV_SEVERITY_CRITICAL: /*SEVERITY_MEDIUM according to old codes  */
+        statusText.severity = MAV_SEVERITY_ALERT;
+        break;
+    case MAV_SEVERITY_ERROR:    /*SEVERITY_HIGH according to old codes */
+        statusText.severity = MAV_SEVERITY_CRITICAL;
+        break;
     }
 
     mavlink_msg_statustext_encode(message->sysid, message->compid, message, &statusText);
@@ -520,3 +574,14 @@ QObject* APMFirmwarePlugin::loadParameterMetaData(const QString& metaDataFile)
     APMParameterMetaData* metaData = new APMParameterMetaData;
     return metaData;
 }
+
+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");
+}
diff --git a/src/FirmwarePlugin/APM/APMFirmwarePlugin.h b/src/FirmwarePlugin/APM/APMFirmwarePlugin.h
index 7a7e7bc86bb89a3c34a18ed74446bdd037f414bb..2e5eeeb7191438b634011a62f2fb835929dbd140 100644
--- a/src/FirmwarePlugin/APM/APMFirmwarePlugin.h
+++ b/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;
diff --git a/src/FirmwarePlugin/APM/ArduCopterFirmwarePlugin.cc b/src/FirmwarePlugin/APM/ArduCopterFirmwarePlugin.cc
index 23c793f9f6a8d2162cd25a0c3cb10f26bb80321d..48a469b31aaf831d769585a83a1fdfdb9fb0668a 100644
--- a/src/FirmwarePlugin/APM/ArduCopterFirmwarePlugin.cc
+++ b/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);
+    }
+}
diff --git a/src/FirmwarePlugin/APM/ArduCopterFirmwarePlugin.h b/src/FirmwarePlugin/APM/ArduCopterFirmwarePlugin.h
index 4f8cfa1b40305f398fd6ff4a05987d8be85e8b6e..c4555d0cf6ac50ef06210f708ec20a3ae30c6fc0 100644
--- a/src/FirmwarePlugin/APM/ArduCopterFirmwarePlugin.h
+++ b/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
diff --git a/src/FirmwarePlugin/FirmwarePlugin.cc b/src/FirmwarePlugin/FirmwarePlugin.cc
index 2e0e38cdb63cfc29c6bb3afa15993939fbce2e40..9b94fb4eef31f7b725c5d7d0dd808da4f3c57b84 100644
--- a/src/FirmwarePlugin/FirmwarePlugin.cc
+++ b/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."));
+}
diff --git a/src/FirmwarePlugin/FirmwarePlugin.h b/src/FirmwarePlugin/FirmwarePlugin.h
index 6bdc4f83ea900933a566e0d6359bede3e25c7fc8..92b4b3c69b35499796623ffad86aadfe5dc16c5a 100644
--- a/src/FirmwarePlugin/FirmwarePlugin.h
+++ b/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);
 
diff --git a/src/FirmwarePlugin/PX4/PX4FirmwarePlugin.cc b/src/FirmwarePlugin/PX4/PX4FirmwarePlugin.cc
index 90aeb08d1e4ff46a58e638b2b41c33b9f2f31d23..01f7d89623c75375817971f5d0213bd90f8669ca 100644
--- a/src/FirmwarePlugin/PX4/PX4FirmwarePlugin.cc
+++ b/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");
+}
diff --git a/src/FirmwarePlugin/PX4/PX4FirmwarePlugin.h b/src/FirmwarePlugin/PX4/PX4FirmwarePlugin.h
index 4d8f9fdb14e134097d127a1700f8e9bb60759676..894eb2b189279a1e14bf0a512daf4a26bb376dbf 100644
--- a/src/FirmwarePlugin/PX4/PX4FirmwarePlugin.h
+++ b/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;
diff --git a/src/FlightDisplay/FlightDisplayView.qml b/src/FlightDisplay/FlightDisplayView.qml
index 8a7c39798be42964d9d8ddafea4854276419a7c7..1dfa87a381b6d29229304aafb2c1ed5c40089528 100644
--- a/src/FlightDisplay/FlightDisplayView.qml
+++ b/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
             }
         }
 
diff --git a/src/FlightDisplay/FlightDisplayViewMap.qml b/src/FlightDisplay/FlightDisplayViewMap.qml
index d278e8b530ebbdae5838707b2e98841975d8e859..884719ea9e7d8f6c350ff0e587e641fcf8cd96d2 100644
--- a/src/FlightDisplay/FlightDisplayViewMap.qml
+++ b/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)
+            }
+        }
     }
 }
diff --git a/src/FlightDisplay/FlightDisplayViewWidgets.qml b/src/FlightDisplay/FlightDisplayViewWidgets.qml
index 3f998e9d4ea78ed847a849c193162baf831252d9..371ffcf17dc995486475f19dd7bfa2c2659c4dc5 100644
--- a/src/FlightDisplay/FlightDisplayViewWidgets.qml
+++ b/src/FlightDisplay/FlightDisplayViewWidgets.qml
@@ -38,9 +38,13 @@ import QGroundControl.FlightMap     1.0
 Item {
     id: _root
 
-    property var    _activeVehicle: multiVehicleManager.activeVehicle
+    property alias guidedModeBar: _guidedModeBar
+
+    property var    _activeVehicle:         QGroundControl.multiVehicleManager.activeVehicle
     property bool   _isSatellite:   _mainIsMap ? _flightMap ? _flightMap.isSatelliteMap : true : true
 
+    readonly property real _margins: ScreenTools.defaultFontPixelHeight / 2
+
     QGCMapPalette { id: mapPal; lightColors: !isBackgroundDark }
 
     function getGadgetWidth() {
@@ -93,7 +97,7 @@ Item {
     QGCInstrumentWidget {
         id:                     instrumentGadget
         anchors.margins:        ScreenTools.defaultFontPixelHeight / 2
-        anchors.right:          parent.right
+        anchors.right:          altitudeSlider.visible ? altitudeSlider.left : parent.right
         anchors.verticalCenter: parent.verticalCenter
         visible:                !QGroundControl.virtualTabletJoystick
         size:                   getGadgetWidth()
@@ -113,7 +117,7 @@ Item {
         id:                     instrumentGadgetAlternate
         anchors.margins:        ScreenTools.defaultFontPixelHeight / 2
         anchors.top:            parent.top
-        anchors.right:          parent.right
+        anchors.right:          altitudeSlider.visible ? altitudeSlider.left : parent.right
         visible:                QGroundControl.virtualTabletJoystick
         width:                  getGadgetWidth()
         active:                 _activeVehicle != null
@@ -259,7 +263,232 @@ Item {
                 checked = false
             }
         }
-
     }
 
+    //-- Guided mode buttons
+    Rectangle {
+        id:                         _guidedModeBar
+        anchors.margins:            _margins
+        anchors.bottom:             parent.bottom
+        anchors.horizontalCenter:   parent.horizontalCenter
+        width:                      (guidedModeButtons.visible ? guidedModeButtons.width : guidedModeConfirm.width) + (_margins * 2)
+        height:                     (guidedModeButtons.visible ? guidedModeButtons.height : guidedModeConfirm.height) + (_margins * 2)
+        color:                      qgcPal.window
+        visible:                    _activeVehicle
+        opacity:                    0.9
+        z:                          QGroundControl.zOrderWidgets
+
+        readonly property int confirmHome:          1
+        readonly property int confirmLand:          2
+        readonly property int confirmTakeoff:       3
+        readonly property int confirmArm:           4
+        readonly property int confirmDisarm:        5
+        readonly property int confirmEmergencyStop: 6
+        readonly property int confirmChangeAlt:     7
+        readonly property int confirmGoTo:          8
+
+        property int    confirmActionCode
+        property string confirmText
+
+        function actionConfirmed() {
+            switch (confirmActionCode) {
+            case confirmHome:
+                _activeVehicle.guidedModeRTL()
+                break;
+            case confirmLand:
+                _activeVehicle.guidedModeLand()
+                break;
+            case confirmTakeoff:
+                var altitude = altitudeSlider.getValue()
+                if (!isNaN(altitude)) {
+                    _activeVehicle.guidedModeTakeoff(altitude)
+                }
+                break;
+            case confirmArm:
+                _activeVehicle.armed = true
+                break;
+            case confirmDisarm:
+                _activeVehicle.armed = false
+                break;
+            case confirmEmergencyStop:
+                _activeVehicle.emergencyStop()
+                break;
+            case confirmChangeAlt:
+                var altitude = altitudeSlider.getValue()
+                if (!isNaN(altitude)) {
+                    _activeVehicle.guidedModeChangeAltitude(altitude)
+                }
+                break;
+            case confirmGoTo:
+                _activeVehicle.guidedModeGotoLocation(_flightMap._gotoHereCoordinate)
+                break;
+            default:
+                console.warn("Internal error: unknown confirmActionCode", confirmActionCode)
+            }
+        }
+
+        function confirmAction(actionCode) {
+            confirmActionCode = actionCode
+            switch (confirmActionCode) {
+            case confirmArm:
+                _guidedModeBar.confirmText = "arm"
+                break;
+            case confirmDisarm:
+                _guidedModeBar.confirmText = "disarm"
+                break;
+            case confirmEmergencyStop:
+                _guidedModeBar.confirmText = "emergency stop"
+                break;
+            case confirmTakeoff:
+                altitudeSlider.visible = true
+                altitudeSlider.initialValue = 10
+                _guidedModeBar.confirmText = "takeoff"
+                break;
+            case confirmLand:
+                _guidedModeBar.confirmText = "land"
+                break;
+            case confirmChangeAlt:
+                altitudeSlider.visible = true
+                altitudeSlider.initialValue = _activeVehicle.altitudeAMSL.value
+                _guidedModeBar.confirmText = "altitude change"
+                break;
+            case confirmGoTo:
+                _guidedModeBar.confirmText = "move"
+                break;
+            }
+            guidedModeButtons.visible = false
+            guidedModeConfirm.visible = true
+        }
+
+        Row {
+            id:                 guidedModeButtons
+            anchors.margins:    _margins
+            anchors.top:        parent.top
+            anchors.left:       parent.left
+            spacing:            _margins
+
+            QGCButton {
+                text:       _activeVehicle.armed ? (_activeVehicle.flying ? "Emergency Stop" : "Disarm") : "Arm"
+                onClicked:  _guidedModeBar.confirmAction(_activeVehicle.armed ? (_activeVehicle.flying ? _guidedModeBar.confirmEmergencyStop : _guidedModeBar.confirmDisarm) : _guidedModeBar.confirmArm)
+            }
+
+            QGCButton {
+                text:       "RTL"
+                visible:    _activeVehicle.guidedModeSupported && _activeVehicle.flying
+                onClicked:  _guidedModeBar.confirmAction(_guidedModeBar.confirmHome)
+            }
+
+            QGCButton {
+                text:        _activeVehicle.flying ? "Land" : "Takeoff"
+                visible:    _activeVehicle.guidedModeSupported && _activeVehicle.armed
+                onClicked:  _guidedModeBar.confirmAction(_activeVehicle.flying ? _guidedModeBar.confirmLand : _guidedModeBar.confirmTakeoff)
+            }
+
+            QGCButton {
+                text:       "Pause"
+                visible:    _activeVehicle.pauseVehicleSupported && _activeVehicle.flying
+                onClicked:  _activeVehicle.pauseVehicle()
+            }
+
+            QGCButton {
+                text:       "Change Altitude"
+                visible:    _activeVehicle.guidedModeSupported && _activeVehicle.armed
+                onClicked:  _guidedModeBar.confirmAction(_guidedModeBar.confirmChangeAlt)
+            }
+        }
+
+        Row {
+            id:                 guidedModeConfirm
+            anchors.margins:    _margins
+            anchors.top:        parent.top
+            anchors.left:       parent.left
+            spacing:            _margins
+            visible:            false
+
+            QGCLabel {
+                text: "Confirm " + _guidedModeBar.confirmText + " :"
+                anchors.verticalCenter: parent.verticalCenter
+            }
+
+            QGCButton {
+                text: "Yes"
+                onClicked: {
+                    guidedModeConfirm.visible = false
+                    guidedModeButtons.visible = true
+                    _guidedModeBar.actionConfirmed()
+                    altitudeSlider.visible = false
+                }
+            }
+
+            QGCButton {
+                text: "No"
+                onClicked: {
+                    guidedModeConfirm.visible = false
+                    guidedModeButtons.visible = true
+                    altitudeSlider.visible = false
+                    _flightMap._gotoHereCoordinate = QtPositioning.coordinate()
+                }
+            }
+        }
+    } // Column - Vertical tool buttons
+
+    //-- Altitude slider
+    Rectangle {
+        id:                 altitudeSlider
+        anchors.margins:    _margins
+        anchors.right:      parent.right
+        anchors.top:        parent.top
+        anchors.bottom:     parent.bottom
+        color:              qgcPal.window
+        width:              ScreenTools.defaultFontPixelWidth * 10
+        opacity:            0.8
+        visible:            false
+
+        property alias initialValue:    altSlider.value
+
+        /// Returns NaN for bad value
+        function getValue() {
+            return parseFloat(altField.text)
+        }
+
+
+        Column {
+            id:                 headerColumn
+            anchors.margins:    _margins
+            anchors.top:        parent.top
+            anchors.left:       parent.left
+            anchors.right:      parent.right
+
+            QGCLabel {
+                anchors.horizontalCenter: parent.horizontalCenter
+                text: "Altitude"
+            }
+
+            QGCLabel {
+                anchors.horizontalCenter: parent.horizontalCenter
+                text: "meters (rel)"
+            }
+
+            QGCTextField {
+                id:             altField
+                anchors.left:   parent.left
+                anchors.right:  parent.right
+                text:           altSlider.value.toFixed(1)
+            }
+        }
+
+        Slider {
+            id:                 altSlider
+            anchors.margins:    _margins
+            anchors.top:        headerColumn.bottom
+            anchors.bottom:     parent.bottom
+            anchors.left:       parent.left
+            anchors.right:      parent.right
+            orientation:        Qt.Vertical
+            minimumValue:       0
+            maximumValue:       100
+            value:              30
+            //anchors.horizontalCenter: parent.horizontalCenter
+        }
+    }
 }
diff --git a/src/MissionManager/MissionManager.cc b/src/MissionManager/MissionManager.cc
index 79c4252c00e6af866a5bbdf941d5f516fe8e5df7..c3664f27f71a6d00c8436e58b9236e95746f687c 100644
--- a/src/MissionManager/MissionManager.cc
+++ b/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");
diff --git a/src/MissionManager/MissionManager.h b/src/MissionManager/MissionManager.h
index 8115f4c60a42f6da79af363ff17c875ee0f65687..47b7ca83ac1b26f79e2dc319c1237247ee8c079a 100644
--- a/src/MissionManager/MissionManager.h
+++ b/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);
diff --git a/src/Vehicle/Vehicle.cc b/src/Vehicle/Vehicle.cc
index e5fb7d1700fc5d0bfa83c0936a17159cb6e54739..0884e89de5aac76b754d070ce9917b547c235fc0 100644
--- a/src/Vehicle/Vehicle.cc
+++ b/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";
diff --git a/src/Vehicle/Vehicle.h b/src/Vehicle/Vehicle.h
index 3775a2a783d46a5108f1a5b3d4f9d5788e99c92c..9eaed890d8ca19db4ccab1946a8cb4d9e1de09e0 100644
--- a/src/Vehicle/Vehicle.h
+++ b/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;
diff --git a/src/ui/toolbar/MainToolBarIndicators.qml b/src/ui/toolbar/MainToolBarIndicators.qml
index fab64e789174419ea823bfa89207f0aa62f32a56..7a549621147e69b179ed5777d2dcac29c9b53a7a 100644
--- a/src/ui/toolbar/MainToolBarIndicators.qml
+++ b/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"