Merge pull request #8181 from DonLakeFlyer/GimbalYaw

Plan View: Update vehicle heading from possible item change

src/MissionManager/MissionController.cc

@@ -1484,6 +1484,7 @@ void MissionController::_recalcMissionFlightStatus()
     bool vtolInHover = true;
     bool linkStartToHome = false;
     bool foundRTL = false;
+    bool vehicleYawSpecificallySet = false;
 
     for (int i=0; i<_visualItems->count(); i++) {
         VisualMissionItem*  item =          qobject_cast<VisualMissionItem*>(_visualItems->get(i));
@@ -1614,9 +1615,14 @@ void MissionController::_recalcMissionFlightStatus()
             if (!item->isStandaloneCoordinate()) {
                 firstCoordinateItem = false;
 
-                // Update vehicle yaw assuming direction to next waypoint
+                // Update vehicle yaw assuming direction to next waypoint and/or mission item change
                 if (item != lastCoordinateItemBeforeRTL) {
-                    _missionFlightStatus.vehicleYaw = lastCoordinateItemBeforeRTL->exitCoordinate().azimuthTo(item->coordinate());
+                    if (simpleItem && !qIsNaN(simpleItem->specifiedVehicleYaw())) {
+                        vehicleYawSpecificallySet = true;
+                        _missionFlightStatus.vehicleYaw = simpleItem->specifiedVehicleYaw();
+                    } else if (!vehicleYawSpecificallySet) {
+                        _missionFlightStatus.vehicleYaw = lastCoordinateItemBeforeRTL->exitCoordinate().azimuthTo(item->coordinate());
+                    }
                     lastCoordinateItemBeforeRTL->setMissionVehicleYaw(_missionFlightStatus.vehicleYaw);
                 }
 
@@ -1868,6 +1874,7 @@ void MissionController::_initVisualItem(VisualMissionItem* visualItem)
     connect(visualItem, &VisualMissionItem::specifiedFlightSpeedChanged,                this, &MissionController::_recalcMissionFlightStatus);
     connect(visualItem, &VisualMissionItem::specifiedGimbalYawChanged,                  this, &MissionController::_recalcMissionFlightStatus);
     connect(visualItem, &VisualMissionItem::specifiedGimbalPitchChanged,                this, &MissionController::_recalcMissionFlightStatus);
+    connect(visualItem, &VisualMissionItem::specifiedVehicleYawChanged,                 this, &MissionController::_recalcMissionFlightStatus);
     connect(visualItem, &VisualMissionItem::terrainAltitudeChanged,                     this, &MissionController::_recalcMissionFlightStatus);
     connect(visualItem, &VisualMissionItem::additionalTimeDelayChanged,                 this, &MissionController::_recalcMissionFlightStatus);
     connect(visualItem, &VisualMissionItem::lastSequenceNumberChanged,                  this, &MissionController::_recalcSequence);

src/MissionManager/SimpleMissionItem.cc

@@ -177,6 +177,7 @@ void SimpleMissionItem::_connectSignals(void)
     connect(&_missionItem._param7Fact,  &Fact::valueChanged, this, &SimpleMissionItem::_sendCoordinateChanged);
 
     connect(&_missionItem._param1Fact,  &Fact::valueChanged, this, &SimpleMissionItem::_possibleAdditionalTimeDelayChanged);
+    connect(&_missionItem._param4Fact,  &Fact::valueChanged, this, &SimpleMissionItem::_possibleVehicleYawChanged);
 
     // The following changes may also change friendlyEditAllowed
     connect(&_missionItem._autoContinueFact,    &Fact::valueChanged, this, &SimpleMissionItem::_sendFriendlyEditAllowedChanged);
@@ -834,6 +835,18 @@ double SimpleMissionItem::specifiedGimbalPitch(void)
     return _cameraSection->available() ? _cameraSection->specifiedGimbalPitch() : missionItem().specifiedGimbalPitch();
 }
 
+double SimpleMissionItem::specifiedVehicleYaw(void)
+{
+    return command() == MAV_CMD_NAV_WAYPOINT ? missionItem().param4() : qQNaN();
+}
+
+void SimpleMissionItem::_possibleVehicleYawChanged(void)
+{
+    if (command() == MAV_CMD_NAV_WAYPOINT) {
+        emit specifiedVehicleYawChanged();
+    }
+}
+
 bool SimpleMissionItem::scanForSections(QmlObjectListModel* visualItems, int scanIndex, Vehicle* vehicle)
 {
     bool sectionFound = false;

src/MissionManager/SimpleMissionItem.h

@@ -114,6 +114,7 @@ public:
     double          specifiedFlightSpeed    (void) override;
     double          specifiedGimbalYaw      (void) override;
     double          specifiedGimbalPitch    (void) override;
+    double          specifiedVehicleYaw     (void) override;
     QString         mapVisualQML            (void) const override { return QStringLiteral("SimpleItemMapVisual.qml"); }
     void            appendMissionItems      (QList<MissionItem*>& items, QObject* missionItemParent) final;
     void            applyNewAltitude        (double newAltitude) final;
@@ -155,6 +156,7 @@ private slots:
     void _rebuildTextFieldFacts             (void);
     void _possibleAdditionalTimeDelayChanged(void);
     void _setDefaultsForCommand             (void);
+    void _possibleVehicleYawChanged   (void);
 
 private:
     void _connectSignals        (void);

src/MissionManager/VisualMissionItem.h

@@ -70,9 +70,10 @@ public:
     Q_PROPERTY(QString          editorQml                           MEMBER _editorQml                                                       CONSTANT)                                           ///< Qml code for editing this item
     Q_PROPERTY(QString          mapVisualQML                        READ mapVisualQML                                                       CONSTANT)                                           ///< QMl code for map visuals
     Q_PROPERTY(QmlObjectListModel* childItems                       READ childItems                                                         CONSTANT)
-    Q_PROPERTY(double           specifiedFlightSpeed                READ specifiedFlightSpeed                                               NOTIFY specifiedFlightSpeedChanged)                 ///< NaN if this item does not specify flight speed
-    Q_PROPERTY(double           specifiedGimbalYaw                  READ specifiedGimbalYaw                                                 NOTIFY specifiedGimbalYawChanged)                   ///< Gimbal yaw, NaN for not specified
-    Q_PROPERTY(double           specifiedGimbalPitch                READ specifiedGimbalPitch                                               NOTIFY specifiedGimbalPitchChanged)                 ///< Gimbal pitch, NaN for not specified
+    Q_PROPERTY(double           specifiedFlightSpeed                READ specifiedFlightSpeed                                               NOTIFY specifiedFlightSpeedChanged)                 ///< NaN for not specified
+    Q_PROPERTY(double           specifiedGimbalYaw                  READ specifiedGimbalYaw                                                 NOTIFY specifiedGimbalYawChanged)                   ///< NaN for not specified
+    Q_PROPERTY(double           specifiedGimbalPitch                READ specifiedGimbalPitch                                               NOTIFY specifiedGimbalPitchChanged)                 ///< NaN for not specified
+    Q_PROPERTY(double           specifiedVehicleYaw                 READ specifiedVehicleYaw                                                NOTIFY specifiedVehicleYawChanged)                  ///< NaN for not specified
     Q_PROPERTY(double           missionGimbalYaw                    READ missionGimbalYaw                                                   NOTIFY missionGimbalYawChanged)                     ///< Current gimbal yaw state at this point in mission
     Q_PROPERTY(double           missionVehicleYaw                   READ missionVehicleYaw                                                  NOTIFY missionVehicleYawChanged)                    ///< Expected vehicle yaw at this point in mission
     Q_PROPERTY(bool             flyView                             READ flyView                                                            CONSTANT)
@@ -140,6 +141,7 @@ public:
     virtual double          specifiedFlightSpeed    (void) = 0;
     virtual double          specifiedGimbalYaw      (void) = 0;
     virtual double          specifiedGimbalPitch    (void) = 0;
+    virtual double          specifiedVehicleYaw     (void) { return qQNaN(); }
 
     //-- Default implementation returns an invalid bounding cube
     virtual QGCGeoBoundingCube* boundingCube        (void) { return &_boundingCube; }
@@ -210,6 +212,7 @@ signals:
     void specifiedFlightSpeedChanged    (void);
     void specifiedGimbalYawChanged      (void);
     void specifiedGimbalPitchChanged    (void);
+    void specifiedVehicleYawChanged     (void);
     void lastSequenceNumberChanged      (int sequenceNumber);
     void missionGimbalYawChanged        (double missionGimbalYaw);
     void missionVehicleYawChanged       (double missionVehicleYaw);