I have problem in understanding thrust_heading_rotation_angles() in line 455 of AC_AttitudeControl.cpp. At first you define the rotation matrix in line 458 att_to_quat.rotation_matrix(att_to_rot_matrix). This is the rotation matrix from inertial frame to att_to body frame which is calculated from att_to_quat quaternion. Then in line 459 there is a definition Vector3f att_to_thrust_vec = att_to_rot_matrix*Vector3f(0.0f,0.0f,1.0f);
The name att_to_thrust_vec may be misleading because Vector3f(0.0f,0.0f,1.0f) is the unit gravity vector in inertial frame so this product should be the unit gravity vector’s coordinates in att_to body frame. Similliarly with the definition of the att_from_thrust_vec. Actually these 2 vectors are the coordinates of the same unit gravity vector in 2 different att_to and att_from body frames.
The question now is how can you define the cross product of these 2 vectors in line 466
Vector3f thrust_vec_cross = att_from_thrust_vec % att_to_thrust_vec;
when they are not in the same reference frame?
Unfortunately it takes a lot longer to answer some of these questions than to ask them. And thankfully for you a hell of a lot longer than to answer them 
Yes, naming is a pain and as I don’t have a formal background in aircraft control theory or even quaternion math I struggle here a bit. There is method to my madness however.
Some definitions:
att_to_*** is the rotation from the target body frame to the inertial frame.
att_from_*** is the rotation from the current body frame to the inertial frame.
So att_to_thrust_vec is the thrust vector or z-vector associated with the target body frame in the inertial frame. Similarly, att_from_thrust_vec is the thrust vector or z-vector associated with the current body frame in the inertial frame.
So the cross product of these two is the vector that we can use to rotate the thrust vector from the current attitude to the desired attitude in the shortest possible path.
So actually they are both vectors in the inertial frame.
Yes, naming sucks. I am very happy to get naming suggestions. I would appreciate it if you would provide a link to the resource where you are basing your naming conventions on so I can reference it in the code if I change it.
Thanks for looking through the code so deeply.
Sorry that asking is easier than answering, but this is your find out so I need to ask from you. I still think that att_to_thrust_vector’s coordinates is relative to att_to body frame because it is the result of the multiplication with att_to_rot_matrix. These coordinates relates to att_to body frame unit vectors, not the inertial frame unit vectors of x, y, z in NED inertial frame.
no problem at all, I was just explaining why sometimes it takes time to answer properly.
That originaly comes from here:
You can see that it is the rotation from the body frame to the NED.
So why do you think it is the rotation from NED to body?
I can show an example. Lets look at the followings
att_from: roll(phi)=0, pitch(theta)=0. yaw(tsi)=pi/4 then att_from_thrust_vector=(0, 0, 1)
att_to: roll(phi)=0, ptch(theta)=pi/4, yaw(tsi)=pi/4 then att_to_thrust_vector=(0, -1/sqrt(2), 1/sqrt(2))
Vector3f thrust_vec_cross = att_from_thrust_vec % att_to_thrust_vec; this should be zero because both are of the same z vector
with definition in inertial frame =(a2b3-a3b2)i+(a3b1-a1b3)j+(a1b2-a2b1)k=(1/sqrt(2),0,0) is not zero?
This is correct. Why do you expect it to be zero?
Hi, Leonard Hall,
Reading your question I’ve found out that I misunderstood your algorithm from the beginning. Yes, I was wrong when I thought that att_from_thrust_vector and att_to_thrust_vector are only the representations of the z unit vector in att_from and att_to body frame. Therefore, I wrongly accepted that their cross product is zero. In fact those vectors are the rotated z vector representations in inertial frame and the thrust_vec_correction_quat is to rotate one to another. Things are clearer now, but I still need more time to understand your algorithm now. Anyway, thank you very much for the effective help you gave to me, if I have more question on this topic, please be patient to me.
No problem. It was just as much of a brain bender when I wrote it. Walking you through it helps me comment it better.
In fact, if you added some comments as you read through it and understood what I have done that would help me make it easier for others. I will read through your comments and edit them and add them to the code.
Help me help you 
Hi Leonard Hall,
As I see, the rotation matrix is often defined from NED to body frame, so I think the quaternion is defined the same way, from NED to body frame. However I see your definition of the vehicle quaternion as from body frame to NED
attitude_vehicle_quat.from_rotation_matrix(_ahrs.get_rotation_body_to_ned())
and I think it is the same with attitude_target_quat. Why are they defined this way? Is it this way sothat you can define cross and dot product in NED frame?
And another question is that what is the heading_quad and why only it has role in att_diff_angle.z
I chose that representation as I am defining my desired attitude in the inertial NED frame. So it made sense to me to define my current attitude and the desired attitude in that frame and do the comparison between the two in that frame. So yes, I am using the inertial NED frame to compare the two using the cross product to define the rotation vector needed to put the thrust vector where I want it.
The heading quat is there because you do not need to define the heading to put the thrust vector where it needs to be. The thrust vector is what keeps the aircraft flying while the heading is much less important. When correcting from large errors we ignore heading completly and focus only on correcting the thrust vector. When the thrust vector error is small then we correct all three axis.
The other thing to keep in mind, in almost all hovering aircraft the controllers used to define the thrust vector are generally similar to each other but different to the yaw control.
I’ve found in AC_AttitudeControl.h line 344-346:
// This represents a quaternion rotation from NED frame to the target (setpoint)
// attitude used in the attitude controller.
Quaternion _ attitude_target_quat;
This quaternion is in thrust_heading_rotation_angles call in line 415 that would mean that att_to_thrust_vector is not in NED frame. In this case how do you define the cross and dot product with att_from_thrust_vector in NED frame?
It’s a pity. I’ve found in line 114 _attitude_target_quat.from_rotation_matrix(_ahrs.get_rotation_body_to_ned());
This means that you’ve already changed the definition.
Sorry, what are you saying?
I’ve found in line 114 in AC_AttitudeControl::relax_attitude_controllers()
_ attitude_target_quat.from_rotation_matrix(ahrs.getrotation_body_to_ned())
It redefines _ attitude_target_quat to NED. However I cannot find where it is called
It is called when the aircraft has landed.
You seem to still think
I don’t understand why you think this.
I thought that way because the definition of _ attitude_target_quat in AC_AttitudeControl.h in line 344-346 is that this is rotation from NED. But you did compare att_from_thrust_vector and att_to_thrust_vector in NED. But then I found that you redefined _ attitude_target_quat in relax_attitude_controllers() so I asked where this function is called or the _ attitude_target_quat is redefined?
Ok, I see the issue.
// This represents a quaternion rotation from NED frame to the target (setpoint)
// attitude used in the attitude controller.
Should probably read:
// This represents a quaternion rotation in NED frame to the target (setpoint)
// attitude used in the attitude controller.
Can you tell me more concretely, in what file and line number?
https://github.com/bnsgeyer/ardupilot/blob/master/ArduCopter/control_stabilize.cpp#L32
https://github.com/bnsgeyer/ardupilot/blob/master/ArduCopter/control_althold.cpp#L84
Looks like we need to be a little more consistent there.
Hi Leonadt Hall,
Thank you very much for the help. Then I saw you calculate the corected quaternion back in the curent body frame and you used quaternion math to calculate the angular error from the corected quaternion. This is what I don’t understand.
Can you show me what document I should read to understand the way to convert from the quaternion to angular error.