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?