Bug in AC_PosControl accel_to_lean_angles()

ArduCopter Copter 3.2
1

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

  1. Multiply the rotation matrices for Pitch and Roll
  2. Take the transpose to find R_body_to_world
  3. Multiply R_body_to_world * [0;0;1] to find the unit vector in the direction of thrust in world frame
  4. Set this equal to [ax, ay, g].
  5. 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?

2

To expedite a response you might post the same question here groups.google.com/forum/?fromgr … es-discuss , it should get more views from the devs there.

3

Ah, thank you :slight_smile:

4

And to correct this, I mean it should be cos(_pitch_target), not just _pitch_target.