Mount: improve yaw lock reporting to GCS

[rmackay9]

This makes a few enhancements to our AP_Mount library to improve the GCS's ability to set and retrieve the current yaw-lock state. This is required for the QGC custom gimbal control screen (see mavlink/qgroundcontrol#11264)

To clear up terminology:

• yaw lock means the yaw (target or actual) is in earth-frame
• yaw follow means the yaw is in body-frame

Detailed changes are:

1. MNT1_OPTIONS parameter added with a single option bit called, "RC lock state from previous mode" allows users to specify that they want the yaw lock state carried over from the previous mode when they switch to RC_TARGETING (e.g. the pilot controls the sticks). Previously the yaw-lock state was only set-able using an RC auxiliary switch. If the user is actively controlling the gimbal with both the RC and GCS though it can be convenient to carry over the state.
2. GCS can use the DO_GIMBAL_MANAGER_PITCHYAW command and GIMBAL_MANAGER_SET_PITCHYAW message can be used to set the yaw lock state in RC_TARGETING mode by setting target pitch and yaw angles and rates to NaN
3. Comments added to clarify that AP_Mount's set_lock() method only affects RC_TARGETTING mode (e.g. when the pilot is controlling the gimbal with the sticks)
4. GIMBAL_DEVICE_ATTITUDE_STATUS reports the target yaw lock state. This message's flags are confusing but what we do here seems correct (see here)

This work was based on @Davidsastresas's earlier PR #26474

This has been lightly tested in SITL.

[Davidsastresas]

Hello Randy, thank you for the effort. I think you missed my last commit on the original #26474 PR I opened about this:

81dfeab

Ardupilot is always sending yaw in vehicle frame, and as per the documentation about this ( see mavlink/mavlink-devguide#531 ) It is preferable to use:

GIMBAL_DEVICE_FLAGS_YAW_IN_VEHICLE_FRAME
GIMBAL_DEVICE_FLAGS_YAW_IN_EARTH_FRAME

To indicate yaw frame, but if none of the above flags are set, the yaw frame will be interpreted from GIMBAL_DEVICE_FLAGS_YAW_LOCK.

So this branch, as it is now, is not reporting correctly this. I tried to review and suggest the changes with github functionality but I don't think I have permission for it in AP ( would be good to have for such situations though! ). So please allow me to indicate here what I mean. In AP_Mount_Backend.cpp, in get_gimbal_device_flags(), at the end ( https://github.com/rmackay9/rmackay9-ardupilot/blob/3959c214e4ccf29802cb58489bee080090ddfd7b/libraries/AP_Mount/AP_Mount_Backend.cpp#L853 ), we would need to add the flag GIMBAL_DEVICE_FLAGS_YAW_IN_VEHICLE_FRAME always, as Ardupilot is always using vehicle frame for gimbal yaw. It would end up something like this:

    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) |
                           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
                           GIMBAL_DEVICE_FLAGS_YAW_IN_VEHICLE_FRAME | // Yaw angle is always in vehicle-frame for Ardupilot
                           (yaw_lock_state ? GIMBAL_DEVICE_FLAGS_YAW_LOCK : 0);
    return flags;
}

Otherwise, the receiver of this message will interpret angles based purely on GIMBAL_DEVICE_FLAGS_YAW_LOCK, and AP is not sending angles in earth frame when such flag is active.

I tested that change ( just adding that flag as the code snippet above ) to your branch locally and it seems to work alright with the QGC PR you mentioned: mavlink/qgroundcontrol#11264.

Thanks!

[rmackay9]

@Davidsastresas,

Thanks very much for the feedback. I've added your fix to the device flags and re-tested and it's working very well now, thanks!

[rmackay9]

This is included in 4.5.2-beta1