Only show gimbal yaw if vehicle points to next waypoint

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;
+}

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;

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;
+}

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;
 

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;
+}

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 {

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

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) {

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);
+}
 
 //-----------------------------------------------------------------------------
 //-----------------------------------------------------------------------------

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.