From 70daedbc60fb77482b97668b73357601485c61ec Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Fri, 2 Aug 2024 13:11:23 +1200 Subject: [PATCH] [v4.4] Gimbal fixes (#11706) * Gimbal: ignore invalid gimbal_device_id Otherwise, we potentially work with garbage. * 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. * Gimbal: fix yaw calculation We should use the new yaw value, not the previous one when calculating the missing frame. * 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. * Gimbal: fix operator== --- src/Gimbal/GimbalController.cc | 135 ++++++++++++++++++++++++++--------------- src/Gimbal/GimbalController.h | 52 +++++++++++++--- 2 files changed, 132 insertions(+), 55 deletions(-) diff --git a/src/Gimbal/GimbalController.cc b/src/Gimbal/GimbalController.cc index cdb8853..622b09e 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,62 +247,74 @@ 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; - // 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; + // 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; } + 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); - 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; } - gimbal_it->setBodyYaw(bodyYaw); - gimbal_it->setAbsoluteYaw(absoluteYaw); + gimbal.setBodyYaw(bodyYaw); + gimbal.setAbsoluteYaw(absoluteYaw); } 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; } - 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 @@ -299,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. @@ -307,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, @@ -353,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() @@ -481,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, @@ -514,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, @@ -562,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(), @@ -581,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. @@ -600,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 diff --git a/src/Gimbal/GimbalController.h b/src/Gimbal/GimbalController.h index 33bf769..97869be 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 @@ public: 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 @@ public: 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 @@ public: 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 @@ private: 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 @@ private: static const char* _bodyYawFactName; static const char* _absoluteYawFactName; static const char* _deviceIdFactName; + static const char* _managerCompidFactName; }; class GimbalController : public QObject @@ -98,12 +104,43 @@ public: 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: 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; };