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

Gimbal/Joystick: implement rate commands #12049

Merged
merged 5 commits into from
Nov 6, 2024
Merged

Gimbal/Joystick: implement rate commands #12049

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
a40e4b9
Gimbal/Joystick: implement rate commands
julianoes Oct 30, 2024
560244a
Gimbal: set parent of Gimbal correctly
julianoes Oct 30, 2024
c827568
Gimbal: Set parent of GimbalController
julianoes Oct 30, 2024
cfdadfd
Gimbal: expose to UI joystick button control rate
Davidsastresas Nov 4, 2024
f01403c
GimbalIndicator.qml: fix buttons after 45b0c0909bfdfb833c6d88ed67a501…
Davidsastresas Nov 4, 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
182 changes: 134 additions & 48 deletions src/Gimbal/GimbalController.cc
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -27,14 +27,14 @@ 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
Gimbal::Gimbal(GimbalController* parent)
: FactGroup(100, parent, ":/json/Vehicle/GimbalFact.json")
{
_initFacts();
}

Gimbal::Gimbal(const Gimbal& other)
: FactGroup(100, ":/json/Vehicle/GimbalFact.json") // No need to set parent as this will be deleted by gimbalController destructor
: FactGroup(100, other.parent(), ":/json/Vehicle/GimbalFact.json")
{
_initFacts();
*this = other;
Expand Down Expand Up @@ -91,12 +91,18 @@ void Gimbal::_initFacts()
}

GimbalController::GimbalController(MAVLinkProtocol* mavlink, Vehicle* vehicle)
: _mavlink(mavlink)
: QObject(vehicle)
, _mavlink(mavlink)
, _vehicle(vehicle)
, _activeGimbal(nullptr)
{
QQmlEngine::setObjectOwnership(this, QQmlEngine::CppOwnership);
connect(_vehicle, &Vehicle::mavlinkMessageReceived, this, &GimbalController::_mavlinkMessageReceived);

_rateSenderTimer = new QTimer(this);
_rateSenderTimer->setInterval(500);

connect(_rateSenderTimer, &QTimer::timeout, this, &GimbalController::_rateSenderTimeout);
}

GimbalController::~GimbalController()
Expand Down Expand Up @@ -177,22 +183,26 @@ GimbalController::_handleGimbalManagerInformation(const mavlink_message_t& messa
qCDebug(GimbalLog) << "_handleGimbalManagerInformation for gimbal device: " << information.gimbal_device_id << ", component id: " << message.compid;

GimbalPairId pairId{message.compid, information.gimbal_device_id};
auto& gimbal = _potentialGimbals[pairId];

gimbal.setManagerCompid(message.compid);
gimbal.setDeviceId(information.gimbal_device_id);
auto gimbal = _potentialGimbals.find(pairId);
if (gimbal == _potentialGimbals.end()) {
gimbal = _potentialGimbals.insert(pairId, this);
}

gimbal->setManagerCompid(message.compid);
gimbal->setDeviceId(information.gimbal_device_id);

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

gimbal._receivedInformation = true;
gimbal->_receivedInformation = true;
// It is important to flag our potential gimbal manager as well, so we stop requesting gimbal_manger_information message
auto& gimbalManager = _potentialGimbalManagers[message.compid];
gimbalManager.receivedInformation = true;

_checkComplete(gimbal, pairId);
_checkComplete(*gimbal, pairId);
}

void
Expand All @@ -210,27 +220,32 @@ GimbalController::_handleGimbalManagerStatus(const mavlink_message_t& message)
}

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) {
auto gimbal = _potentialGimbals.find(pairId);
if (gimbal == _potentialGimbals.end()) {
gimbal = _potentialGimbals.insert(pairId, this);
}


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) {
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) {
if (!gimbal->_receivedStatus) {
qCDebug(GimbalLog) << "_handleGimbalManagerStatus: gimbal manager with compId " << message.compid
<< " is responsible for gimbal device " << status.gimbal_device_id;
}

gimbal._receivedStatus = true;
gimbal->_receivedStatus = true;

const bool haveControl =
(status.primary_control_sysid == _mavlink->getSystemId()) &&
Expand All @@ -239,15 +254,15 @@ GimbalController::_handleGimbalManagerStatus(const mavlink_message_t& message)
const bool othersHaveControl = !haveControl &&
(status.primary_control_sysid != 0 && status.primary_control_compid != 0);

if (gimbal.gimbalHaveControl() != haveControl) {
gimbal.setGimbalHaveControl(haveControl);
if (gimbal->gimbalHaveControl() != haveControl) {
gimbal->setGimbalHaveControl(haveControl);
}

if (gimbal.gimbalOthersHaveControl() != othersHaveControl) {
gimbal.setGimbalOthersHaveControl(othersHaveControl);
if (gimbal->gimbalOthersHaveControl() != othersHaveControl) {
gimbal->setGimbalOthersHaveControl(othersHaveControl);
}

_checkComplete(gimbal, pairId);
_checkComplete(*gimbal, pairId);
}

void
Expand Down Expand Up @@ -286,19 +301,22 @@ GimbalController::_handleGimbalDeviceAttitudeStatus(const mavlink_message_t& mes
return;
}

auto& gimbal = _potentialGimbals[pairId];
auto gimbal = _potentialGimbals.find(pairId);
if (gimbal == _potentialGimbals.end()) {
gimbal = _potentialGimbals.insert(pairId, this);
}

const bool yaw_in_vehicle_frame = _yawInVehicleFrame(attitude_status.flags);

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;
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.setAbsoluteRoll(qRadiansToDegrees(roll));
gimbal.setAbsolutePitch(qRadiansToDegrees(pitch));
gimbal->setAbsoluteRoll(qRadiansToDegrees(roll));
gimbal->setAbsolutePitch(qRadiansToDegrees(pitch));

if (yaw_in_vehicle_frame) {
float bodyYaw = qRadiansToDegrees(yaw);
Expand All @@ -307,8 +325,8 @@ GimbalController::_handleGimbalDeviceAttitudeStatus(const mavlink_message_t& mes
absoluteYaw -= 360.0f;
}

gimbal.setBodyYaw(bodyYaw);
gimbal.setAbsoluteYaw(absoluteYaw);
gimbal->setBodyYaw(bodyYaw);
gimbal->setAbsoluteYaw(absoluteYaw);

} else {
float absoluteYaw = qRadiansToDegrees(yaw);
Expand All @@ -317,13 +335,13 @@ GimbalController::_handleGimbalDeviceAttitudeStatus(const mavlink_message_t& mes
bodyYaw += 360.0f;
}

gimbal.setBodyYaw(bodyYaw);
gimbal.setAbsoluteYaw(absoluteYaw);
gimbal->setBodyYaw(bodyYaw);
gimbal->setAbsoluteYaw(absoluteYaw);
}

gimbal._receivedAttitude = true;
gimbal->_receivedAttitude = true;

_checkComplete(gimbal, pairId);
_checkComplete(*gimbal, pairId);
}

void
Expand Down Expand Up @@ -435,32 +453,51 @@ bool GimbalController::_yawInVehicleFrame(uint32_t flags)
}
}

void GimbalController::gimbalPitchStep(int direction)
void GimbalController::gimbalPitchStart(int direction)
{
if (!_activeGimbal) {
qCDebug(GimbalLog) << "gimbalStepPitch: active gimbal is nullptr, returning";
qCDebug(GimbalLog) << "gimbalPitchStart: active gimbal is nullptr, returning";
return;
}

if (_activeGimbal->yawLock()) {
sendPitchAbsoluteYaw(_activeGimbal->absolutePitch()->rawValue().toFloat() + direction, _activeGimbal->absoluteYaw()->rawValue().toFloat(), false);
} else {
sendPitchBodyYaw(_activeGimbal->absolutePitch()->rawValue().toFloat() + direction, _activeGimbal->bodyYaw()->rawValue().toFloat(), false);
float speed = qgcApp()->toolbox()->settingsManager()->gimbalControllerSettings()->joystickButtonsSpeed()->rawValue().toInt();
activeGimbal()->setPitchRate(direction * speed);

sendRate();
}

void GimbalController::gimbalYawStart(int direction)
{
if (!_activeGimbal) {
qCDebug(GimbalLog) << "gimbalYawStart: active gimbal is nullptr, returning";
return;
}

float speed = qgcApp()->toolbox()->settingsManager()->gimbalControllerSettings()->joystickButtonsSpeed()->rawValue().toInt();
activeGimbal()->setYawRate(direction * speed);
sendRate();
}

void GimbalController::gimbalYawStep(int direction)
void GimbalController::gimbalPitchStop()
{
if (!_activeGimbal) {
qCDebug(GimbalLog) << "gimbalStepPitch: active gimbal is nullptr, returning";
qCDebug(GimbalLog) << "gimbalPitchStop: active gimbal is nullptr, returning";
return;
}

if (_activeGimbal->yawLock()) {
sendPitchAbsoluteYaw(_activeGimbal->absolutePitch()->rawValue().toFloat(), _activeGimbal->absoluteYaw()->rawValue().toFloat() + direction, false);
} else {
sendPitchBodyYaw(_activeGimbal->absolutePitch()->rawValue().toFloat(), _activeGimbal->bodyYaw()->rawValue().toFloat() + direction, false);
activeGimbal()->setPitchRate(0.0f);
sendRate();
}

void GimbalController::gimbalYawStop()
{
if (!_activeGimbal) {
qCDebug(GimbalLog) << "gimbalYawStop: active gimbal is nullptr, returning";
return;
}

activeGimbal()->setYawRate(0.0f);
sendRate();
}

void GimbalController::centerGimbal()
Expand Down Expand Up @@ -522,6 +559,10 @@ void GimbalController::sendPitchBodyYaw(float pitch, float yaw, bool showError)
return;
}

_rateSenderTimer->stop();
_activeGimbal->setAbsolutePitch(0.0f);
_activeGimbal->setYawRate(0.0f);

// qDebug() << "sendPitch: " << pitch << " BodyYaw: " << yaw;

unsigned flags = GIMBAL_MANAGER_FLAGS_ROLL_LOCK
Expand All @@ -546,6 +587,10 @@ void GimbalController::sendPitchAbsoluteYaw(float pitch, float yaw, bool showErr
return;
}

_rateSenderTimer->stop();
_activeGimbal->setAbsolutePitch(0.0f);
_activeGimbal->setYawRate(0.0f);

if (yaw > 180.0f) {
yaw -= 360.0f;
}
Expand Down Expand Up @@ -590,6 +635,47 @@ void GimbalController::toggleGimbalRetracted(bool set)
sendPitchYawFlags(flags);
}

void GimbalController::sendRate()
{
if (!_tryGetGimbalControl()) {
return;
}

unsigned flags = GIMBAL_MANAGER_FLAGS_ROLL_LOCK
| GIMBAL_MANAGER_FLAGS_PITCH_LOCK;

if (_activeGimbal->yawLock()) {
flags |= GIMBAL_MANAGER_FLAGS_YAW_LOCK;
}

_vehicle->sendMavCommand(
_activeGimbal->managerCompid()->rawValue().toUInt(),
MAV_CMD_DO_GIMBAL_MANAGER_PITCHYAW,
false,
NAN,
NAN,
_activeGimbal->pitchRate(),
_activeGimbal->yawRate(),
flags,
0,
_activeGimbal->deviceId()->rawValue().toUInt());

qCDebug(GimbalLog) << "Gimbal rate sent!";

// Stop timeout if both unset.
if (_activeGimbal->pitchRate() == 0.f && _activeGimbal->yawRate() == 0.f) {
_rateSenderTimer->stop();
} else {
_rateSenderTimer->start();
}
}

void GimbalController::_rateSenderTimeout()
{
// Send rate again to avoid timeout on autopilot side.
sendRate();
}

void GimbalController::toggleGimbalYawLock(bool set)
{
if (!_tryGetGimbalControl()) {
Expand Down
Loading
Loading