Gimbal: Use delta_yaw in heading calc when provided rather than vehicle

src/Comms/MockLink/MockLinkGimbal.cc

@@ -320,7 +320,8 @@ void MockLinkGimbal::_sendGimbalDeviceAttitudeStatus()
         q,
         0.0f, 0.0f, 0.0f,   // angular_velocity_x, y, z
         0,                  // failure flags
-        _pitch, _yaw,       // delta_pitch, delta_yaw (Euler angles in degrees)
+        yawRad,             // deltaYaw (rad)
+        0.0f,               // deltaYawVelocity (rad/s)
         0);                 // gimbal_device_id = 0 means the device id is the same as the message component ID
     _mockLink->respondWithMavlinkMessage(msg);
 }

src/Gimbal/Gimbal.cc

@@ -41,6 +41,7 @@ const Gimbal &Gimbal::operator=(const Gimbal &other)
     _absolutePitchFact = other._absolutePitchFact;
     _bodyYawFact = other._bodyYawFact;
     _absoluteYawFact = other._absoluteYawFact;
+    _deltaYawFact = other._deltaYawFact;
     _deviceIdFact = other._deviceIdFact;
     _yawLock = other._yawLock;
     _haveControl = other._haveControl;
@@ -55,13 +56,15 @@ void Gimbal::_initFacts()
     _addFact(&_absolutePitchFact);
     _addFact(&_bodyYawFact);
     _addFact(&_absoluteYawFact);
+    _addFact(&_deltaYawFact);
     _addFact(&_deviceIdFact);
     _addFact(&_managerCompidFact);
 
     _absoluteRollFact.setRawValue(0.0f);
     _absolutePitchFact.setRawValue(0.0f);
     _bodyYawFact.setRawValue(0.0f);
     _absoluteYawFact.setRawValue(0.0f);
+    _deltaYawFact.setRawValue(qQNaN());
     _deviceIdFact.setRawValue(0);
     _managerCompidFact.setRawValue(0);
 }

src/Gimbal/Gimbal.h

@@ -12,6 +12,7 @@ class Gimbal : public FactGroup
     Q_PROPERTY(Fact     *absolutePitch          READ absolutePitch              CONSTANT)
     Q_PROPERTY(Fact     *bodyYaw                READ bodyYaw                    CONSTANT)
     Q_PROPERTY(Fact     *absoluteYaw            READ absoluteYaw                CONSTANT)
+    Q_PROPERTY(Fact     *deltaYaw               READ deltaYaw                   CONSTANT)
     Q_PROPERTY(Fact     *deviceId               READ deviceId                   CONSTANT)
     Q_PROPERTY(Fact     *managerCompid          READ managerCompid              CONSTANT)
     Q_PROPERTY(float    pitchRate               READ pitchRate                  NOTIFY pitchRateChanged)
@@ -35,6 +36,7 @@ class Gimbal : public FactGroup
     Fact *absolutePitch() { return &_absolutePitchFact; }
     Fact *bodyYaw() { return &_bodyYawFact; }
     Fact *absoluteYaw() { return &_absoluteYawFact; }
+    Fact *deltaYaw() { return &_deltaYawFact; }
     Fact *deviceId() { return &_deviceIdFact; }
     Fact *managerCompid() { return &_managerCompidFact; }
 
@@ -49,6 +51,7 @@ class Gimbal : public FactGroup
     void setAbsolutePitch(float absPitch) { absolutePitch()->setRawValue(absPitch); }
     void setBodyYaw(float yaw) { bodyYaw()->setRawValue(yaw); }
     void setAbsoluteYaw(float absYaw) { absoluteYaw()->setRawValue(absYaw); }
+    void setDeltaYaw(float delta) { deltaYaw()->setRawValue(delta); _receivedDeltaYaw = true; }
     void setDeviceId(uint id) { deviceId()->setRawValue(id); }
     void setManagerCompid(uint id) { managerCompid()->setRawValue(id); }
 
@@ -62,6 +65,7 @@ class Gimbal : public FactGroup
     void setCapabilityFlags(uint32_t flags);
     bool supportsRetract() const { return (_capabilityFlags & GIMBAL_MANAGER_CAP_FLAGS_HAS_RETRACT) != 0; }
     bool supportsYawLock() const { return (_capabilityFlags & GIMBAL_MANAGER_CAP_FLAGS_HAS_YAW_LOCK) != 0; }
+    bool hasDeltaYaw() { return _receivedDeltaYaw; }
 
 signals:
     void pitchRateChanged();
@@ -81,6 +85,7 @@ class Gimbal : public FactGroup
     bool _receivedGimbalManagerInformation = false;
     bool _receivedGimbalManagerStatus = false;
     bool _receivedGimbalDeviceAttitudeStatus = false;
+    bool _receivedDeltaYaw = false;
     bool _isComplete = false;
     bool _neutral = false;
     uint32_t _capabilityFlags = 0; // GIMBAL_MANAGER_CAP_FLAGS
@@ -89,6 +94,7 @@ class Gimbal : public FactGroup
     Fact _absolutePitchFact = Fact(0, QStringLiteral("gimbalPitch"), FactMetaData::valueTypeFloat);
     Fact _bodyYawFact = Fact(0, QStringLiteral("gimbalYaw"), FactMetaData::valueTypeFloat);
     Fact _absoluteYawFact = Fact(0, QStringLiteral("gimbalAzimuth"), FactMetaData::valueTypeFloat);
+    Fact _deltaYawFact = Fact(0, QStringLiteral("gimbalDeltaYaw"), FactMetaData::valueTypeFloat);
     Fact _deviceIdFact = Fact(0, QStringLiteral("deviceId"), FactMetaData::valueTypeUint8); ///< Component ID of gimbal device (or 1-6 for non-MAVLink gimbal)
     Fact _managerCompidFact = Fact(0, QStringLiteral("managerCompid"), FactMetaData::valueTypeUint8);
 

src/Gimbal/GimbalController.cc

@@ -240,23 +240,28 @@ void GimbalController::_handleGimbalDeviceAttitudeStatus(const mavlink_message_t
     gimbal->setAbsoluteRoll(qRadiansToDegrees(roll));
     gimbal->setAbsolutePitch(qRadiansToDegrees(pitch));
 
+    const float deltaYawDeg = qRadiansToDegrees(attitude_status.delta_yaw);
+    // Extension field: unsent decodes as 0.0, so ignore 0 until a value proves support.
+    if (gimbal->hasDeltaYaw() || attitude_status.delta_yaw != 0.0f) {
+        gimbal->setDeltaYaw(deltaYawDeg);
+    }
+
+    // Prefer the gimbal's own heading estimate; fall back to vehicle heading.
+    const float headingDeg = (gimbal->hasDeltaYaw() && !qIsNaN(deltaYawDeg))
+        ? deltaYawDeg
+        : _vehicle->heading()->rawValue().toFloat();
+
     const bool yaw_in_vehicle_frame = _yawInVehicleFrame(attitude_status.flags);
     if (yaw_in_vehicle_frame) {
         const float bodyYaw = qRadiansToDegrees(yaw);
-        float absoluteYaw = bodyYaw + _vehicle->heading()->rawValue().toFloat();
-        if (absoluteYaw > 180.0f) {
-            absoluteYaw -= 360.0f;
-        }
+        const float absoluteYaw = std::remainder(bodyYaw + headingDeg, 360.0f);
 
         gimbal->setBodyYaw(bodyYaw);
         gimbal->setAbsoluteYaw(absoluteYaw);
 
     } else {
         const float absoluteYaw = qRadiansToDegrees(yaw);
-        float bodyYaw = absoluteYaw - _vehicle->heading()->rawValue().toFloat();
-        if (bodyYaw < -180.0f) {
-            bodyYaw += 360.0f;
-        }
+        const float bodyYaw = std::remainder(absoluteYaw - headingDeg, 360.0f);
 
         gimbal->setBodyYaw(bodyYaw);
         gimbal->setAbsoluteYaw(absoluteYaw);
@@ -450,7 +455,7 @@ void GimbalController::gimbalOnScreenControl(float panPct, float tiltPct, bool c
         const float tiltDesired = tiltIncDesired + _activeGimbal->absolutePitch()->rawValue().toFloat();
 
         if (_activeGimbal->yawLock()) {
-            sendPitchAbsoluteYaw(tiltDesired, panDesired + _vehicle->heading()->rawValue().toFloat(), false);
+            sendPitchAbsoluteYaw(tiltDesired, std::remainder(panIncDesired + _activeGimbal->absoluteYaw()->rawValue().toFloat(), 360.0f), false);
         } else {
             sendPitchBodyYaw(tiltDesired, panDesired, false);
         }
@@ -467,7 +472,7 @@ void GimbalController::gimbalOnScreenControl(float panPct, float tiltPct, bool c
         const float tiltDesired = tiltIncDesired + _activeGimbal->absolutePitch()->rawValue().toFloat();
 
         if (_activeGimbal->yawLock()) {
-            sendPitchAbsoluteYaw(tiltDesired, panDesired + _vehicle->heading()->rawValue().toFloat(), false);
+            sendPitchAbsoluteYaw(tiltDesired, std::remainder(panIncDesired + _activeGimbal->absoluteYaw()->rawValue().toFloat(), 360.0f), false);
         } else {
             sendPitchBodyYaw(tiltDesired, panDesired, false);
         }

src/Gimbal/GimbalFact.json

@@ -24,6 +24,13 @@
     "decimalPlaces":    1,
     "units":            "deg"
 },
+{
+    "name":             "gimbalDeltaYaw",
+    "shortDesc":        "Gimbal Delta Yaw",
+    "type":             "float",
+    "decimalPlaces":    1,
+    "units":            "deg"
+},
 {
     "name":             "gimbalAzimuth",
     "shortDesc":        "Azimuth",