Gimbal control using mount control plugin

I’m trying to understand the mount control plugin from mavros_extras, to control the gimbal from the Typhoon. In master branch from mavros, mount_control plugin uses gimbal protocol v1 from mavlink.

Although I’ve read the code and the docs from protocol v1, I can’t figure out how to command it properly. Suppose I want the gimbal to have Y=130 degrees, then P=45 degrees, and then R=20 degrees, so the gimbal would point upwards, with the camera rotated clock-wise.

q_uav = [orientation.x, orientation.y, orientation.z, orientation.w]
q = quaternion_from_euler(R, P, Y, 'rxyz')               # command in quaternion format
q = quaternion_multiply(quaternion_conjugate(q_uav), q)  # compensation for UAV's orientation
q = quaternion_conjugate(q)                              # command with inverted angles
R, P, Y = euler_from_quaternion(q)                       # euler angles to gimbal (in radians)

But it’s not clear if I have to compensate only the Yaw rotation of the UAV.

According to the plugin’s code, which publishes the mount orientation as a quaternion, this quaternion comes from a MOUNT_ORIENTATION message —roll and pitch in global frame, yaw relative to UAV’s heading—, and uses ftf::quaternion_from_rpy(). Which would be the correct way to retrieve the gimbal’s orientation in body frame?

Thank you :slight_smile: