diff --git a/src/FirmwarePlugin/APM/ArduCopterFirmwarePlugin.cc b/src/FirmwarePlugin/APM/ArduCopterFirmwarePlugin.cc
index 3c3ef9e5ef05bb093a999ac3a4a0558a77377f01..5bdb2b627587528f2fbd0afc29b3b253feada315 100644
--- a/src/FirmwarePlugin/APM/ArduCopterFirmwarePlugin.cc
+++ b/src/FirmwarePlugin/APM/ArduCopterFirmwarePlugin.cc
@@ -224,3 +224,13 @@ QString ArduCopterFirmwarePlugin::takeControlFlightMode(void)
 {
     return QStringLiteral("Stabilize");
 }
+
+
+bool ArduCopterFirmwarePlugin::vehicleYawsToNextWaypointInMission(const Vehicle* vehicle) const
+{
+    if (vehicle->parameterManager()->parameterExists(FactSystem::defaultComponentId, QStringLiteral("WP_YAW_BEHAVIOR"))) {
+        Fact* yawMode = vehicle->parameterManager()->getParameter(FactSystem::defaultComponentId, QStringLiteral("WP_YAW_BEHAVIOR"));
+        return yawMode && yawMode->rawValue().toInt() != 0;
+    }
+    return false;
+}
diff --git a/src/FirmwarePlugin/APM/ArduCopterFirmwarePlugin.h b/src/FirmwarePlugin/APM/ArduCopterFirmwarePlugin.h
index 9eb8e0d73a74c989c5c63c066ddfaeedb6627740..40c5f76233fba1991758ae45c39ef41618f37fbd 100644
--- a/src/FirmwarePlugin/APM/ArduCopterFirmwarePlugin.h
+++ b/src/FirmwarePlugin/APM/ArduCopterFirmwarePlugin.h
@@ -69,6 +69,7 @@ public:
     QString geoFenceRadiusParam(Vehicle* vehicle) final;
     QString offlineEditingParamFile(Vehicle* vehicle) final { Q_UNUSED(vehicle); return QStringLiteral(":/FirmwarePlugin/APM/Copter.OfflineEditing.params"); }
     QString takeControlFlightMode(void) final;
+    bool vehicleYawsToNextWaypointInMission(const Vehicle* vehicle) const final;
 
 private:
     static bool _remapParamNameIntialized;
diff --git a/src/FirmwarePlugin/FirmwarePlugin.cc b/src/FirmwarePlugin/FirmwarePlugin.cc
index 18c2d31a436d20194305df53d54825214e481fc3..498be2571186328e0052828a216d67abdd1667b0 100644
--- a/src/FirmwarePlugin/FirmwarePlugin.cc
+++ b/src/FirmwarePlugin/FirmwarePlugin.cc
@@ -431,3 +431,8 @@ const QVariantList& FirmwarePlugin::cameraList(const Vehicle* vehicle)
 
     return _cameraList;
 }
+
+bool FirmwarePlugin::vehicleYawsToNextWaypointInMission(const Vehicle* vehicle) const
+{
+    return vehicle->multiRotor() ? false : true;
+}
diff --git a/src/FirmwarePlugin/FirmwarePlugin.h b/src/FirmwarePlugin/FirmwarePlugin.h
index a6bfa5c300adaac5271cef5df607b02cd7ee695d..2a9f51da29fa09bfd6e09a4b10713ec365199564 100644
--- a/src/FirmwarePlugin/FirmwarePlugin.h
+++ b/src/FirmwarePlugin/FirmwarePlugin.h
@@ -267,6 +267,9 @@ public:
     /// Returns a list of CameraMetaData objects for available cameras on the vehicle.
     virtual const QVariantList& cameraList(const Vehicle* vehicle);
 
+    /// @true: When flying a mission the vehicle is always facing towards the next waypoint
+    virtual bool vehicleYawsToNextWaypointInMission(const Vehicle* vehicle) const;
+
     // FIXME: Hack workaround for non pluginize FollowMe support
     static const char* px4FollowMeFlightMode;
 
diff --git a/src/FirmwarePlugin/PX4/PX4FirmwarePlugin.cc b/src/FirmwarePlugin/PX4/PX4FirmwarePlugin.cc
index c399aab7231a7a09cfa87d8a4bb2f1b19ff063b5..cd2314cbc7955017996542a287f849d45642f615 100644
--- a/src/FirmwarePlugin/PX4/PX4FirmwarePlugin.cc
+++ b/src/FirmwarePlugin/PX4/PX4FirmwarePlugin.cc
@@ -498,3 +498,12 @@ QString PX4FirmwarePlugin::takeControlFlightMode(void)
 {
     return QString(_manualFlightMode);
 }
+
+bool PX4FirmwarePlugin::vehicleYawsToNextWaypointInMission(const Vehicle* vehicle) const
+{
+    if ( vehicle->parameterManager()->parameterExists(FactSystem::defaultComponentId, QStringLiteral("MIS_YAWMODE"))) {
+        Fact* yawMode = vehicle->parameterManager()->getParameter(FactSystem::defaultComponentId, QStringLiteral("MIS_YAWMODE"));
+        return yawMode && yawMode->rawValue().toInt() == 1;
+    }
+    return false;
+}
diff --git a/src/FirmwarePlugin/PX4/PX4FirmwarePlugin.h b/src/FirmwarePlugin/PX4/PX4FirmwarePlugin.h
index ef0eed447b5cbfa8df6c08fdfe65ca86257fe988..7394abb023a304feb06fa93fe622ef8b685f98e9 100644
--- a/src/FirmwarePlugin/PX4/PX4FirmwarePlugin.h
+++ b/src/FirmwarePlugin/PX4/PX4FirmwarePlugin.h
@@ -63,6 +63,7 @@ public:
     QString             missionFlightMode               (void) override;
     QString             rtlFlightMode                   (void) override;
     QString             takeControlFlightMode           (void) override;
+    bool                vehicleYawsToNextWaypointInMission(const Vehicle* vehicle) const override;
 
 protected:
     typedef struct {
diff --git a/src/MissionEditor/MissionEditor.qml b/src/MissionEditor/MissionEditor.qml
index 61f599d41b50ef28ed9679a6c465bc3b0d8818d8..b3ecbf9f71c2d3b8e21651712952fb2b6ac79727 100644
--- a/src/MissionEditor/MissionEditor.qml
+++ b/src/MissionEditor/MissionEditor.qml
@@ -236,7 +236,6 @@ QGCView {
     /// Sets a new current mission item
     ///     @param sequenceNumber - index for new item, -1 to clear current item
     function setCurrentItem(sequenceNumber) {
-        console.log("setCurrentItem", sequenceNumber, _currentMissionIndex)
         if (sequenceNumber !== _currentMissionIndex) {
             _currentMissionItem = undefined
             _currentMissionIndex = -1
diff --git a/src/MissionManager/MissionController.cc b/src/MissionManager/MissionController.cc
index e7c26d259a509546e1afc42ea6403669da0e4a7e..293f71621adb681b87ed0360166d7f445f6104f2 100644
--- a/src/MissionManager/MissionController.cc
+++ b/src/MissionManager/MissionController.cc
@@ -939,9 +939,12 @@ void MissionController::_recalcMissionFlightStatus()
         }
 
         // Look for gimbal change
-        double gimbalYaw = item->specifiedGimbalYaw();
-        if (!qIsNaN(gimbalYaw)) {
-            _missionFlightStatus.gimbalYaw = gimbalYaw;
+        if (_activeVehicle->vehicleYawsToNextWaypointInMission()) {
+            // We current only support gimbal display in this mode
+            double gimbalYaw = item->specifiedGimbalYaw();
+            if (!qIsNaN(gimbalYaw)) {
+                _missionFlightStatus.gimbalYaw = gimbalYaw;
+            }
         }
 
         if (i == 0) {
diff --git a/src/Vehicle/Vehicle.cc b/src/Vehicle/Vehicle.cc
index 70b88a0e26f2b6936c837711523631b7d4168bb6..bd6359fd0d69f8d344dff5f5f83804f197ac1bd9 100644
--- a/src/Vehicle/Vehicle.cc
+++ b/src/Vehicle/Vehicle.cc
@@ -2436,6 +2436,10 @@ const QVariantList& Vehicle::cameraList(void) const
     return emptyList;
 }
 
+bool Vehicle::vehicleYawsToNextWaypointInMission(void) const
+{
+    return _firmwarePlugin->vehicleYawsToNextWaypointInMission(this);
+}
 
 //-----------------------------------------------------------------------------
 //-----------------------------------------------------------------------------
diff --git a/src/Vehicle/Vehicle.h b/src/Vehicle/Vehicle.h
index c2c9a71a3917e344be68e756b6f711a2cad4bfeb..4e53537685a55b7d0bba6de0db05f34c8dfc3632 100644
--- a/src/Vehicle/Vehicle.h
+++ b/src/Vehicle/Vehicle.h
@@ -660,6 +660,9 @@ public:
 
     bool supportsMissionItemInt(void) const { return _supportsMissionItemInt; }
 
+    /// @true: When flying a mission the vehicle is always facing towards the next waypoint
+    bool vehicleYawsToNextWaypointInMission(void) const;
+
 public slots:
     /// Sets the firmware plugin instance data associated with this Vehicle. This object will be parented to the Vehicle
     /// and destroyed when the vehicle goes away.