fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +08:00
Code Issues Packages Projects Releases Wiki Activity

mc_att_control: add more comments to the code, better explaining what it does

Browse Source 
No semantic changes.
This commit is contained in:
Beat Küng 2017-09-21 13:23:20 +02:00 committed by Matthias Grob
parent 69b1bfca6c
commit 2f1ca409b7
1 changed files with 6 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

10
src/modules/mc_att_control/mc_att_control_main.cpp
View File

@ -874,12 +874,13 @@ MulticopterAttitudeControl::control_attitude(float dt)
math::Vector<3> R_z(R(0, 2), R(1, 2), R(2, 2));
math::Vector<3> R_sp_z(R_sp(0, 2), R_sp(1, 2), R_sp(2, 2));
/* axis and sin(angle) of desired rotation */
/* axis and sin(angle) of desired rotation (indexes: 0=pitch, 1=roll, 2=yaw).
* This is for roll/pitch only (tilt), e_R(2) is 0 */
math::Vector<3> 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;
float e_R_z_sin = e_R.length(); // == sin(tilt angle error)
float e_R_z_cos = R_z * R_sp_z; // == cos(tilt angle error) == (R.transposed() * R_sp)(2, 2)
/* calculate weight for yaw control */
float yaw_w = R_sp(2, 2) * R_sp(2, 2);
@ -912,9 +913,10 @@ MulticopterAttitudeControl::control_attitude(float dt)
R_rp = R;
}
/* R_rp and R_sp has the same Z axis, calculate yaw error */
/* R_rp and R_sp have the same Z axis, calculate yaw error */
math::Vector<3> R_sp_x(R_sp(0, 0), R_sp(1, 0), R_sp(2, 0));
math::Vector<3> R_rp_x(R_rp(0, 0), R_rp(1, 0), R_rp(2, 0));
/* calculate the angle between R_rp_x and R_sp_x (yaw angle error), and apply the yaw weight */
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 < 0.0f) {