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 