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

Leverage yaw lock/follow support on gimbal manager v2 protocol #26474

Closed
wants to merge 3 commits into from
Closed

Leverage yaw lock/follow support on gimbal manager v2 protocol #26474

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
3 commits
Select commit Hold shift + click to select a range
bc5d022
AP_Mount_Backend.h: add getter for _yaw_lock
Davidsastresas Mar 10, 2024
941c572
AP_Mount.cpp: handle gimbal yaw lock from gimbal_manager_pitchyaw flags:
Davidsastresas Mar 10, 2024
81dfeab
AP_Mount_Backend.cpp: use yaw_lock and yaw_in_vehicle_frame gimbal_de…
Davidsastresas Mar 10, 2024
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Jump to
Jump to file
Failed to load files.
Diff view
Diff view
12 changes: 11 additions & 1 deletion libraries/AP_Mount/AP_Mount.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -340,6 +340,15 @@ MAV_RESULT AP_Mount::handle_command_do_gimbal_manager_pitchyaw(const mavlink_com
return MAV_RESULT_ACCEPTED;
}

// Check for yaw lock/follow in flags, and apply to backend if needed.
// We don't return yet, as we could have a command sending targets plus yaw lock/unlock
bool yaw_lock = flags & GIMBAL_DEVICE_FLAGS_YAW_LOCK;
bool yaw_lock_changed = false;
if (backend->get_yaw_lock() != yaw_lock) {
backend->set_yaw_lock(yaw_lock);
yaw_lock_changed = true;
}

// param1 : pitch_angle (in degrees)
// param2 : yaw angle (in degrees)
const float pitch_angle_deg = packet.param1;
Expand All @@ -358,7 +367,8 @@ MAV_RESULT AP_Mount::handle_command_do_gimbal_manager_pitchyaw(const mavlink_com
return MAV_RESULT_ACCEPTED;
}

return MAV_RESULT_FAILED;
// If this was a yaw lock/unlock only command, we need to return MAV_RESULT_ACCEPTED
return yaw_lock_changed ? MAV_RESULT_ACCEPTED : MAV_RESULT_FAILED;
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I think we should return success as long as the actual yaw lock state is in the requested state. So it shouldn't matter if it was changed or not.

}

// handle mav_cmd_do_gimbal_manager_configure for deconflicting different mavlink message senders
Expand Down
2 changes: 2 additions & 0 deletions libraries/AP_Mount/AP_Mount_Backend.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -799,6 +799,8 @@ uint16_t AP_Mount_Backend::get_gimbal_device_flags() const
{
const uint16_t flags = (get_mode() == MAV_MOUNT_MODE_RETRACT ? GIMBAL_DEVICE_FLAGS_RETRACT : 0) |
(get_mode() == MAV_MOUNT_MODE_NEUTRAL ? GIMBAL_DEVICE_FLAGS_NEUTRAL : 0) |
(_yaw_lock ? GIMBAL_DEVICE_FLAGS_YAW_LOCK : 0) |
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I think this is mixing up the target angles or rates vs the actual angles or rates. "get_gimbal_device_flags()" is only used to fill in the GIMBAL_DEVICE_ATTITUDE_STATUS message's flags field that is sent to the GCS. Even if the target are in earth-frame, the angles and rates we're reporting are always in body-frame. I could be wrong but I think this change would lead to us sometimes tell the GCS the angles and rates are earth-frame when they're not.

GIMBAL_DEVICE_FLAGS_YAW_IN_VEHICLE_FRAME | // Yaw angle is always in vehicle-frame
GIMBAL_DEVICE_FLAGS_ROLL_LOCK | // roll angle is always earth-frame
GIMBAL_DEVICE_FLAGS_PITCH_LOCK; // pitch angle is always earth-frame, yaw_angle is always body-frame
return flags;
Expand Down
3 changes: 3 additions & 0 deletions libraries/AP_Mount/AP_Mount_Backend.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -83,6 +83,9 @@ class AP_Mount_Backend
// If false (aka "follow") the gimbal's yaw is maintained in body-frame meaning it will rotate with the vehicle
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Sorry, it's an existing issue but we should add a comment above "set_yaw_lock" to clarify that it only applies in RC_TARGETING mode. We should add a similar comment besides the _yaw_lock member declaration.

void set_yaw_lock(bool yaw_lock) { _yaw_lock = yaw_lock; }

// get yaw lock state
bool get_yaw_lock() { return _yaw_lock; }
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

let's make this const. e.g. "bool get_yaw_lock() const { ... }"


// set angle target in degrees
// yaw_is_earth_frame (aka yaw_lock) should be true if yaw angle is earth-frame, false if body-frame
void set_angle_target(float roll_deg, float pitch_deg, float yaw_deg, bool yaw_is_earth_frame);
Expand Down