Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

[v4.4] Gimbal fixes #11706

Merged
merged 5 commits into from
Aug 2, 2024
+132 −55
Merged

[v4.4] Gimbal fixes #11706

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
5 commits
Select commit Hold shift + click to select a range
db54dd9
Gimbal: ignore invalid gimbal_device_id
julianoes Jul 24, 2024
46a337a
Gimbal: Reference gimbal with manager + device IDs
julianoes Jul 26, 2024
ddeb2b5
Gimbal: fix yaw calculation
julianoes Jul 26, 2024
ca7a3b6
Gimbal: send commands to gimbal component
julianoes Jul 26, 2024
cee8f6a
Gimbal: fix operator==
julianoes Jul 29, 2024
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
135 changes: 87 additions & 48 deletions src/Gimbal/GimbalController.cc
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down Expand Up @@ -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)
Expand Down Expand Up @@ -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
Expand All @@ -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) {
Expand All @@ -218,7 +238,7 @@ GimbalController::_handleGimbalManagerStatus(const mavlink_message_t& message)
gimbal.setGimbalOthersHaveControl(othersHaveControl);
}

_checkComplete(gimbal, message.compid);
_checkComplete(gimbal, pairId);
}

void
Expand All @@ -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
Expand All @@ -299,38 +331,45 @@ 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.
return;
}

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,
Expand All @@ -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()
Expand Down Expand Up @@ -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,
Expand Down Expand Up @@ -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,
Expand Down Expand Up @@ -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(),
Expand All @@ -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.
Expand All @@ -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
Expand Down
Loading
Loading