From db54dd9f46ce873202e24cf541015aa358119276 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Thu, 25 Jul 2024 09:03:45 +1200 Subject: [PATCH 1/5] Gimbal: ignore invalid gimbal_device_id Otherwise, we potentially work with garbage. --- src/Gimbal/GimbalController.cc | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/src/Gimbal/GimbalController.cc b/src/Gimbal/GimbalController.cc index cdb8853e508..7ab92ff6faf 100644 --- a/src/Gimbal/GimbalController.cc +++ b/src/Gimbal/GimbalController.cc @@ -236,6 +236,11 @@ GimbalController::_handleGimbalDeviceAttitudeStatus(const mavlink_message_t& mes // If the gimbal_device_id field is set to 1-6, we must use this device id instead } else if (attitude_status.gimbal_device_id <= 6) { gimbal_device_id_or_compid = attitude_status.gimbal_device_id; + + // Otherwise, this is invalid and we don't know how to deal with it. + } else { + qCDebug(GimbalLog) << "_handleGimbalDeviceAttitudeStatus for invalid device id: " << attitude_status.gimbal_device_id << " from component id: " << message.compid; + return; } // We do a reverse lookup here From 46a337a06958be1bc4e3cf74316ecc2c1b49bdd9 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Fri, 26 Jul 2024 13:34:05 +1200 Subject: [PATCH 2/5] Gimbal: Reference gimbal with manager + device IDs This changes how the found gimbals are referenced. Instead of only using the gimbal device ID for the gimbal map, we now also use the gimbal manager compid. This assumes that it is valid to have more than one gimbal manager with non-MAVLink gimbals attached, which means the gimbal_device_id would clash in that case, e.g. both would be 1. Therefore, we use the gimbal manager compid as well as the associated gimbal_device_id as the map key. --- src/Gimbal/GimbalController.cc | 120 +++++++++++++++++++++------------ src/Gimbal/GimbalController.h | 52 ++++++++++++-- 2 files changed, 122 insertions(+), 50 deletions(-) diff --git a/src/Gimbal/GimbalController.cc b/src/Gimbal/GimbalController.cc index 7ab92ff6faf..731d9786176 100644 --- a/src/Gimbal/GimbalController.cc +++ b/src/Gimbal/GimbalController.cc @@ -16,6 +16,7 @@ const char* Gimbal::_absolutePitchFactName = "gimbalPitch"; const char* Gimbal::_bodyYawFactName = "gimbalYaw"; const char* Gimbal::_absoluteYawFactName = "gimbalAzimuth"; const char* Gimbal::_deviceIdFactName = "deviceId"; +const char* Gimbal::_managerCompidFactName = "managerCompid"; Gimbal::Gimbal() : FactGroup(100, ":/json/Vehicle/GimbalFact.json") // No need to set parent as this will be deleted by gimbalController destructor @@ -63,18 +64,21 @@ void Gimbal::_initFacts() _bodyYawFact = Fact(0, _bodyYawFactName, FactMetaData::valueTypeFloat); _absoluteYawFact = Fact(0, _absoluteYawFactName, FactMetaData::valueTypeFloat); _deviceIdFact = Fact(0, _deviceIdFactName, FactMetaData::valueTypeUint8); + _managerCompidFact = Fact(0, _managerCompidFactName, FactMetaData::valueTypeUint8); _addFact(&_absoluteRollFact, _absoluteRollFactName); _addFact(&_absolutePitchFact, _absolutePitchFactName); _addFact(&_bodyYawFact, _bodyYawFactName); _addFact(&_absoluteYawFact, _absoluteYawFactName); _addFact(&_deviceIdFact, _deviceIdFactName); + _addFact(&_managerCompidFact, _managerCompidFactName); _absoluteRollFact.setRawValue (0.0f); _absolutePitchFact.setRawValue (0.0f); _bodyYawFact.setRawValue (0.0f); _absoluteYawFact.setRawValue (0.0f); _deviceIdFact.setRawValue (0); + _managerCompidFact.setRawValue (0); } GimbalController::GimbalController(MAVLinkProtocol* mavlink, Vehicle* vehicle) @@ -155,13 +159,19 @@ GimbalController::_handleGimbalManagerInformation(const mavlink_message_t& messa mavlink_gimbal_manager_information_t information; mavlink_msg_gimbal_manager_information_decode(&message, &information); + if (information.gimbal_device_id == 0) { + qCWarning(GimbalLog) << "_handleGimbalManagerInformation for invalid gimbal device: " + << information.gimbal_device_id << ", from component id: " << message.compid; + return; + } + qCDebug(GimbalLog) << "_handleGimbalManagerInformation for gimbal device: " << information.gimbal_device_id << ", component id: " << message.compid; - auto& gimbal = _potentialGimbals[information.gimbal_device_id]; + GimbalPairId pairId{message.compid, information.gimbal_device_id}; + auto& gimbal = _potentialGimbals[pairId]; - if (information.gimbal_device_id != 0) { - gimbal.setDeviceId(information.gimbal_device_id); - } + gimbal.setManagerCompid(message.compid); + gimbal.setDeviceId(information.gimbal_device_id); if (!gimbal._receivedInformation) { qCDebug(GimbalLog) << "gimbal manager with compId: " << message.compid @@ -173,27 +183,37 @@ GimbalController::_handleGimbalManagerInformation(const mavlink_message_t& messa auto& gimbalManager = _potentialGimbalManagers[message.compid]; gimbalManager.receivedInformation = true; - _checkComplete(gimbal, message.compid); + _checkComplete(gimbal, pairId); } void GimbalController::_handleGimbalManagerStatus(const mavlink_message_t& message) { - mavlink_gimbal_manager_status_t status; mavlink_msg_gimbal_manager_status_decode(&message, &status); // qCDebug(GimbalLog) << "_handleGimbalManagerStatus for gimbal device: " << status.gimbal_device_id << ", component id: " << message.compid; - auto& gimbal = _potentialGimbals[status.gimbal_device_id]; - - if (!status.gimbal_device_id) { + if (status.gimbal_device_id == 0) { qCDebug(GimbalLog) << "gimbal manager with compId: " << message.compid << " reported status of gimbal device id: " << status.gimbal_device_id << " which is not a valid gimbal device id"; return; } - gimbal.setDeviceId(status.gimbal_device_id); + GimbalPairId pairId{message.compid, status.gimbal_device_id}; + auto& gimbal = _potentialGimbals[pairId]; + + if (gimbal.deviceId()->rawValue().toUInt() == 0) { + gimbal.setDeviceId(status.gimbal_device_id); + } else if (gimbal.deviceId()->rawValue().toUInt() != status.gimbal_device_id) { + qCWarning(GimbalLog) << "conflicting GIMBAL_MANAGER_STATUS.gimbal_device_id: " << status.gimbal_device_id; + } + + if (gimbal.managerCompid()->rawValue().toUInt() == 0) { + gimbal.setManagerCompid(message.compid); + } else if (gimbal.managerCompid()->rawValue().toUInt() != message.compid) { + qCWarning(GimbalLog) << "conflicting GIMBAL_MANAGER_STATUS compid: " << message.compid; + } // Only log this message once if (!gimbal._receivedStatus) { @@ -218,7 +238,7 @@ GimbalController::_handleGimbalManagerStatus(const mavlink_message_t& message) gimbal.setGimbalOthersHaveControl(othersHaveControl); } - _checkComplete(gimbal, message.compid); + _checkComplete(gimbal, pairId); } void @@ -227,42 +247,49 @@ GimbalController::_handleGimbalDeviceAttitudeStatus(const mavlink_message_t& mes mavlink_gimbal_device_attitude_status_t attitude_status; mavlink_msg_gimbal_device_attitude_status_decode(&message, &attitude_status); - uint8_t gimbal_device_id_or_compid; + GimbalPairId pairId; // If gimbal_device_id is 0, we must take the compid of the message if (attitude_status.gimbal_device_id == 0) { - gimbal_device_id_or_compid = message.compid; + pairId.deviceId = message.compid; + + // We do a reverse lookup here + auto foundGimbal = std::find_if(_potentialGimbals.begin(), _potentialGimbals.end(), + [&](auto& gimbal) { return gimbal.deviceId()->rawValue().toUInt() == pairId.deviceId; }); + + if (foundGimbal == _potentialGimbals.end()) { + qCDebug(GimbalLog) << "_handleGimbalDeviceAttitudeStatus for unknown device id: " + << pairId.deviceId << " from component id: " << message.compid; + return; + } + + pairId.managerCompid = foundGimbal.key().managerCompid; // If the gimbal_device_id field is set to 1-6, we must use this device id instead } else if (attitude_status.gimbal_device_id <= 6) { - gimbal_device_id_or_compid = attitude_status.gimbal_device_id; + pairId.deviceId = attitude_status.gimbal_device_id; + pairId.managerCompid = message.compid; // Otherwise, this is invalid and we don't know how to deal with it. } else { - qCDebug(GimbalLog) << "_handleGimbalDeviceAttitudeStatus for invalid device id: " << attitude_status.gimbal_device_id << " from component id: " << message.compid; + qCDebug(GimbalLog) << "_handleGimbalDeviceAttitudeStatus for invalid device id: " + << attitude_status.gimbal_device_id << " from component id: " << message.compid; return; } - // We do a reverse lookup here - auto gimbal_it = std::find_if(_potentialGimbals.begin(), _potentialGimbals.end(), - [&](auto& gimbal) { return gimbal.deviceId()->rawValue().toUInt() == gimbal_device_id_or_compid; }); - - if (gimbal_it == _potentialGimbals.end()) { - qCDebug(GimbalLog) << "_handleGimbalDeviceAttitudeStatus for unknown device id: " << gimbal_device_id_or_compid << " from component id: " << message.compid; - return; - } + auto& gimbal = _potentialGimbals[pairId]; const bool yaw_in_vehicle_frame = _yawInVehicleFrame(attitude_status.flags); - gimbal_it->setRetracted((attitude_status.flags & GIMBAL_DEVICE_FLAGS_RETRACT) > 0); - gimbal_it->setYawLock((attitude_status.flags & GIMBAL_DEVICE_FLAGS_YAW_LOCK) > 0); - gimbal_it->_neutral = (attitude_status.flags & GIMBAL_DEVICE_FLAGS_NEUTRAL) > 0; + gimbal.setRetracted((attitude_status.flags & GIMBAL_DEVICE_FLAGS_RETRACT) > 0); + gimbal.setYawLock((attitude_status.flags & GIMBAL_DEVICE_FLAGS_YAW_LOCK) > 0); + gimbal._neutral = (attitude_status.flags & GIMBAL_DEVICE_FLAGS_NEUTRAL) > 0; float roll, pitch, yaw; mavlink_quaternion_to_euler(attitude_status.q, &roll, &pitch, &yaw); - gimbal_it->setAbsoluteRoll(qRadiansToDegrees(roll)); - gimbal_it->setAbsolutePitch(qRadiansToDegrees(pitch)); + gimbal.setAbsoluteRoll(qRadiansToDegrees(roll)); + gimbal.setAbsolutePitch(qRadiansToDegrees(pitch)); if (yaw_in_vehicle_frame) { float bodyYaw = qRadiansToDegrees(yaw); @@ -271,8 +298,8 @@ GimbalController::_handleGimbalDeviceAttitudeStatus(const mavlink_message_t& mes absoluteYaw -= 360.0f; } - gimbal_it->setBodyYaw(bodyYaw); - gimbal_it->setAbsoluteYaw(absoluteYaw); + gimbal.setBodyYaw(bodyYaw); + gimbal.setAbsoluteYaw(absoluteYaw); } else { float absoluteYaw = qRadiansToDegrees(yaw); @@ -281,13 +308,13 @@ GimbalController::_handleGimbalDeviceAttitudeStatus(const mavlink_message_t& mes bodyYaw += 360.0f; } - gimbal_it->setBodyYaw(bodyYaw); - gimbal_it->setAbsoluteYaw(absoluteYaw); + gimbal.setBodyYaw(bodyYaw); + gimbal.setAbsoluteYaw(absoluteYaw); } - gimbal_it->_receivedAttitude = true; + gimbal._receivedAttitude = true; - _checkComplete(*gimbal_it, message.compid); + _checkComplete(gimbal, pairId); } void @@ -304,7 +331,7 @@ GimbalController::_requestGimbalInformation(uint8_t compid) } void -GimbalController::_checkComplete(Gimbal& gimbal, uint8_t compid) +GimbalController::_checkComplete(Gimbal& gimbal, GimbalPairId pairId) { if (gimbal._isComplete) { // Already complete, nothing to do. @@ -312,30 +339,37 @@ GimbalController::_checkComplete(Gimbal& gimbal, uint8_t compid) } if (!gimbal._receivedInformation && gimbal._requestInformationRetries > 0) { - _requestGimbalInformation(compid); + _requestGimbalInformation(pairId.managerCompid); --gimbal._requestInformationRetries; } - // Limit to 1 second between set message interfacl requests + // Limit to 1 second between set message interface requests static qint64 lastRequestStatusMessage = 0; qint64 now = QDateTime::currentMSecsSinceEpoch(); if (!gimbal._receivedStatus && gimbal._requestStatusRetries > 0 && now - lastRequestStatusMessage > 1000) { lastRequestStatusMessage = now; - _vehicle->sendMavCommand(compid, + _vehicle->sendMavCommand(pairId.managerCompid, MAV_CMD_SET_MESSAGE_INTERVAL, false /* no error */, MAVLINK_MSG_ID_GIMBAL_MANAGER_STATUS, (gimbal._requestStatusRetries > 2) ? 0 : 5000000); // request default rate, if we don't succeed, last attempt is fixed 0.2 Hz instead --gimbal._requestStatusRetries; - qCDebug(GimbalLog) << "attempt to set GIMBAL_MANAGER_STATUS message at" << (gimbal._requestStatusRetries > 2 ? "default rate" : "0.2 Hz") << "interval for device: " - << gimbal.deviceId()->rawValue().toUInt() << "compID: " << compid << ", retries remaining: " << gimbal._requestStatusRetries; + qCDebug(GimbalLog) << "attempt to set GIMBAL_MANAGER_STATUS message at" + << (gimbal._requestStatusRetries > 2 ? "default rate" : "0.2 Hz") << " interval for device: " + << gimbal.deviceId()->rawValue().toUInt() << "manager compID: " << pairId.managerCompid + << ", retries remaining: " << gimbal._requestStatusRetries; } if (!gimbal._receivedAttitude && gimbal._requestAttitudeRetries > 0 && - gimbal._receivedInformation && gimbal.deviceId()->rawValue().toUInt() != 0) { + gimbal._receivedInformation && pairId.deviceId != 0) { // We request the attitude directly from the gimbal device component. // We can only do that once we have received the gimbal manager information // telling us which gimbal device it is responsible for. - _vehicle->sendMavCommand(gimbal.deviceId()->rawValue().toUInt(), + uint8_t gimbalDeviceCompid = pairId.deviceId; + // If the device ID is 1-6, we need to request the message from the manager itself. + if (gimbalDeviceCompid <= 6) { + gimbalDeviceCompid = pairId.managerCompid; + } + _vehicle->sendMavCommand(gimbalDeviceCompid, MAV_CMD_SET_MESSAGE_INTERVAL, false /* no error */, MAVLINK_MSG_ID_GIMBAL_DEVICE_ATTITUDE_STATUS, @@ -358,7 +392,7 @@ GimbalController::_checkComplete(Gimbal& gimbal, uint8_t compid) _gimbals.append(&gimbal); // This is needed for new Gimbals telemetry to be available for the user to show in flyview telemetry panel - _vehicle->_addFactGroup(&gimbal, QStringLiteral("%1%2").arg(_gimbalFactGroupNamePrefix).arg(gimbal.deviceId()->rawValue().toUInt())); + _vehicle->_addFactGroup(&gimbal, QStringLiteral("%1%2%3").arg(_gimbalFactGroupNamePrefix).arg(pairId.managerCompid).arg(pairId.deviceId)); } bool GimbalController::_tryGetGimbalControl() diff --git a/src/Gimbal/GimbalController.h b/src/Gimbal/GimbalController.h index 33bf769c4c2..949abe6b129 100644 --- a/src/Gimbal/GimbalController.h +++ b/src/Gimbal/GimbalController.h @@ -4,6 +4,7 @@ #pragma once #include +#include #include "Vehicle.h" #include "QmlObjectListModel.h" @@ -27,6 +28,7 @@ class Gimbal : public FactGroup Q_PROPERTY(Fact* bodyYaw READ bodyYaw CONSTANT) Q_PROPERTY(Fact* absoluteYaw READ absoluteYaw CONSTANT) Q_PROPERTY(Fact* deviceId READ deviceId CONSTANT) + Q_PROPERTY(Fact* managerCompid READ managerCompid CONSTANT) Q_PROPERTY(bool yawLock READ yawLock NOTIFY yawLockChanged) Q_PROPERTY(bool retracted READ retracted NOTIFY retractedChanged) Q_PROPERTY(bool gimbalHaveControl READ gimbalHaveControl NOTIFY gimbalHaveControlChanged) @@ -37,6 +39,7 @@ class Gimbal : public FactGroup Fact* bodyYaw() { return &_bodyYawFact; } Fact* absoluteYaw() { return &_absoluteYawFact; } Fact* deviceId() { return &_deviceIdFact; } + Fact* managerCompid() { return &_managerCompidFact; } bool yawLock() const { return _yawLock; } bool retracted() const { return _retracted; } bool gimbalHaveControl() const { return _haveControl; } @@ -47,8 +50,9 @@ class Gimbal : public FactGroup void setBodyYaw(float bodyYaw) { _bodyYawFact.setRawValue(bodyYaw); } void setAbsoluteYaw(float absoluteYaw) { _absoluteYawFact.setRawValue(absoluteYaw); } void setDeviceId(uint id) { _deviceIdFact.setRawValue(id); } + void setManagerCompid(uint id) { _managerCompidFact.setRawValue(id); } void setYawLock(bool yawLock) { _yawLock = yawLock; emit yawLockChanged(); } - void setRetracted(bool retracted) { _retracted = retracted; emit retractedChanged(); } + void setRetracted(bool retracted) { _retracted = retracted; emit retractedChanged(); } void setGimbalHaveControl(bool set) { _haveControl = set; emit gimbalHaveControlChanged(); } void setGimbalOthersHaveControl(bool set) { _othersHaveControl = set; emit gimbalOthersHaveControlChanged(); } @@ -78,6 +82,7 @@ class Gimbal : public FactGroup Fact _bodyYawFact; Fact _absoluteYawFact; Fact _deviceIdFact; // Component ID of gimbal device (or 1-6 for non-MAVLink gimbal) + Fact _managerCompidFact; bool _yawLock = false; bool _retracted = false; bool _haveControl = false; @@ -89,6 +94,7 @@ class Gimbal : public FactGroup static const char* _bodyYawFactName; static const char* _absoluteYawFactName; static const char* _deviceIdFactName; + static const char* _managerCompidFactName; }; class GimbalController : public QObject @@ -98,12 +104,43 @@ class GimbalController : public QObject GimbalController(MAVLinkProtocol* mavlink, Vehicle* vehicle); ~GimbalController(); - class GimbalManager { + class PotentialGimbalManager { public: unsigned requestGimbalManagerInformationRetries = 6; bool receivedInformation = false; }; + class GimbalPairId { + public: + uint8_t managerCompid {0}; + uint8_t deviceId {0}; + + GimbalPairId() = default; + GimbalPairId(uint8_t _managerCompid, uint8_t _deviceId) : + managerCompid(_managerCompid), + deviceId(_deviceId) {} + + // In order to use this as a key, we need to implement <, + bool operator<(const GimbalPairId& other) const { + // We compare managerCompid primarily, if they are equal, we compare the deviceId + if (managerCompid < other.managerCompid) { + return true; + } else if (managerCompid > other.managerCompid) { + return false; + } else { + if (deviceId < other.deviceId) { + return true; + } else { + return false; + } + } + } + + bool operator=(const GimbalPairId& other) const { + return (managerCompid == other.managerCompid) && (deviceId == other.deviceId); + } + }; + Q_PROPERTY(Gimbal* activeGimbal READ activeGimbal WRITE setActiveGimbal NOTIFY activeGimbalChanged) Q_PROPERTY(QmlObjectListModel* gimbals READ gimbals CONSTANT) @@ -141,17 +178,18 @@ private slots: void _handleGimbalManagerInformation (const mavlink_message_t& message); void _handleGimbalManagerStatus (const mavlink_message_t& message); void _handleGimbalDeviceAttitudeStatus (const mavlink_message_t& message); - void _checkComplete (Gimbal& gimbal, uint8_t compid); + void _checkComplete (Gimbal& gimbal, GimbalPairId pairId); bool _tryGetGimbalControl (); bool _yawInVehicleFrame (uint32_t flags); - + MAVLinkProtocol* _mavlink = nullptr; Vehicle* _vehicle = nullptr; Gimbal* _activeGimbal = nullptr; - QMap _potentialGimbalManagers; - QMap _potentialGimbals; + QMap _potentialGimbalManagers; // key is compid + + QMap _potentialGimbals; QmlObjectListModel _gimbals; - + static const char* _gimbalFactGroupNamePrefix; }; From ddeb2b59fbe1e8f0cca2a931f1f5f46bd8d6233f Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Fri, 26 Jul 2024 13:36:51 +1200 Subject: [PATCH 3/5] Gimbal: fix yaw calculation We should use the new yaw value, not the previous one when calculating the missing frame. --- src/Gimbal/GimbalController.cc | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/Gimbal/GimbalController.cc b/src/Gimbal/GimbalController.cc index 731d9786176..c1c6412d02d 100644 --- a/src/Gimbal/GimbalController.cc +++ b/src/Gimbal/GimbalController.cc @@ -293,7 +293,7 @@ GimbalController::_handleGimbalDeviceAttitudeStatus(const mavlink_message_t& mes if (yaw_in_vehicle_frame) { float bodyYaw = qRadiansToDegrees(yaw); - float absoluteYaw = gimbal_it->bodyYaw()->rawValue().toFloat() + _vehicle->heading()->rawValue().toFloat(); + float absoluteYaw = bodyYaw + _vehicle->heading()->rawValue().toFloat(); if (absoluteYaw > 180.0f) { absoluteYaw -= 360.0f; } @@ -303,7 +303,7 @@ GimbalController::_handleGimbalDeviceAttitudeStatus(const mavlink_message_t& mes } else { float absoluteYaw = qRadiansToDegrees(yaw); - float bodyYaw = gimbal_it->bodyYaw()->rawValue().toFloat() - _vehicle->heading()->rawValue().toFloat(); + float bodyYaw = absoluteYaw - _vehicle->heading()->rawValue().toFloat(); if (bodyYaw < 180.0f) { bodyYaw += 360.0f; } From ca7a3b6cc288de1c4781730e6a773710da30fa85 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Fri, 26 Jul 2024 13:58:32 +1200 Subject: [PATCH 4/5] Gimbal: send commands to gimbal component We shouldn't just send the commands to the vehicle because the gimbal manager might be implemented on any component, not just the autopilot. --- src/Gimbal/GimbalController.cc | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/src/Gimbal/GimbalController.cc b/src/Gimbal/GimbalController.cc index c1c6412d02d..622b09e3995 100644 --- a/src/Gimbal/GimbalController.cc +++ b/src/Gimbal/GimbalController.cc @@ -520,7 +520,7 @@ void GimbalController::sendPitchBodyYaw(float pitch, float yaw, bool showError) | GIMBAL_MANAGER_FLAGS_YAW_IN_VEHICLE_FRAME; _vehicle->sendMavCommand( - _vehicle->compId(), + _activeGimbal->managerCompid()->rawValue().toUInt(), MAV_CMD_DO_GIMBAL_MANAGER_PITCHYAW, showError, pitch, @@ -553,7 +553,7 @@ void GimbalController::sendPitchAbsoluteYaw(float pitch, float yaw, bool showErr | GIMBAL_MANAGER_FLAGS_YAW_IN_EARTH_FRAME; _vehicle->sendMavCommand( - _vehicle->compId(), + _activeGimbal->managerCompid()->rawValue().toUInt(), MAV_CMD_DO_GIMBAL_MANAGER_PITCHYAW, showError, pitch, @@ -601,7 +601,7 @@ void GimbalController::sendPitchYawFlags(uint32_t flags) const bool yaw_in_vehicle_frame = _yawInVehicleFrame(flags); _vehicle->sendMavCommand( - _vehicle->compId(), + _activeGimbal->managerCompid()->rawValue().toUInt(), MAV_CMD_DO_GIMBAL_MANAGER_PITCHYAW, true, _activeGimbal->absolutePitch()->rawValue().toFloat(), @@ -620,7 +620,7 @@ void GimbalController::acquireGimbalControl() return; } _vehicle->sendMavCommand( - _vehicle->compId(), + _activeGimbal->managerCompid()->rawValue().toUInt(), MAV_CMD_DO_GIMBAL_MANAGER_CONFIGURE, true, _mavlink->getSystemId(), // Set us in primary control. @@ -639,7 +639,7 @@ void GimbalController::releaseGimbalControl() return; } _vehicle->sendMavCommand( - _vehicle->compId(), + _activeGimbal->managerCompid()->rawValue().toUInt(), MAV_CMD_DO_GIMBAL_MANAGER_CONFIGURE, true, -3.f, // Release primary control if we have control From cee8f6aedece940dfbc8136ff560f1fc0531c585 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Tue, 30 Jul 2024 05:46:17 +1200 Subject: [PATCH 5/5] Gimbal: fix operator== --- src/Gimbal/GimbalController.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/Gimbal/GimbalController.h b/src/Gimbal/GimbalController.h index 949abe6b129..97869be7e8b 100644 --- a/src/Gimbal/GimbalController.h +++ b/src/Gimbal/GimbalController.h @@ -136,7 +136,7 @@ class GimbalController : public QObject } } - bool operator=(const GimbalPairId& other) const { + bool operator==(const GimbalPairId& other) const { return (managerCompid == other.managerCompid) && (deviceId == other.deviceId); } };