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.
That is an easy one 
You calculate the quaternion that rotates you from where you are to where you want to be and you do a very simple conversion to angle-axis and you have the angle error right there.
Happy New Year! Leonard Hall,
I was affected by the diagram on ardupilot page http://ardupilot.org/dev/docs/apmcopter-programming-attitude-control-2.html and it was misleading when you talked about angle error. Therefore, I thought that the error should be euler angle error and tried hard to find the relation between the quaternion error and euler angle error.
In the end, I came out that it was not neccessary and understood that the diagram is only true for attitude representation with euler angle. With quarternion representation, the error here is only the quaternion error, and to represent it in 3 dimension we can use the axis-angle conversion as you said, then we can apply P-controller on 3 axes. Now I begin to understand the attitude control process with quaternion representation but there is still a question. According to the heading_quat definition, itâs known that thrust_vec_correction_quat*heading_quat is the difference between att_to_quad and att_from_quad, so when calculating error, why do you took into account only x and y in thrust_vec_correction_quat.to_axis_angle() and z in heading_quat.to_axis_angle() instead of looking into the product of two quaternions?
The reason is we want to rotate the thrust vector to the desired direction in the shortest possible rotation. When we do this the rotation about the thrust vector (z) is not important. So we first calculate the rotation that keeps us flying, the thrust vector rotation. Then once we have taken care of that then we look at correcting the Z rotation.
This is also related to the fact that if we loose a motor we want to loose control of the Z axis, not the X, Y axis as the X, Y axis are the ones that control the thrust vector. So this approach lets us prioritise the X, Y axis when the error is larger.
Finally, if we use a quaternion that simply rotates from the current position to the desired position it can heavily rely on the yaw control meaning that it can take much longer than necessary to get the thrust vector to where it needs to be.
In short, this approach minimises the time taken to correct the thrust vector and is tolerant to a different level of control on the z axis.
Are you sure that this approach minimises the time taken to correct the thrust vector because I see the corection frequency remains 400Hz and you even need to calculate more. I think that rotations on different axes are independant, the thrust vector rotation doesnât need to wait for the yaw control, so improvement in yaw control may not have effect on thrust vector rotation. Did you compare the performance of the two approaches: rotates all frame and rotates only thrust vector?
None of this changes the correction time.
With my implementation that is correct but if we used just one quaternion then it would not be correct. Especially with complete loss of yaw control.
Yes but this is also obvious. Using an example of an error created by yawing 180 degrees and pitching down 90 degrees:
-
A direct quaternion rotation will take 180 degrees to correct the thrust vector. That rotation will be a combination of yaw and roll. Yaw being a much slower axis than roll will lag behind roll. The aircraft will tend to roll upside down then correct with pitch. This will tend to increase the thrust vector error before reducing it.
-
Separately rotating the thrust vector will result in the thrust vector being rotated by 90 degrees immediately and the 180 degree yaw will happen during or after that thrust vector rotation. You can see that in this example the thrust vector is corrected in half the time compared to the direct quaternion case if we assume that yaw is just as quick as roll and pitch.
I donât understand why a direct quaternion rotation would take 180 degrees to correct the thrust vector because yawing 180 degrees doesnât change the thrust vector, only pitching down 90 degrees would change it up 90 degrees. Anyway, I may think that your example is too exaggerate. Knowing that the corection frequency is 400Hz, so the the yaw,pitch and roll error should be much smaller than 180 degrees.
When the angle is very small then the two solutions are very similar. When the aircraft gets knocked upside down for some reason or has lost a motor that is when the errors get very large and the aircraft is recovering from large lean angles. This isnât an update rate thing this is a large external disturbance that has flipped your aircraft upside down.
The difference between what we have done in Arducopter vs what others do is we handle the extreme cases. It is easy to make a multirotor hover and fly around gently. Most other controllers do not survive my basic test flight much less my extended stress test.
That is why Arducopter is the best multirotor autopilot in the world. We search for and handle the corner cases.
@minhtq98 I was wondering why you are so interested in this?
What is your background, what are you working towards?
My background is computer science, but now Iâm working for controlling UAV. Iâm building a big quadcopter based on Arducopter, but to maintain it, I need to understand the firmware. Thatâs why Iâm asking so many questions, sorry about that.
No problem at all!!!
Not many people take so much interest. It has been good for me. I have been trying to improve the comments based on your questions. So you have been indirectly improving the code!
1 Like
When you talked about extraordinary cases, I begin to understand your approach. Before that I only thought about ordinary cases and didnât see your solution possible advantage. But now I can see it. Maybe you should comment that your approach is better in extraordinary cases when the errors get very large due to some external disturbance.
The problem is 80% of the work is dealing with the corner cases (I wouldnât say extraordinary cases). When you have many hundreds of thousands of aircraft flying the code you quickly realise that these things happen much easier than most university level control system designers think.
For example I can create the large error condition we are looking at here on most multirotors designed for long flight times or using flexible props just by flying forward at full speed in alt hold and then pulling half back stick. If you add a poor tune to that you will get a complete inverted flip.
Anyway, once again many thanks for the valuable code that you brought open so that we can learn much from it
1 Like
Reviving this discussionâŚ
I am trying to understand the function âattitude_controller_run_quatâ, but iâm comfused about the equation:
Quaternion to_to_from_quat = attitude_vehicle_quat.inverse() * _attitude_target_quat; which in https://github.com/ArduPilot/ardupilot/blob/master/libraries/AC_AttitudeControl/AC_AttitudeControl.cpp#L549
attitude_vehicle_quat is the quaternion rotation from the current body frame to the inertial frame;
_attitude_target_quat is the quaternion rotation from the target body frame to the inertial frame;
to_to_from_quat is the error, representing the rotation from target body frame to current body frame;
so i think to_to_from_quat should be rewritten :
Quaternion to_to_from_quat = _attitude_target_quat * attitude_vehicle_quat.inverse();
Hi Leonadt Hall,
Recently I have some time to look back at the attitude control by quaternion. When looking back at Copter-3.5 AC_AttitudeControl.cpp line 615, Iâve found the definition
Quaternion yaw_vec_correction_quat = thrust_vec_correction_quat.inverse()*att_from_quat.inverse()att_to_quat;
According to this definition thrust_vec_correction_quatyaw_vec_correction_quat = att_from_quat.inverse()att_to_quat = _attitude_ang_error. This would mean that first we apply the yaw_vec_correction_quat, then apply the thrust_vec_correction_quat which is not like what youâve said that the algorithm first try to rotate the thrust vector, then try to rotate the yaw vector. IMO I think that this code should be there instead yaw_vec_correction_quatthrust_vec_correction_quat = att_from_quat.inverse()*att_to_quat. Therefore yaw_vec_correction_quat = att_from_quat.inverse()att_to_quatthrust_vec_correction_quat.inverse() and this code should replace the definition in line 615.