zhaohuwei
/
QG4
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

[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==
QGC4.4
Julian Oes 7 months ago committed by GitHub
parent
08d6c050c2
commit
70daedbc60
No known key found for this signature in database
GPG Key ID: B5690EEEBB952194
2 changed files with 132 additions and 55 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Unified View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 131
      src/Gimbal/GimbalController.cc
  2. 46
      src/Gimbal/GimbalController.h

131
src/Gimbal/GimbalController.cc
Unescape View File

@ -16,6 +16,7 @@ const char* Gimbal::_absolutePitchFactName = "gimbalPitch";
const char* Gimbal::_bodyYawFactName = "gimbalYaw"; const char* Gimbal::_bodyYawFactName = "gimbalYaw";
const char* Gimbal::_absoluteYawFactName = "gimbalAzimuth"; const char* Gimbal::_absoluteYawFactName = "gimbalAzimuth";
const char* Gimbal::_deviceIdFactName = "deviceId"; const char* Gimbal::_deviceIdFactName = "deviceId";
const char* Gimbal::_managerCompidFactName = "managerCompid";
Gimbal::Gimbal() Gimbal::Gimbal()
: FactGroup(100, ":/json/Vehicle/GimbalFact.json") // No need to set parent as this will be deleted by gimbalController destructor : 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); _bodyYawFact = Fact(0, _bodyYawFactName, FactMetaData::valueTypeFloat);
_absoluteYawFact = Fact(0, _absoluteYawFactName, FactMetaData::valueTypeFloat); _absoluteYawFact = Fact(0, _absoluteYawFactName, FactMetaData::valueTypeFloat);
_deviceIdFact = Fact(0, _deviceIdFactName, FactMetaData::valueTypeUint8); _deviceIdFact = Fact(0, _deviceIdFactName, FactMetaData::valueTypeUint8);
_managerCompidFact = Fact(0, _managerCompidFactName, FactMetaData::valueTypeUint8);
_addFact(&_absoluteRollFact, _absoluteRollFactName); _addFact(&_absoluteRollFact, _absoluteRollFactName);
_addFact(&_absolutePitchFact, _absolutePitchFactName); _addFact(&_absolutePitchFact, _absolutePitchFactName);
_addFact(&_bodyYawFact, _bodyYawFactName); _addFact(&_bodyYawFact, _bodyYawFactName);
_addFact(&_absoluteYawFact, _absoluteYawFactName); _addFact(&_absoluteYawFact, _absoluteYawFactName);
_addFact(&_deviceIdFact, _deviceIdFactName); _addFact(&_deviceIdFact, _deviceIdFactName);
_addFact(&_managerCompidFact, _managerCompidFactName);
_absoluteRollFact.setRawValue (0.0f); _absoluteRollFact.setRawValue (0.0f);
_absolutePitchFact.setRawValue (0.0f); _absolutePitchFact.setRawValue (0.0f);
_bodyYawFact.setRawValue (0.0f); _bodyYawFact.setRawValue (0.0f);
_absoluteYawFact.setRawValue (0.0f); _absoluteYawFact.setRawValue (0.0f);
_deviceIdFact.setRawValue (0); _deviceIdFact.setRawValue (0);
_managerCompidFact.setRawValue (0);
} }
GimbalController::GimbalController(MAVLinkProtocol* mavlink, Vehicle* vehicle) GimbalController::GimbalController(MAVLinkProtocol* mavlink, Vehicle* vehicle)
@ -155,13 +159,19 @@ GimbalController::_handleGimbalManagerInformation(const mavlink_message_t& messa
mavlink_gimbal_manager_information_t information; mavlink_gimbal_manager_information_t information;
mavlink_msg_gimbal_manager_information_decode(&message, &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; 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.setManagerCompid(message.compid);
gimbal.setDeviceId(information.gimbal_device_id); gimbal.setDeviceId(information.gimbal_device_id);
}
if (!gimbal._receivedInformation) { if (!gimbal._receivedInformation) {
qCDebug(GimbalLog) << "gimbal manager with compId: " << message.compid qCDebug(GimbalLog) << "gimbal manager with compId: " << message.compid
@ -173,27 +183,37 @@ GimbalController::_handleGimbalManagerInformation(const mavlink_message_t& messa
auto& gimbalManager = _potentialGimbalManagers[message.compid]; auto& gimbalManager = _potentialGimbalManagers[message.compid];
gimbalManager.receivedInformation = true; gimbalManager.receivedInformation = true;
_checkComplete(gimbal, message.compid); _checkComplete(gimbal, pairId);
} }
void void
GimbalController::_handleGimbalManagerStatus(const mavlink_message_t& message) GimbalController::_handleGimbalManagerStatus(const mavlink_message_t& message)
{ {
mavlink_gimbal_manager_status_t status; mavlink_gimbal_manager_status_t status;
mavlink_msg_gimbal_manager_status_decode(&message, &status); mavlink_msg_gimbal_manager_status_decode(&message, &status);
// qCDebug(GimbalLog) << "_handleGimbalManagerStatus for gimbal device: " << status.gimbal_device_id << ", component id: " << message.compid; // 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 == 0) {
if (!status.gimbal_device_id) {
qCDebug(GimbalLog) << "gimbal manager with compId: " << message.compid 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"; << " reported status of gimbal device id: " << status.gimbal_device_id << " which is not a valid gimbal device id";
return; return;
} }
GimbalPairId pairId{message.compid, status.gimbal_device_id};
auto& gimbal = _potentialGimbals[pairId];
if (gimbal.deviceId()->rawValue().toUInt() == 0) {
gimbal.setDeviceId(status.gimbal_device_id); 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 // Only log this message once
if (!gimbal._receivedStatus) { if (!gimbal._receivedStatus) {
@ -218,7 +238,7 @@ GimbalController::_handleGimbalManagerStatus(const mavlink_message_t& message)
gimbal.setGimbalOthersHaveControl(othersHaveControl); gimbal.setGimbalOthersHaveControl(othersHaveControl);
} }
_checkComplete(gimbal, message.compid); _checkComplete(gimbal, pairId);
} }
void void
@ -227,62 +247,74 @@ GimbalController::_handleGimbalDeviceAttitudeStatus(const mavlink_message_t& mes
mavlink_gimbal_device_attitude_status_t attitude_status; mavlink_gimbal_device_attitude_status_t attitude_status;
mavlink_msg_gimbal_device_attitude_status_decode(&message, &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 gimbal_device_id is 0, we must take the compid of the message
if (attitude_status.gimbal_device_id == 0) { if (attitude_status.gimbal_device_id == 0) {
gimbal_device_id_or_compid = message.compid; pairId.deviceId = message.compid;
// If the gimbal_device_id field is set to 1-6, we must use this device id instead // We do a reverse lookup here
} else if (attitude_status.gimbal_device_id <= 6) { auto foundGimbal = std::find_if(_potentialGimbals.begin(), _potentialGimbals.end(),
gimbal_device_id_or_compid = attitude_status.gimbal_device_id; [&](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;
} }
// We do a reverse lookup here pairId.managerCompid = foundGimbal.key().managerCompid;
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()) { // If the gimbal_device_id field is set to 1-6, we must use this device id instead
qCDebug(GimbalLog) << "_handleGimbalDeviceAttitudeStatus for unknown device id: " << gimbal_device_id_or_compid << " from component id: " << message.compid; } else if (attitude_status.gimbal_device_id <= 6) {
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;
return; return;
} }
auto& gimbal = _potentialGimbals[pairId];
const bool yaw_in_vehicle_frame = _yawInVehicleFrame(attitude_status.flags); const bool yaw_in_vehicle_frame = _yawInVehicleFrame(attitude_status.flags);
gimbal_it->setRetracted((attitude_status.flags & GIMBAL_DEVICE_FLAGS_RETRACT) > 0); gimbal.setRetracted((attitude_status.flags & GIMBAL_DEVICE_FLAGS_RETRACT) > 0);
gimbal_it->setYawLock((attitude_status.flags & GIMBAL_DEVICE_FLAGS_YAW_LOCK) > 0); gimbal.setYawLock((attitude_status.flags & GIMBAL_DEVICE_FLAGS_YAW_LOCK) > 0);
gimbal_it->_neutral = (attitude_status.flags & GIMBAL_DEVICE_FLAGS_NEUTRAL) > 0; gimbal._neutral = (attitude_status.flags & GIMBAL_DEVICE_FLAGS_NEUTRAL) > 0;
float roll, pitch, yaw; float roll, pitch, yaw;
mavlink_quaternion_to_euler(attitude_status.q, &roll, &pitch, &yaw); mavlink_quaternion_to_euler(attitude_status.q, &roll, &pitch, &yaw);
gimbal_it->setAbsoluteRoll(qRadiansToDegrees(roll)); gimbal.setAbsoluteRoll(qRadiansToDegrees(roll));
gimbal_it->setAbsolutePitch(qRadiansToDegrees(pitch)); gimbal.setAbsolutePitch(qRadiansToDegrees(pitch));
if (yaw_in_vehicle_frame) { if (yaw_in_vehicle_frame) {
float bodyYaw = qRadiansToDegrees(yaw); 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) { if (absoluteYaw > 180.0f) {
absoluteYaw -= 360.0f; absoluteYaw -= 360.0f;
} }
gimbal_it->setBodyYaw(bodyYaw); gimbal.setBodyYaw(bodyYaw);
gimbal_it->setAbsoluteYaw(absoluteYaw); gimbal.setAbsoluteYaw(absoluteYaw);
} else { } else {
float absoluteYaw = qRadiansToDegrees(yaw); 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) { if (bodyYaw < 180.0f) {
bodyYaw += 360.0f; bodyYaw += 360.0f;
} }
gimbal_it->setBodyYaw(bodyYaw); gimbal.setBodyYaw(bodyYaw);
gimbal_it->setAbsoluteYaw(absoluteYaw); gimbal.setAbsoluteYaw(absoluteYaw);
} }
gimbal_it->_receivedAttitude = true; gimbal._receivedAttitude = true;
_checkComplete(*gimbal_it, message.compid); _checkComplete(gimbal, pairId);
} }
void void
@ -299,7 +331,7 @@ GimbalController::_requestGimbalInformation(uint8_t compid)
} }
void void
GimbalController::_checkComplete(Gimbal& gimbal, uint8_t compid) GimbalController::_checkComplete(Gimbal& gimbal, GimbalPairId pairId)
{ {
if (gimbal._isComplete) { if (gimbal._isComplete) {
// Already complete, nothing to do. // Already complete, nothing to do.
@ -307,30 +339,37 @@ GimbalController::_checkComplete(Gimbal& gimbal, uint8_t compid)
} }
if (!gimbal._receivedInformation && gimbal._requestInformationRetries > 0) { if (!gimbal._receivedInformation && gimbal._requestInformationRetries > 0) {
_requestGimbalInformation(compid); _requestGimbalInformation(pairId.managerCompid);
--gimbal._requestInformationRetries; --gimbal._requestInformationRetries;
} }
// Limit to 1 second between set message interfacl requests // Limit to 1 second between set message interface requests
static qint64 lastRequestStatusMessage = 0; static qint64 lastRequestStatusMessage = 0;
qint64 now = QDateTime::currentMSecsSinceEpoch(); qint64 now = QDateTime::currentMSecsSinceEpoch();
if (!gimbal._receivedStatus && gimbal._requestStatusRetries > 0 && now - lastRequestStatusMessage > 1000) { if (!gimbal._receivedStatus && gimbal._requestStatusRetries > 0 && now - lastRequestStatusMessage > 1000) {
lastRequestStatusMessage = now; lastRequestStatusMessage = now;
_vehicle->sendMavCommand(compid, _vehicle->sendMavCommand(pairId.managerCompid,
MAV_CMD_SET_MESSAGE_INTERVAL, MAV_CMD_SET_MESSAGE_INTERVAL,
false /* no error */, false /* no error */,
MAVLINK_MSG_ID_GIMBAL_MANAGER_STATUS, 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 > 2) ? 0 : 5000000); // request default rate, if we don't succeed, last attempt is fixed 0.2 Hz instead
--gimbal._requestStatusRetries; --gimbal._requestStatusRetries;
qCDebug(GimbalLog) << "attempt to set GIMBAL_MANAGER_STATUS message at" << (gimbal._requestStatusRetries > 2 ? "default rate" : "0.2 Hz") << "interval for device: " qCDebug(GimbalLog) << "attempt to set GIMBAL_MANAGER_STATUS message at"
<< gimbal.deviceId()->rawValue().toUInt() << "compID: " << compid << ", retries remaining: " << gimbal._requestStatusRetries; << (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 && 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 request the attitude directly from the gimbal device component.
// We can only do that once we have received the gimbal manager information // We can only do that once we have received the gimbal manager information
// telling us which gimbal device it is responsible for. // 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, MAV_CMD_SET_MESSAGE_INTERVAL,
false /* no error */, false /* no error */,
MAVLINK_MSG_ID_GIMBAL_DEVICE_ATTITUDE_STATUS, MAVLINK_MSG_ID_GIMBAL_DEVICE_ATTITUDE_STATUS,
@ -353,7 +392,7 @@ GimbalController::_checkComplete(Gimbal& gimbal, uint8_t compid)
_gimbals.append(&gimbal); _gimbals.append(&gimbal);
// This is needed for new Gimbals telemetry to be available for the user to show in flyview telemetry panel // 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() bool GimbalController::_tryGetGimbalControl()
@ -481,7 +520,7 @@ void GimbalController::sendPitchBodyYaw(float pitch, float yaw, bool showError)
| GIMBAL_MANAGER_FLAGS_YAW_IN_VEHICLE_FRAME; | GIMBAL_MANAGER_FLAGS_YAW_IN_VEHICLE_FRAME;
_vehicle->sendMavCommand( _vehicle->sendMavCommand(
_vehicle->compId(), _activeGimbal->managerCompid()->rawValue().toUInt(),
MAV_CMD_DO_GIMBAL_MANAGER_PITCHYAW, MAV_CMD_DO_GIMBAL_MANAGER_PITCHYAW,
showError, showError,
pitch, pitch,
@ -514,7 +553,7 @@ void GimbalController::sendPitchAbsoluteYaw(float pitch, float yaw, bool showErr
| GIMBAL_MANAGER_FLAGS_YAW_IN_EARTH_FRAME; | GIMBAL_MANAGER_FLAGS_YAW_IN_EARTH_FRAME;
_vehicle->sendMavCommand( _vehicle->sendMavCommand(
_vehicle->compId(), _activeGimbal->managerCompid()->rawValue().toUInt(),
MAV_CMD_DO_GIMBAL_MANAGER_PITCHYAW, MAV_CMD_DO_GIMBAL_MANAGER_PITCHYAW,
showError, showError,
pitch, pitch,
@ -562,7 +601,7 @@ void GimbalController::sendPitchYawFlags(uint32_t flags)
const bool yaw_in_vehicle_frame = _yawInVehicleFrame(flags); const bool yaw_in_vehicle_frame = _yawInVehicleFrame(flags);
_vehicle->sendMavCommand( _vehicle->sendMavCommand(
_vehicle->compId(), _activeGimbal->managerCompid()->rawValue().toUInt(),
MAV_CMD_DO_GIMBAL_MANAGER_PITCHYAW, MAV_CMD_DO_GIMBAL_MANAGER_PITCHYAW,
true, true,
_activeGimbal->absolutePitch()->rawValue().toFloat(), _activeGimbal->absolutePitch()->rawValue().toFloat(),
@ -581,7 +620,7 @@ void GimbalController::acquireGimbalControl()
return; return;
} }
_vehicle->sendMavCommand( _vehicle->sendMavCommand(
_vehicle->compId(), _activeGimbal->managerCompid()->rawValue().toUInt(),
MAV_CMD_DO_GIMBAL_MANAGER_CONFIGURE, MAV_CMD_DO_GIMBAL_MANAGER_CONFIGURE,
true, true,
_mavlink->getSystemId(), // Set us in primary control. _mavlink->getSystemId(), // Set us in primary control.
@ -600,7 +639,7 @@ void GimbalController::releaseGimbalControl()
return; return;
} }
_vehicle->sendMavCommand( _vehicle->sendMavCommand(
_vehicle->compId(), _activeGimbal->managerCompid()->rawValue().toUInt(),
MAV_CMD_DO_GIMBAL_MANAGER_CONFIGURE, MAV_CMD_DO_GIMBAL_MANAGER_CONFIGURE,
true, true,
-3.f, // Release primary control if we have control -3.f, // Release primary control if we have control

46
src/Gimbal/GimbalController.h
Unescape View File

@ -4,6 +4,7 @@
#pragma once #pragma once
#include <QLoggingCategory> #include <QLoggingCategory>
#include <cstdint>
#include "Vehicle.h" #include "Vehicle.h"
#include "QmlObjectListModel.h" #include "QmlObjectListModel.h"
@ -27,6 +28,7 @@ public:
Q_PROPERTY(Fact* bodyYaw READ bodyYaw CONSTANT) Q_PROPERTY(Fact* bodyYaw READ bodyYaw CONSTANT)
Q_PROPERTY(Fact* absoluteYaw READ absoluteYaw CONSTANT) Q_PROPERTY(Fact* absoluteYaw READ absoluteYaw CONSTANT)
Q_PROPERTY(Fact* deviceId READ deviceId 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 yawLock READ yawLock NOTIFY yawLockChanged)
Q_PROPERTY(bool retracted READ retracted NOTIFY retractedChanged) Q_PROPERTY(bool retracted READ retracted NOTIFY retractedChanged)
Q_PROPERTY(bool gimbalHaveControl READ gimbalHaveControl NOTIFY gimbalHaveControlChanged) Q_PROPERTY(bool gimbalHaveControl READ gimbalHaveControl NOTIFY gimbalHaveControlChanged)
@ -37,6 +39,7 @@ public:
Fact* bodyYaw() { return &_bodyYawFact; } Fact* bodyYaw() { return &_bodyYawFact; }
Fact* absoluteYaw() { return &_absoluteYawFact; } Fact* absoluteYaw() { return &_absoluteYawFact; }
Fact* deviceId() { return &_deviceIdFact; } Fact* deviceId() { return &_deviceIdFact; }
Fact* managerCompid() { return &_managerCompidFact; }
bool yawLock() const { return _yawLock; } bool yawLock() const { return _yawLock; }
bool retracted() const { return _retracted; } bool retracted() const { return _retracted; }
bool gimbalHaveControl() const { return _haveControl; } bool gimbalHaveControl() const { return _haveControl; }
@ -47,6 +50,7 @@ public:
void setBodyYaw(float bodyYaw) { _bodyYawFact.setRawValue(bodyYaw); } void setBodyYaw(float bodyYaw) { _bodyYawFact.setRawValue(bodyYaw); }
void setAbsoluteYaw(float absoluteYaw) { _absoluteYawFact.setRawValue(absoluteYaw); } void setAbsoluteYaw(float absoluteYaw) { _absoluteYawFact.setRawValue(absoluteYaw); }
void setDeviceId(uint id) { _deviceIdFact.setRawValue(id); } void setDeviceId(uint id) { _deviceIdFact.setRawValue(id); }
void setManagerCompid(uint id) { _managerCompidFact.setRawValue(id); }
void setYawLock(bool yawLock) { _yawLock = yawLock; emit yawLockChanged(); } 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 setGimbalHaveControl(bool set) { _haveControl = set; emit gimbalHaveControlChanged(); }
@ -78,6 +82,7 @@ private:
Fact _bodyYawFact; Fact _bodyYawFact;
Fact _absoluteYawFact; Fact _absoluteYawFact;
Fact _deviceIdFact; // Component ID of gimbal device (or 1-6 for non-MAVLink gimbal) Fact _deviceIdFact; // Component ID of gimbal device (or 1-6 for non-MAVLink gimbal)
Fact _managerCompidFact;
bool _yawLock = false; bool _yawLock = false;
bool _retracted = false; bool _retracted = false;
bool _haveControl = false; bool _haveControl = false;
@ -89,6 +94,7 @@ private:
static const char* _bodyYawFactName; static const char* _bodyYawFactName;
static const char* _absoluteYawFactName; static const char* _absoluteYawFactName;
static const char* _deviceIdFactName; static const char* _deviceIdFactName;
static const char* _managerCompidFactName;
}; };
class GimbalController : public QObject class GimbalController : public QObject
@ -98,12 +104,43 @@ public:
GimbalController(MAVLinkProtocol* mavlink, Vehicle* vehicle); GimbalController(MAVLinkProtocol* mavlink, Vehicle* vehicle);
~GimbalController(); ~GimbalController();
class GimbalManager { class PotentialGimbalManager {
public: public:
unsigned requestGimbalManagerInformationRetries = 6; unsigned requestGimbalManagerInformationRetries = 6;
bool receivedInformation = false; 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(Gimbal* activeGimbal READ activeGimbal WRITE setActiveGimbal NOTIFY activeGimbalChanged)
Q_PROPERTY(QmlObjectListModel* gimbals READ gimbals CONSTANT) Q_PROPERTY(QmlObjectListModel* gimbals READ gimbals CONSTANT)
@ -141,7 +178,7 @@ private:
void _handleGimbalManagerInformation (const mavlink_message_t& message); void _handleGimbalManagerInformation (const mavlink_message_t& message);
void _handleGimbalManagerStatus (const mavlink_message_t& message); void _handleGimbalManagerStatus (const mavlink_message_t& message);
void _handleGimbalDeviceAttitudeStatus (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 _tryGetGimbalControl ();
bool _yawInVehicleFrame (uint32_t flags); bool _yawInVehicleFrame (uint32_t flags);
@ -149,8 +186,9 @@ private:
Vehicle* _vehicle = nullptr; Vehicle* _vehicle = nullptr;
Gimbal* _activeGimbal = nullptr; Gimbal* _activeGimbal = nullptr;
QMap<uint8_t, GimbalManager> _potentialGimbalManagers; QMap<uint8_t, PotentialGimbalManager> _potentialGimbalManagers; // key is compid
QMap<uint8_t, Gimbal> _potentialGimbals;
QMap<GimbalPairId, Gimbal> _potentialGimbals;
QmlObjectListModel _gimbals; QmlObjectListModel _gimbals;
static const char* _gimbalFactGroupNamePrefix; static const char* _gimbalFactGroupNamePrefix;

Write Preview
Loading…
Cancel
Save