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

Davidsastresas · 2024-03-10T13:35:30Z

currently the only way to command yaw lock/follow is by rc channels MOUNT_LOCK (163) action. As we are moving to gimbal manager v2 protocol we need some way to command this from gimbal manager protocol.

I didn't place a return MAV_CMD right after the yaw lock flags check because we could have the situation where only gimbal yaw lock is commanded, but we could also receive a gimbal lock/unlock command together with rate or angle targets. This way both situations are handled.

I need clarification in one particular situation. As it is now, if it receives a yaw lock, but yaw was already locked, it will return MAV_RESULT_FAILED. It will only return MAV_RESULT_ACCEPTED if the state changed. Is this preferable or should we return accepted even if the command didn't change it?

@rmackay9 if you could double check this one is fine it would be great. Thanks!

currently the only way to command yaw lock/follow is by rc channels MOUNT_LOCK (163) action. As we are moving to gimbal manager v2 protocol we need some way to command this from gimbal manager protocol

…vice flags: In AP we always use vehicle frame for mount yaw, so we must specify it on gimbal_device_flags, so the receivers can interpret gimbal_device_attitude_status correctly. We are also adding yaw_lock flags, so the receiver can understand the yaw lock/follow status of the mount

Davidsastresas · 2024-03-10T23:57:41Z

I just added a new commit adding permanently GIMBAL_DEVICE_FLAGS_YAW_IN_VEHICLE_FRAME for gimbal device flags ( It seems all the mount backends use vehicle frame for yaw ), and also adding GIMBAL_DEVICE_FLAGS_YAW_LOCK depending on yaw lock/follow state.

GIMBAL_DEVICE_FLAGS_YAW_IN_VEHICLE_FRAME is needed for a GCS to interpret correctly the yaw angle received, and GIMBAL_DEVICE_FLAGS_YAW_LOCK is interesting to have so we can display the state of it from a GCS.

rmackay9 · 2024-03-19T05:04:09Z

libraries/AP_Mount/AP_Mount.cpp

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


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.

rmackay9 · 2024-03-19T05:07:55Z

libraries/AP_Mount/AP_Mount_Backend.h

@@ -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
    void set_yaw_lock(bool yaw_lock) { _yaw_lock = yaw_lock; }

+    // get yaw lock state
+    bool get_yaw_lock() { return _yaw_lock; }


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

rmackay9 · 2024-03-19T06:20:33Z

I don't quite understand why we need another way for the DO_GIMBAL_MANAGER_PITCH_YAW command/message to change the yaw-lock state. I think this is already possible using the existing flags field. The only time this isn't possible is when there are no targets provided.. but why would there be no target provided? The only reason I can think of is if the targets are provided by the RC input (e.g. the mount is in RC_TARGETING mode) but will it be intuitive for a user to click on the GCS screen to change the follow/lock behaviour but then use the transmitter sticks to control the yaw and pitch movement? I think if they're holding a transmitter then they'd prefer to do that using a button or switch (which we already support).

The bigger issue though is I think this change would make the existing Mount Lock auxiliary switch difficult to use because the follow/lock state becomes shared across RC_TARGETING and MAVLINK_TARGETING modes. So for example, imagine this case:

The user is controlling the gimbal with the transmitter (e.g. RC_TARGETING mode) using rate control (e.g. follow, not-lock)
User clicks on the map and selects "point camera here" (aka ROI) then the camera rotates to the ROI position and enters lock mode (e.g. earth-frame angle control).
If the user then moves the transmitter sticks the gimbal will go back iknto rate control but will be in lock mode (not follow). They would need to move the auxiliary switch back to lock and then follow again... assuming they even have an auxiliary switch setup for this.

I think what's led to this confusion is that AP_Mount's comments don't make it clear that set_yaw_lock() only applies to RC_TARGETING mode.

rmackay9 · 2024-03-19T06:37:00Z

libraries/AP_Mount/AP_Mount_Backend.cpp

@@ -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) |


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.

rmackay9 · 2024-03-19T06:38:27Z

libraries/AP_Mount/AP_Mount_Backend.h

@@ -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


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.

rmackay9 · 2024-03-19T06:59:39Z

Thinking about this more, if we want the RC_TARGETING yaw_lock state to be controlled from the GCS then we could add a MNTx_OPTIONS parameter with a "Lock/Follow from GCS" bit. Then we could check that setting from within the do-gimbal-manager-pitchyaw function and update the lock state if necessary.

This wouldn't resolve the reporting that @Davidsastresas would probably like but unless the user had also setup an auxiliary switch the GCS could assume the yaw_lock was what it had last set it to.

rmackay9 · 2024-03-19T10:51:14Z

To hopefully help things along I've created this branch with a possible alternative implementation https://github.com/rmackay9/rmackay9-ardupilot/commits/mount-yaw-lock-option/

In case this is of interest the way it works is you set MNT1_OPTIONS = 1. Then you should find that if you takeover control of the gimbal with the RC sticks, the lock/follow state is maintained from whatever the GCS last sent.

Davidsastresas · 2024-03-20T19:40:54Z

closing in favor of #26564 as per Randy's request. Thanks!

Davidsastresas added 2 commits March 10, 2024 14:30

AP_Mount_Backend.h: add getter for _yaw_lock

bc5d022

AP_Mount.cpp: handle gimbal yaw lock from gimbal_manager_pitchyaw flags:

941c572

currently the only way to command yaw lock/follow is by rc channels MOUNT_LOCK (163) action. As we are moving to gimbal manager v2 protocol we need some way to command this from gimbal manager protocol

Davidsastresas force-pushed the master-PR/yaw_lock_gimbal_manager_pitchyaw_flags_support branch from 6bdfa20 to 941c572 Compare March 10, 2024 14:13

rmackay9 added the DevCallTopic label Mar 10, 2024

Davidsastresas changed the title ~~Add gimbal yaw lock support from gimbal manager v2 protocol~~ Leverage yaw lock/follow support on gimbal manager v2 protocol Mar 10, 2024

julianoes approved these changes Mar 11, 2024

View reviewed changes

Davidsastresas mentioned this pull request Mar 11, 2024

AP_Mount: Return to RC_Targeting automatically when control is sent by mavlink instead of Rc channels #26479

Closed

rmackay9 removed the DevCallTopic label Mar 12, 2024

rmackay9 reviewed Mar 19, 2024

View reviewed changes

rmackay9 mentioned this pull request Mar 20, 2024

Mount: improve yaw lock reporting to GCS #26564

Merged

Davidsastresas closed this Mar 20, 2024

Davidsastresas mentioned this pull request Mar 20, 2024

Improve gimbal support for QGC 4.4 mavlink/qgroundcontrol#11264

Merged

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

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

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

Davidsastresas commented Mar 10, 2024 •

edited

Davidsastresas commented Mar 10, 2024 •

edited

rmackay9 Mar 19, 2024

rmackay9 Mar 19, 2024

rmackay9 commented Mar 19, 2024 •

edited

rmackay9 Mar 19, 2024

rmackay9 Mar 19, 2024

rmackay9 commented Mar 19, 2024

rmackay9 commented Mar 19, 2024

Davidsastresas commented Mar 20, 2024 •

edited

		@@ -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

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

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

Conversation

Davidsastresas commented Mar 10, 2024 • edited

Davidsastresas commented Mar 10, 2024 • edited

rmackay9 Mar 19, 2024

Choose a reason for hiding this comment

rmackay9 Mar 19, 2024

Choose a reason for hiding this comment

rmackay9 commented Mar 19, 2024 • edited

rmackay9 Mar 19, 2024

Choose a reason for hiding this comment

rmackay9 Mar 19, 2024

Choose a reason for hiding this comment

rmackay9 commented Mar 19, 2024

rmackay9 commented Mar 19, 2024

Davidsastresas commented Mar 20, 2024 • edited

Davidsastresas commented Mar 10, 2024 •

edited

Davidsastresas commented Mar 10, 2024 •

edited

rmackay9 commented Mar 19, 2024 •

edited

Davidsastresas commented Mar 20, 2024 •

edited