I believe I found an issue in the math in AC_PosControl.cpp::accel_to_lean_angles, it should be
_roll_target = … accel_right * _pitch_target … not
_roll_target = accel_right * _ahrs.cos_pitch() as it is now.
I wrote out the math as follows
- Multiply the rotation matrices for Pitch and Roll
- Take the transpose to find R_body_to_world
- Multiply R_body_to_world * [0;0;1] to find the unit vector in the direction of thrust in world frame
- Set this equal to [ax, ay, g].
- Solve for tan(pitch), tan(roll) by dividing row 2 by row 3, and row 1 by row 3.
This matches closely to the code, however, we should be using _pitch_target, not _ahrs.cos_pitch().
As long as the attitude loop tracks quickly, _ahrs.cos_pitch() will quickly approach cos(_pitch_target), so this can almost work. However, the transient behavior is mathematically incorrect.
Am I correct, and should I submit a fix?