### pan tilt angle calculations from roll pitch angles

I have a pan tilt gimbal (tilt over pan i.e. tilt axis changes with pan.) for my camera and and i know the roll and pitch angle of the plane but in order to point the lens of the camera in nadir direction , i need to know the pan and subsequent tilt rotation i need to produce . I know the solution can be found using rotation matrices or quaternions but dont know exactly how to do it. Any suggestions are welcome.

#### Replies

• No need to do it yourself.

Just update the Ardupilot firmware, newer versions already do what you want (Thanks to Greg !).

• Shivang,

Actually this has all ready been done and is an Arducopter and Arduplane code in the AP_Mount.cpp. I will explain how it's done.

1) First start with the ahrs matrix(DCM) and transpose this into a new matrix. That gives the rotation from the earth frame to the aircraft frame.

2) Next you generate a new rotation matrix from your desired camera angles relative to earth frame using a reverse euler angle function.

3) Multiply the first matrix by the camera matrix.

4) Calculate the euler angles of the matrix from step 3. These are the angles to go to the servos.

Her is the actual function.

AP_Mount::calculate()
{
m = _ahrs->get_dcm_matrix();
m.transpose();
cam.from_euler(_roll_control_angle, _pitch_control_angle, _yaw_control_angle);
gimbal_target = m * cam;
gimbal_target.to_euler(&_roll, &_pitch, &_yaw);
_roll_angle = degrees(_roll)*100;
_pitch_angle = degrees(_pitch)*100;
_yaw_angle = degrees(_yaw)*100;
}