diff --git a/src/Gimbal/GimbalController.cc b/src/Gimbal/GimbalController.cc index cdb8853e508..622b09e3995 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 33bf769c4c2..97869be7e8b 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; };