There’s something about online Pixhawk. clear attitude control of the article, written are not very thorough because its attitude control algorithm looks part of the code are obscure error calculating the stance in the shaft, it is hard to in textbooks or online.Find relevant data, developers are purely their own basically without mentioning the need to know this information, I could find Chinese law experience trying to understand basic now, rotor flying of many recent study in Pixhawk its attitude control.In this paper, in principle, trying to succinctly explains my understanding.

I was before this article have been Pixhawk the overall framework made a generalization. code structure, the understanding.Rate of control, trying to use pixhawk powerful hardware and software platform that she wanted to do to expand to the thing is quite simple. Most of the personalized custom should be in control,.For example, when we go on a new design. The simulation on different surfaces, control channel with original rate of the control changes in existing aircraft, plane, only need to start relevant, and change of control or new to realize their own scripts.Posture estimates, and you can.. documents and mixerEstimated position and other modules don’t do anything to change completely complex, it’s Pixhawk news through in the publish / subscribe model to achieve the blocks is communication, and thereby achieve high the benefits of modular software architecture.

[
in advance knowledge

[1.1 to rotate the meaning of the matrix] (# 1.1).
[1.2 attitude control aircraft used in some rotation of representation] (# 1.2).
[the coordinate 1.2.1 the agreement] (# 1.2.1).
[1.2.2 said Eulerian angles] (# 1.2.2).
[said the number of yuan four 1.2.3] (# 1.2.3).
[1.2.4 to rotate Rodrigue formula] (# 1.2.4).
[the peak of the 1.3 profile error] (# 1.3).
[
resolve control stance Pixhawk

[control structure flew 2.1 Pixhawk] (# 2.1).
[2.2 Pixhawk resolve attitude control code] (# 2.2).
[error control of the angle 2.2.1] (# 2.2.1).
[control error rate angle 2.2.2] (# 2.2.2).
in advance knowledge.

Attitude control need a tutorial a lot of, I find it difficult to choose, I’m here, my knowledge, my rotating space to as much as possible in the most concise language to explain all need to use the knowledge of the principle of ( KISS, you know.^ _ ^ ).

<h3 id=“1.1 ”> 1.1 spinning the meaning of the matrix </ h3>.

There are two kinds of rules, the vector between the two matrix to represent relationship matrix : the meaning of rotation.

at a fixed coordinates to vector in the rotation, rotation before rotating coordinate vectors of the relationship ; and coordinate vectors.
the coordinate system to coordinate vectors of the relationship between the two coordinate system ; in rotation, with a vector.

We shall first by a simple two-dimensional spin to these two cases to the point of rotating three-dimensional space, this one by one matrix not too much a dimension, its significance to the same.

The first rotational vector, meaning.

A coordinate $$ {\ displaystyle P (x, y)} a plane every little $$ {\ displaystyle P} $$ \[, and should be a vector $$ {\ displaystyle (x, y)} all common sense. $$ of the facing weight consisting of a space.Every vector can be expressed as two real number because, as ℝ ² an ordered array of $$ {\ displaystyle (x, y)} in vector space. $$ ℝ ², coordinate system will be fixed within a given vector $$ \ mathbf {v_0} $$ ( in this case.To $$ (1, 0) ^ T $$ ) the Counterclockwise $$ \ theta $$ to $$ \ mathbfᆪ\] (In this example to $$ ( \ cos\theta \ sin\theta) ^T $$ ), but are as follows :.

$$ \ mathbf {v ’} = \ mathbf {R_\theta} \mathbf {v_0} to $$.

On the type of $$ \ mathbf {R_\theta} : $$.

$$ \ mathbf {R_\theta}, =. {\ begin{bmatrix}. \ cos\theta & - \ sin\theta \. \ sin\theta & \ cos\theta. \ end {bmatrix}}. $$.

< ! – $$ \ mathbf {v ’} = \ mathbf {R’_\theta} \mathbf {v_0} to $$.

$$ \ mathbf {R’_\theta}, =. {\ begin{bmatrix}. \ cos\theta & \ sin\theta \. - \ sin\theta & \ cos\theta. \ end {bmatrix}}. $$ – >.

Rotary coordinate system : the meaning of # * ** the second.

This paper mainly discuss the kind of practices that, because in the attitude control aircraft will be used more coordinates coordinate system the earth : (inertial system), we need always with department and so on, we need to know … (not an inertial frame) (speed.Angular velocity, acceleration and transform relations, understand the meaning of this kind of representation between the coordinate system, etc) in these very important.

Are still rotated from the simple two-dimensional, shown as follows :.

A matrix can express different coordinate system has always been with the amount of the coordinates of the above diagram as an example. The relationship between $$ \ mathbfᆫ= (- \ sin\theta, \ cos\theta) ^T $$ are vectors and : $$ \ mathbfᆪ= (1, 0) ^T $$ ** V ** in coordinates.Tie OX <Sub> 0 </ sub> Y <Sub> 0 </ sub> and vector coordinates, they are : OX’Y ’.

$$ \ mathbf {v_0} = \ mathbf {R_\theta} \mathbf {v ’} to $$.

$$ \ mathbf {R_\theta}, =. {\ begin{bmatrix}. \ cos\theta & - \ sin\theta \. \ sin\theta & \ cos\theta. \ end {bmatrix} } $$.

Matrix elements can be regarded as two each in a coordinate axis within the vector angle This, too, is known as spin, the cosine of the matrix for direction cosine of the cause of the matrix (Direction Cosine Matrix).

3 - D spinning a 2 D rotates much more complex. In three-dimensional space, the coordinate system in the earth and airframe coordinates the rotation of the relationship between the coordinate system, for example, the earth as the body coordinate system for, Oxyz OXYZ ( coordinate system.An appointment to see [Section 1.2.1] (# 1.2.1)), y, j, y, coordinate system in x, y, z axis of the earth, a unit of Vector ** I, J, Y as the body coordinate system in X, Y, Z axis of the unit of vectors and vector to ensure the body coordinate system and the coordinate system in the earth.Coordinates of, respectively $$ \ mathbf {v^G} the relationship between the amount to $$, they shall not two, {v^B} $$ as follows :.

$$ \ begin {equation} \mathbf {v^G} = \ begin {bmatrix}. \ mathbf {v_x^G} ‘. \ mathbf {v_y^G} ’. \ mathbf {v_z^G}. \ end {bmatrix} = \ begin {bmatrix}. \ mathbf {I} \cdot\mathbf {I} & \ mathbf {I} \cdot\mathbf {J} & \ mathbf {I} \cdot\mathbf {K} ‘. \ mathbf {j} \cdot\mathbf {I} & \ mathbf {j} \cdot\mathbf {J} & \ mathbf {j} \cdot\mathbf {K} ’. \ mathbf {k} \cdot\mathbf {I} & \ mathbf {k} \cdot\mathbf {J} & \ mathbf {k} \cdot\mathbf {K}. \ end {bmatrix} \begin {bmatrix}. \ mathbf {v_x^B} ‘. \ mathbf {v_y^B} ’. \ mathbf {v_z^B}. \ end {bmatrix}, =. \ mathbf {R_B^G}. \ mathbf {v^B} \label {1eq}. \ end {equation} to $$.

a matrix $$ \ mathbf {R_B^G} each element of $$ for the coordinate two in angle between axes of cosine (that is, [take.] the product quantity ( https://zh.wikipedia.org/wiki /) listed three of its vector.) respectively as the body coordinate system.X, Y, Z axis in the coordinate system within Oxyz the coordinates of the vector ; the vector for the line three coordinates x, y, z axis within the body coordinate system coordinate vectors. # :.

$$ \ begin {equation} \mathbf {R_B^G}, =. \ begin {bmatrix}. \ mathbf {I^G} & \ mathbf {J^G} & \ mathbf {K^G}. \ end {bmatrix}, =. \ begin {bmatrix}. \ mathbf {i^B} ‘. \ mathbf {j^B} ’. \ mathbf {k^B}. \ end {bmatrix} \label {2eq}. \ end {equation} to $$.

Orthonormal inverse matrix, the matrix as a matrix for its transpose matrix : $$ \ mathbf {R^{T} }=\mathbf {R^{–1} } $$. take advantage of this nature can be quickly within computer for the reverse.
Please be sure to remember clearly spinning the meaning of the matrix and its nature will not, and two switching sequence is get the coordinate system.

This will be next to, according to various rotary calculation method that the matrix.

Reference materials : in this section.
http://mathworld.wolfram.com/RotationMatrix.html.
https://gentlenav.googlecode.com/files/DCMDraft2.pdf.
http://www.starlino.com/dcm_tutorial.html.
Randal W McLain _ Small Unmanned Aircraft : Theory and Practice _……. Beard & Timothy W

Attitude control used in aircraft <h3 id=“1.2 ”> 1.2 of several rotating method </ h3>.

This spinning … It’s so much more complicated than the 2 D, 3 - D control with more we flew in to the rotation, because 2 - D rotates only have a rotation axis two-dimensional, three-dimensional rotated to any space around the axis.Eulerian angles and the number rotating 3 - D special case. In addition to the matrix, we can just rotate with four yuan to express such as rotating, they have a certain equivalence relation.

<h4 id=“1.2.1 ”> 1.2.1 </ h4> the coordinate system agreement.

The coordinate system usually adopt NED ( North east down).
The coordinate system is generally defined as shown, roll, pitch, Y axis for X, Y, Z axis : respectively.

Right hand from spinning in the direction guided by laws and coordinate system in the right direction.

<h4 id=“1.2.2 ”> 1.2.2 </ h4>. says Eulerian angles

Eulerian angles and spins because for the same rotation times, said many agree on a rotating angle to pull Europe and China order is closely related to the #, reference here..

Aeronautical and astronautical engineering in the common gauge submission z-y′-x ″ , shown as follows :.

The body coordinate system to rotate the coordinate system from the earth oxyz oXYZ : order.

to rotate around oz $$ \ psi $$ ;.
to rotate around oy ′ $$ \ theta $$ ;.
around ox ″ (that is, oX) a rotating $$ \ phi $$.

$$ \ psi $$, $$ \ theta $$, respectively for yaw, pitch and roll angle or $$ \ phi $$ (roll, pitch, yaw).

If the coordinate system, is the opposite of order and the earth rotates from the body to the rotation of the negative direction.

Rotary matrix, angle can be three to pass through this have to.

$$ \ beginᆹ\mathbf ᆺ = \ begin ᆻ c (\ psi) c (\ theta) & c (\ psi) s (\ phi) s (\ theta) - c (\ phi) s (\ psi) & s (\ phi) s (\ psi) + c (\ phi) c (\ psi) s (\ theta) \ c (\ theta) s (\ psi) & c (\ phi) c (\ psi) + s (\ phi).S (\ psi) s (\ theta) & c (\ phi) s (\ psi) s (\ theta) - c (\ psi) s (\ phi) \ - s (\ theta) & c (\ theta) s (\ phi) & c (\ phi) c (\ theta) \ end {bmatrix} \label {3eqEulerAngles}. \ end {equation} to $$.

Type with $$ c (\ alpha) $$ said $$ \ cos (\ alpha) $$ s, $$ (\ alpha) $$ said $$ \ sin (\ alpha) to $$.

< ! –. http://www.chrobotics.com/library/understanding-euler-angles. Matrix of this formula will be in this Web site for $$ R_I^B $$ $$ R_B^I $$ – >, should be.

<h4 id=“1.2.3 ”> 1.2.3 </ h4> four the number of yuan. Day using the euro pulled on the corner locked to a million problems and rotating three times, needs to coordinate rotating between the two said ( Gimbal Lock). between any two coordinate system and the actual.Only one rotation through expressed as a quaternary number, to complete this rotation, and the rotation axis that spins out of the rotation can be intuitive to.

Quaternary number usually expressed as a vector, within the $$ \ mathbb {R} ^4 $$. $$ \ mathbf {e} = \ begin {pmatrix}. E_0 \ e_1 \ e_2 \ e_3. \ end {pmatrix}. $$.

Coordinate system to the earth from the body coordinate system with cash of $ spiral that four cases to the definition of the number of units within the coordinates to the earth : Let the spinning axis of rotation in the amount for : $$ (u_x u_y, u_z) ^ \ mathrm {T} rotation angle, $$ for.For four the number of elements : four out of., but $$ \ Theta $$

$$ e_0 = \ cos\frac\Theta2 $$.

$$ \ begin {pmatrix}. E_1 \ e_2 \ e_3. \ end {pmatrix} = \ begin {pmatrix}. U_x \ u_y \ u_z. \ end {pmatrix} \sin\frac\Theta2. $$.

(Wikipedia in the rotation) said the derivation of a rotating space of four yuan of yuan and four ](https://zh.wikipedia.org/wiki/ more simply, in which to dwell on it.

<! – $$ q=e^{\frac\Theta2 ( u_xi+u_yj+u_zk ) } = \ cos\frac\Theta2 + ( u_xi+u_yj+u_zk ) \ sin\frac\Theta2 $$ –>.

In fact, rotating matrix can directly calculate the angle we already know through Europe pulled in 1.2.2 Day by four yuan number also can calculate the European angle, so the four yuan, Eulerian angles, rotating matrix between can be converted.

Reference materials : in this section.
Rotating space and $ 4 [https://zh.wikipedia.org/wiki/ ](https://zh.wikipedia.org/wiki/ four yuan and rotating space).
https://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles.
Randal W McLain _ Small Unmanned Aircraft : Theory and Practice _……. Beard & Timothy W

<h4 id=“1.2.4 ”> 1.2.4 formula revolving Rodrigue </ h4>. If you have already know the rotation axis $$ \ mathbf {u} rotation angle and $$ (as a unit of vectors) $$ \ Theta $$, we can by calculating quaternary number of rotating out of the structural matrix, but the most simple method is Rodrigue [ the use of rotating the public directly.Type ](https://zh.wikipedia.org/wiki/ rotating formula), Rodrigue, of course, the equivalence at all.

Between two coordinates the rotation of the matrix $$ \ mathbf {R} : $$.

$$ \ beginᆪᆫ}=ᆬ}+ (\ sin \ Theta) {\ mathbf { U} (1-\cos \ Theta) {\ mathbf { U} }^ {2} \label {4eqrodrig}. \ end {equation} to $$.

: writing can be.

$$ \ begin {equation} \mathbf {R} = (\ cos \ Theta) \ mathbf {I} + (\ sin \ Theta) {\ mathbf { U} } + (1-\cos \ Theta) \ mathbf {u} \ otimes \ mathbf {u}. \ label {5eqrodrig}. \ end {equation} to $$.

If the coordinate system to the body coordinate system of rotating the earth continued to indicate the type : $$ \ mathbf {u}, $$ for the rotation axis in the earth rotation angle, coordinate system in the coordinates of the vector for $$ \ Theta \[, it is a calculation result is.\] \ mathbf {R_B^G} to $$.

Two on the type matrix in the $$ {\ mathbf { U} } $$ for vector $$ \ mathbf {u} of $$ a product of matrix ( cross product matrix):.

$$ {\ mathbf { U} } = {\ begin{bmatrix} 0&-u_ {z} &u _ {y} ‘. U_ {z} &0&-u _ {x} ’. - u_ {y} &u_ {x} &0. \ end {bmatrix} } $$.

$$ \ mathbf {u} \ otimes \ mathbf {u}, $$ for vector $$ \ mathbf {u} $$ tensor product of its own. : (vector product of columns and to the line can also be considered u)

$$ \ mathbf {u} \ otimes \ mathbf {u} = {\ begin{bmatrix} u_ {x} ^ {2} &u_ {x} u_ {y} &u_ {x} u _ {z} ‘. U_ {x} u_ {y} &u_ {y} ^ {2} &u_ {y} u _ {z} ’. U_ {x} u_ {z} &u_ {y} u_ {z} &u_ {z} ^ {2} \end {bmatrix} } $$.

As a result of $$ \ mathbf {u}, $$ for a unit of Vector, you can see the matrix $$ {\ mathbf { U} } ^ {2} the $$ and matrix $$ \ mathbf {u} \ otimes \ mathbf {u} the difference $$ for the unit, so $$ \ eqref {4eqrodrig} and $$ $$ \ eqref {5eqrodrig} $$ equivalent..

1.3 for error posture <h3 id=“1.3 ”> </ h3>.

Each posture shaft rotation axis must pass through errors can be # of # : the product of vectors and rotation angle.

$$ \ begin {equation} \mathbf {error correction signaling} = \ begin {pmatrix}. U_x \ u_y \ u_z. \ end {pmatrix} \Theta. \ label {6eqerror}. \ end {equation} to $$.

<where do I find the reference matrix calculation forms, ! – ** TODO ** –>.

Analytic control stance. Pixhawk 2 <h2 id=“2 ”> </ h2>.

A rotating in front of matrix and other related knowledge of the setup, and anti-feudalism rotor attitude control is not particularly difficult for many by telling Pixhawk.

<h3 id=“2.1 ”> 2.1 Pixhawk and control system diagram </ h3>.

Pixhawk and control system diagram to represent as follows :.

Attitude control link to the innermost layer, outer layer, in turn, for route and position control link. Fig. For more on the rotor, different configuration different, for all flight control aircraft and vertical take-off and landing (VTOL) the difference is a matter of attitude.(Attitude) and control and location estimate (Estimator) (Position) (Controller) module will use different algorithm using attitude control module can be estimated location aircraft, so you know how to use different configuration different module. reference.The form .

<h3 id=“2.2 ”> 2.2 Pixhawk attitude control analytic </ h3>.

Pixhawk in the rotor corresponding to the source file for attitude control module.Angle error control and is the key part of its Firmware/src/modules/mc_att_control/mc_att_control_main.cpp..Angular rate of error control, but said : the following figure.

Corner angle error control and control error rate in source document should be to control_attitude (float dt) control_attitude_rates and (float dt) function.
In this module the accelerator not under control, not to discuss basic (Thrust or Throttle).

<h4 id=“2.2.1 ”> </ h4>. control error angle 2.2.1 <! – ad.hoc –>. The link of function by function control_attitude (float dt) its enter the plane for the current stance and objective attitude. to complete the attitude of the two, to pass through this rotating posture three out of the shaft’s point of view matrix calculation error ratio.Angular rate goal output for the weights of feedforward control, as a control, and the heading angle error (yaw error).
The most difficult part of the this control link is rotating matrix calculation error angle # # based on the current stance and objective stance two, this is completely part of the developers Anton Babushkin his own ideas.](https://groups.google.com/forum/#!Ready reference papers or information. The basic guiding principles is :., topic/px4users/ln3yzqF6Vz0 )

as a result of the rotor roll and pitch of yaw the response speed control, so the first pitch and roll, the Z axis of the aircraft with the target attitude z z axis of rotated axis, the last, to eliminate yaw deviation, the Basis of this method to calculate each.Profile error. Eulerian angles by 2. posture Z axis in target levels, the singularity ; error, neglect yaw (singularity).
stance stance and target if the current Z threadless corner of a big, mean pitch and roll needs greater the amount of control. At that time, directly based on four elements to calculate the profile error ; and the error and 1 in front of ways.Calculate the error from their respective weight final summation, the profile error.

<This # or table of the coordinate system a few ! – ** TODO ; –>.

Void.
MulticopterAttitudeControl::control_attitude (float dt).
{.
     vehicle_attitude_setpoint_poll ().

     _ thrust_sp = _ v_att_sp.thrust ;.

     / * construct attitude setpoint rotation matrix * /.
     math::Matrix &lt;3, 3&gt; R_sp ;.
     R_sp.set (_ v_att_sp.R_body).

     / * get current rotation matrix from control state quaternions * /.
     math::Quaternion q_att (_ ctrl_state.q [0], _ ctrl_state.q [1], _ ctrl_state.q [2], _ ctrl_state.q [3]).
     math::Matrix &lt;3, 3&gt; R = q_att.to_dcm ().

     / * all input data is ready run controller itself * /.

This is main piece of code for :.

the current posture of the body coordinate system to the ground coordinates the rotation of the matrix $$ {\ mathbf { R_B^G} } $$ (in the code for ** R **).
goal form the body coordinate system to the ground coordinates the rotation of the matrix $$ {\ mathbf { R_{sp} ^ G }} $$ (in the code for ** R_sp **).

So far all the input the next step by step, ready for you to calculate the posture axis ’ angle error.

Have we mentioned before, rotating the body, so that the current attitude attitude attitude and the goal only this coincidence is the Z axis, the Z axis of body and goals, aunt and this posture for error yaw ** rp manner # The following code will be.It will be from the current posture rotated to ** rp attitude # axis of rotation.

     / * try to move thrust vector shortest way because yaw response is slower than roll/pitch * /.
     math::Vector &lt;3&gt; R_z (2, R ( 0), R R, (1, 2) (2, 2) ) ;.
     math::Vector &lt;3&gt; R_sp_z (2, R_sp ( 0), R_sp (1, 2), R_sp (2, 2) ) ;.

     / * axis and sin (angle) of desired rotation * /.
     math::Vector &lt;3&gt; e_R = R.transposed (). ; * (R_z % R_sp_z)

     / * calculate angle error * /.
     float e_R_z_sin = e_R.length ().
     float e_R_z_cos = R_z * R_sp_z ;.

    .
     / * calculate weight for yaw control * /.
     float yaw_w = R_sp (2, 2) * R_sp (2, 2).

Note that in the code ** % **, operator was overloaded, to fork is in terms of volume, respectively, by and, under the same.
According to the front of the [1.1’s Day] (# 1.1), ** R_z ** and goals respectively the current target the body posture the Z axis in the earth and the Z axis body coordinate system coordinates and fork by the vector. and the two vectors vertical, that is, for the current body of the Z axis.Vector in the coincidence is the Z axis of rotary axis of rotation in the earth’s stance and target coordinates, and ** R.transposed () ** after which you, this axis of rotation in the current body coordinate system in the coordinates of the vector or ** e_R **..Vector and fork by and by definition, it’s easy to understand e_R_z_sin, e_R_z_cos respectively the current Z axis body with the target body angle is the Z axis of sine, cosine and value value.

** R_sp (2, 2) ** the objective of the body posture z z axis of the earth and angle is the cosine of the behind as an analytic hierarchy process (SAHP), will be. ** yaw_w ** later explain.

     / * calculate rotation matrix after roll/pitch only rotation * /.
     math::Matrix &lt;3, 3&gt; R_rp ;.

     if (e_R_z_sin &gt; 0.0f) {.
         / * get axis-angle representation * /.
         float e_R_z_angle = atan2f (e_R_z_cos, e_R_z_sin).
         math::Vector &lt;3&gt; e_R_z_axis = e_R / e_R_z_sin ;.

         e_R = e_R_z_axis * e_R_z_angle ;.

         / * cross product matrix for e_R_axis * /.
         math::Matrix &lt;3, 3&gt; e_R_cp ;.
         e_R_cp.zero ().
         e_R_cp (0, 1) = - e_R_z_axis (2).
         e_R_cp (2, 0) = e_R_z_axis (1).
         e_R_cp (1, 0) = e_R_z_axis (2).
         e_R_cp (1, 2) = - e_R_z_axis (0).
         e_R_cp (2, 0) = - e_R_z_axis (1).
         e_R_cp (2, 1) = e_R_z_axis (0).

         / * rotation matrix for roll/pitch only rotation * /.
         R_rp = R * (_ I + e_R_cp * e_R_z_sin + e_R_cp * e_R_cp * ( 1.0f - e_R_z_cos) ) ;.

     } else {.
         / * zero roll/pitch rotation * /.
         R_rp = R ;.
     }.

e_R_z_sin is not negative attitude rotated to the code to calculate from the line (because of a fork by definition in the two vector of angle for 0₁₈₀ °), so the Z axis if there is error, 6₉ rp the attitude of the rotary axial (a unit of the), and rotating.Angle, from the current posture or e_R rp the attitude of the posture axis of the error. Pay attention to this vector are in the current posture of a few lines of code within coordinate system.

The 12₂₂ code that is, according to the rotation angle and rotating shaft, take advantage of Rodrigue formula of the rotation matrix. 22 line equation on the right side The outermost layer of brackets is in the rp attitude to the current posture the rotation of the matrix, multiplied by R., rp posture the coordinate system to the coordinate system of rotating as ** R_rp ** the matrix.

    / * R_rp and R_sp has the same Z axis calculate yaw error * /.
     math::Vector &lt;3&gt; R_sp_x (0, R_sp ( 0), R_sp (1, 0), R_sp (2, 0) ) ;.
     math::Vector &lt;3&gt; R_rp_x (0, R_rp ( 0), R_rp (1, 0), R_rp (2, 0) ) ;.
     e_R (2) = atan2f (( R_rp_x % R_sp_x) * R_sp_z R_rp_x * R_sp_x ) * yaw_w ;.

     if (e_R_z_cos &lt; 0.0f) {.
         / * for large thrust vector rotations use another rotation method :.
         * calculate angle and axis for R - &gt; R_sp rotation directly * /.
         math::Quaternion q_error ;.
         q_error.from_dcm (R.transposed () * R_sp ) ;.
         math::Vector &lt;3&gt; e_R_d = q_error (0) &gt; = 0.0f? q_error.imag () * 2.0f : - q_error.imag () * 2.0f ;.

         / * use fusion of Z axis based rotation and direct rotation * /.
         float direct_w = e_R_z_cos * e_R_z_cos * yaw_w ;.
         e_R = e_R * (1.0f - direct_w) + e_R_d * direct_w ;.
     }.

The calculated bit of code in the U.S. second. First of all objective stance the Z axis and the current posture. angle error is the Z axis in front of posture Z axis and the target coordinates to mention yaw_w is cosine the square of the value angle, z axis for analytic hierarchy process (SAHP). " When the two.Angle of 0, yaw_w 1 ; by Eulerian angles of the singularity, and cause the angle of 90 ° yaw coincide with the pitch, do not need to control at this time, yaw yaw_w. understand this point, 0 for the three lines of code also is not difficult to understand.

If the target attitude the Z axis with the present posture Z axis different point of view smaller (± 90 °), according to the rotation twice the method of calculating the amount that is, to e_R error to a stance the axis for the final.
If the target attitude the Z axis with the present posture Z axis different angles only consider a spin, but larger (90₂₇₀ °) (I don’t need the middle of the rp attitude), using three-axis quaternary number of direct calculation error angle.

The 9₁₁ line code by four yuan would be counted out of the posture three out of the rotation angle and the rotation axis, thus directly to a goal from the current posture to posture shaft angle error of the vector ** e_R_d **.

The 180,000 lines of code, will pass through two different methods to calculate the error of angle error angle of the respective weight, computed final vector to vector ** e_R_d e_R ** (here is not particularly understand.).

At this point, he had gone completely calculation came to the posture axis ’ angle error calculating error, according to the corner out of the target rate.

/* calculate angular rates setpoint */ _rates_sp = _params.att_p.emult(e_R);

/* limit rates */
for (int i = 0; i < 3; i++) {
    if (_v_control_mode.flag_control_velocity_enabled && !_v_control_mode.flag_control_manual_enabled) {
        _rates_sp(i) = math::constrain(_rates_sp(i), -_params.auto_rate_max(i), _params.auto_rate_max(i));
    } else {
        _rates_sp(i) = math::constrain(_rates_sp(i), -_params.mc_rate_max(i), _params.mc_rate_max(i));
    }
}

/* weather-vane mode, dampen yaw rate */
if (_v_att_sp.disable_mc_yaw_control == true && _v_control_mode.flag_control_velocity_enabled && !_v_control_mode.flag_control_manual_enabled) {
    float wv_yaw_rate_max = _params.auto_rate_max(2) * _params.vtol_wv_yaw_rate_scale;
    _rates_sp(2) = math::constrain(_rates_sp(2), -wv_yaw_rate_max, wv_yaw_rate_max);
    // prevent integrator winding up in weathervane mode
    _rates_int(2) = 0.0f;
}

/* feed forward yaw setpoint rate */
_rates_sp(2) += _v_att_sp.yaw_sp_move_rate * yaw_w * _params.yaw_ff;

/* weather-vane mode, scale down yaw rate */
if (_v_att_sp.disable_mc_yaw_control == true && _v_control_mode.flag_control_velocity_enabled && !_v_control_mode.flag_control_manual_enabled) {
    float wv_yaw_rate_max = _params.auto_rate_max(2) * _params.vtol_wv_yaw_rate_scale;
    _rates_sp(2) = math::constrain(_rates_sp(2), -wv_yaw_rate_max, wv_yaw_rate_max);
    // prevent integrator winding up in weathervane mode
    _rates_int(2) = 0.0f;
}

2.2.2 angular rate error control

In the previous section, we have obtained the target angular rate, which is given by the pose estimation module. The input of the angular rate error control link is the two angular rate. The output is the control commands of each attitude axis. The control code is easy to see the control block diagram The source code is no longer listed here, as follows:

It should be noted here in addition to feedforward than the figure PI controller and the differential part is not a typical PID controller, since this part of the differential input current angular rate, rather than the angular rate error can be so understanding this differential part of , If the current angular acceleration is too large, should be controlled to reduce it, that is, the current angular rate differential negative feedback.