diff --git a/src/Comms/MockLink/MockLinkGimbal.cc b/src/Comms/MockLink/MockLinkGimbal.cc index 4932c0c1d320..0f59e59bb6aa 100644 --- a/src/Comms/MockLink/MockLinkGimbal.cc +++ b/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); } diff --git a/src/Gimbal/Gimbal.cc b/src/Gimbal/Gimbal.cc index f9277c355313..8d0f36305d52 100644 --- a/src/Gimbal/Gimbal.cc +++ b/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,6 +56,7 @@ void Gimbal::_initFacts() _addFact(&_absolutePitchFact); _addFact(&_bodyYawFact); _addFact(&_absoluteYawFact); + _addFact(&_deltaYawFact); _addFact(&_deviceIdFact); _addFact(&_managerCompidFact); @@ -62,6 +64,7 @@ void Gimbal::_initFacts() _absolutePitchFact.setRawValue(0.0f); _bodyYawFact.setRawValue(0.0f); _absoluteYawFact.setRawValue(0.0f); + _deltaYawFact.setRawValue(qQNaN()); _deviceIdFact.setRawValue(0); _managerCompidFact.setRawValue(0); } diff --git a/src/Gimbal/Gimbal.h b/src/Gimbal/Gimbal.h index 944b1d034a40..d79aba93af9a 100644 --- a/src/Gimbal/Gimbal.h +++ b/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); diff --git a/src/Gimbal/GimbalController.cc b/src/Gimbal/GimbalController.cc index 6851f2741859..8e3eb0ba5685 100644 --- a/src/Gimbal/GimbalController.cc +++ b/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); } diff --git a/src/Gimbal/GimbalFact.json b/src/Gimbal/GimbalFact.json index 868b242b842a..fca9b5ddfd62 100644 --- a/src/Gimbal/GimbalFact.json +++ b/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",