Skip to content

Commit

Permalink
Gimbal Controller rework, use gimbal_device_id instead of compid, see…
Browse files Browse the repository at this point in the history 
… long notes:

Implementation was based on the assumption that all the gimbal managers would
have a different compId, which is not always the case. As per mavlink documentation:

https://mavlink.io/en/services/gimbal_v2.html

Discovery should be based on GCS sending a MAV_CMD_REQUEST_MESSAGE for GIMBAL_MANAGER_INFORMATION
and we should instantiate gimbals when we receive response to this message.

This commit introduces several modifications toi allow this.

This was needed because otherwise the existent implementation would not allow multi gimbal
systems with gimbals belonging to the same compid to work correctly
  • Loading branch information
@Davidsastresas
Davidsastresas committed Jul 20, 2023
1 parent 60ef91d commit 7221c21
Show file tree
Hide file tree
Showing 2 changed files with 97 additions and 59 deletions.
144 changes: 87 additions & 57 deletions src/Gimbal/GimbalController.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -3,8 +3,7 @@
#include "MAVLinkProtocol.h"
#include "Vehicle.h"

QGC_LOGGING_CATEGORY(GimbalLog, "GimbalLog")

QGC_LOGGING_CATEGORY(GimbalLog, "GimbalLog")

GimbalController::GimbalController(MAVLinkProtocol* mavlink, Vehicle* vehicle)
: _mavlink(mavlink)
Expand Down Expand Up @@ -36,80 +35,70 @@ GimbalController::_mavlinkMessageReceived(const mavlink_message_t& message)
void
GimbalController::_handleHeartbeat(const mavlink_message_t& message)
{
if (!_potentialGimbals.contains(message.compid)) {
qCDebug(GimbalLog) << "new potential gimbal component: " << message.compid;
if (!_potentialGimbalManagers.contains(message.compid)) {
qCDebug(GimbalLog) << "new potential gimbal manager component: " << message.compid;
}

auto& gimbal = _potentialGimbals[message.compid];
auto& gimbalManager = _potentialGimbalManagers[message.compid];

if (!gimbal.receivedInformation && gimbal.requestInformationRetries > 0) {
if (!gimbalManager.receivedInformation && gimbalManager.requestGimbalManagerInformationRetries > 0) {
_requestGimbalInformation(message.compid);
--gimbal.requestInformationRetries;
}

if (!gimbal.receivedStatus && gimbal.requestStatusRetries > 0) {
_vehicle->sendMavCommand(message.compid,
MAV_CMD_SET_MESSAGE_INTERVAL,
false /* no error */,
MAVLINK_MSG_ID_GIMBAL_MANAGER_STATUS,
(gimbal.requestStatusRetries > 1) ? 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 interval for compID " << message.compid << ", retries remaining: " << gimbal.requestStatusRetries;
}

if (!gimbal.receivedAttitude && gimbal.requestAttitudeRetries > 0 &&
gimbal.receivedInformation && gimbal.responsibleCompid != 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.responsibleCompid,
MAV_CMD_SET_MESSAGE_INTERVAL,
false /* no error */,
MAVLINK_MSG_ID_GIMBAL_DEVICE_ATTITUDE_STATUS,
0 /* request default rate */);

--gimbal.requestAttitudeRetries;
--gimbalManager.requestGimbalManagerInformationRetries;
}
}

void
GimbalController::_handleGimbalManagerInformation(const mavlink_message_t& message)
{
qCDebug(GimbalLog) << "_handleGimbalManagerInformation for component " << message.compid;

auto& gimbal = _potentialGimbals[message.compid];

mavlink_gimbal_manager_information_t information;
mavlink_msg_gimbal_manager_information_decode(&message, &information);

gimbal.receivedInformation = true;
qCDebug(GimbalLog) << "_handleGimbalManagerInformation for gimbal device: " << information.gimbal_device_id << ", component id: " << message.compid;

auto& gimbal = _potentialGimbals[information.gimbal_device_id];

if (information.gimbal_device_id != 0) {
gimbal.responsibleCompid = information.gimbal_device_id;
qCDebug(GimbalLog) << "gimbal manager with compId " << message.compid
<< " is responsible for gimbal device " << information.gimbal_device_id;
gimbal.deviceId = information.gimbal_device_id;
}

if (!gimbal.receivedInformation) {
qCDebug(GimbalLog) << "gimbal manager with compId: " << message.compid
<< " is responsible for gimbal device: " << information.gimbal_device_id;
}

_checkComplete(gimbal);
gimbal.receivedInformation = true;

_checkComplete(gimbal, message.compid);
}

void
GimbalController::_handleGimbalManagerStatus(const mavlink_message_t& message)
{
qCDebug(GimbalLog) << "_handleGimbalManagerStatus for component " << message.compid;

auto& gimbal = _potentialGimbals[message.compid];

mavlink_gimbal_manager_status_t status;
mavlink_msg_gimbal_manager_status_decode(&message, &status);

gimbal.receivedStatus = true;
// 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) {
gimbal.responsibleCompid = status.gimbal_device_id;
qCDebug(GimbalLog) << "gimbal manager with compId " << message.compid
if (!status.gimbal_device_id) {
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.deviceId = status.gimbal_device_id;

// Only log this message once
if (!gimbal.receivedStatus) {
qCDebug(GimbalLog) << "_handleGimbalManagerStatus: gimbal manager with compId " << message.compid
<< " is responsible for gimbal device " << status.gimbal_device_id;
}

gimbal.receivedStatus = true;

const bool haveControl =
(status.primary_control_sysid == _mavlink->getSystemId()) &&
(status.primary_control_compid == _mavlink->getComponentId());
Expand All @@ -133,25 +122,35 @@ GimbalController::_handleGimbalManagerStatus(const mavlink_message_t& message)
//emit gimbalsChanged();
}

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

void
GimbalController::_handleGimbalDeviceAttitudeStatus(const mavlink_message_t& message)
{
// We do a reverse lookup here because the gimbal device might have a
// different component ID than the gimbal manager.
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;

// 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;

// 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;
}

// We do a reverse lookup here
auto gimbal_it = std::find_if(_potentialGimbals.begin(), _potentialGimbals.end(),
[&](auto& gimbal) { return gimbal.responsibleCompid == message.compid; });
[&](auto& gimbal) { return gimbal.deviceId == gimbal_device_id_or_compid; });

if (gimbal_it == _potentialGimbals.end()) {
qCDebug(GimbalLog) << "_handleGimbalDeviceAttitudeStatus for unknown device with component " << message.compid;
qCDebug(GimbalLog) << "_handleGimbalDeviceAttitudeStatus for unknown device id: " << gimbal_device_id_or_compid << " from component id: " << message.compid;
return;
}

mavlink_gimbal_device_attitude_status_t attitude_status;
mavlink_msg_gimbal_device_attitude_status_decode(&message, &attitude_status);

gimbal_it->retracted = (attitude_status.flags & GIMBAL_MANAGER_FLAGS_RETRACT) > 0;
gimbal_it->neutral = (attitude_status.flags & GIMBAL_MANAGER_FLAGS_NEUTRAL) > 0;
gimbal_it->yawLock = (attitude_status.flags & GIMBAL_MANAGER_FLAGS_YAW_LOCK) > 0;
Expand All @@ -166,7 +165,7 @@ GimbalController::_handleGimbalDeviceAttitudeStatus(const mavlink_message_t& mes

gimbal_it->receivedAttitude = true;

_checkComplete(*gimbal_it);
_checkComplete(*gimbal_it, message.compid);
}

void
Expand All @@ -182,13 +181,44 @@ GimbalController::_requestGimbalInformation(uint8_t compid)
}
}

void GimbalController::_checkComplete(Gimbal& gimbal)
void
GimbalController::_checkComplete(Gimbal& gimbal, uint8_t compid)
{
if (gimbal.isComplete) {
// Already complete, nothing to do.
return;
}

if (!gimbal.receivedInformation && gimbal.requestInformationRetries > 0) {
_requestGimbalInformation(compid);
--gimbal.requestInformationRetries;
}

if (!gimbal.receivedStatus && gimbal.requestStatusRetries > 0) {
_vehicle->sendMavCommand(compid,
MAV_CMD_SET_MESSAGE_INTERVAL,
false /* no error */,
MAVLINK_MSG_ID_GIMBAL_MANAGER_STATUS,
(gimbal.requestStatusRetries > 1) ? 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 > 1 ? "default rate" : "0.2 Hz") << "interval for device: "
<< gimbal.deviceId << "compID: " << compid << ", retries remaining: " << gimbal.requestStatusRetries;
}

if (!gimbal.receivedAttitude && gimbal.requestAttitudeRetries > 0 &&
gimbal.receivedInformation && gimbal.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,
MAV_CMD_SET_MESSAGE_INTERVAL,
false /* no error */,
MAVLINK_MSG_ID_GIMBAL_DEVICE_ATTITUDE_STATUS,
0 /* request default rate */);

--gimbal.requestAttitudeRetries;
}

if (!gimbal.receivedInformation || !gimbal.receivedStatus || !gimbal.receivedAttitude) {
// Not complete yet.
return;
Expand All @@ -198,4 +228,4 @@ void GimbalController::_checkComplete(Gimbal& gimbal)

_gimbals.push_back(&gimbal);
_vehicle->gimbalDataChanged();
}
}
12 changes: 10 additions & 2 deletions src/Gimbal/GimbalController.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -22,7 +22,7 @@ class GimbalController : public QObject
unsigned requestInformationRetries = 3;
unsigned requestStatusRetries = 6;
unsigned requestAttitudeRetries = 3;
uint8_t responsibleCompid = 0;
uint8_t deviceId = 0; // Component ID of gimbal device (or 1-6 for non-MAVLink gimbal)
bool receivedInformation = false;
bool receivedStatus = false;
bool receivedAttitude = false;
Expand All @@ -38,6 +38,12 @@ class GimbalController : public QObject
bool haveControl = false;
bool othersHaveControl = false;
};

class GimbalManager {
public:
unsigned requestGimbalManagerInformationRetries = 3;
bool receivedInformation = false;
};

// TODO: Some sort of selection of gimbal to access the API.

Expand All @@ -59,11 +65,13 @@ 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);
void _checkComplete (Gimbal& gimbal, uint8_t compid);

MAVLinkProtocol* _mavlink = nullptr;
Vehicle* _vehicle = nullptr;


QMap<uint8_t, GimbalManager> _potentialGimbalManagers;
QMap<uint8_t, Gimbal> _potentialGimbals;
QVector<Gimbal*> _gimbals;
};

0 comments on commit 7221c21

Please sign in to comment.