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, &