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

mc_att_control_vector: independent thrust vector and attitude control

Browse Source
This commit is contained in:
Anton Babushkin
2013-12-15 16:24:45 +04:00
parent f5c24c6e71
commit badf146e19
1 changed files with 42 additions and 3 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/mc_att_control_vector/mc_att_control_vector_main.cpp
+42 -3
View File
@@ -539,9 +539,48 @@ MulticopterAttitudeControl::task_main()
/* current body angular rates */
math::Vector3 rates(_att.rollspeed, _att.pitchspeed, _att.yawspeed);
/* orientation error between R and R_des */
math::Matrix e_R_m = (R_des.transpose() * R - R.transpose() * R_des).transpose() * 0.5f;
math::Vector3 e_R(-e_R_m(1, 2), e_R_m(0, 2), -e_R_m(0, 1));
/* try to move thrust vector shortest way, because yaw response is slower than roll/pitch */
/* angle error between current and desired thrust vectors (Z axes) */
math::Vector3 R_z(R(0, 2), R(1, 2), R(2, 2));
math::Vector3 R_z_des(R_des(0, 2), R_des(1, 2), R_des(2, 2));
math::Vector3 e_R = R_z.cross(R_z_des);
/* convert angle error from NED to bady frame */
e_R = R.transpose() * e_R;
/* e_R has zero Z component, set it according to yaw */
/* calculate rotation matrix after roll/pitch only rotation */
math::Matrix R_rp(3, 3);
float e_R_angle = e_R.norm();
if (e_R_angle > 0.0f) {
/* get axis-angle representation */
math::Vector3 e_R_axis = e_R / e_R_angle;
/* cross product matrix for e_R_axis */
math::Matrix e_R_cp(3, 3);
e_R_cp.setAll(0.0f);
e_R_cp(0, 1) = -e_R_axis(2);
e_R_cp(0, 2) = e_R_axis(1);
e_R_cp(1, 0) = e_R_axis(2);
e_R_cp(1, 2) = -e_R_axis(0);
e_R_cp(2, 0) = -e_R_axis(1);
e_R_cp(2, 1) = e_R_axis(0);
/* rotation matrix for roll/pitch only rotation */
math::Matrix I(3, 3);
I.setAll(0.0f);
I(0, 0) = 1.0f;
I(1, 1) = 1.0f;
I(2, 2) = 1.0f;
R_rp = R * (I + e_R_cp * sinf(e_R_angle) + e_R_cp * e_R_cp * (1.0f - cosf(e_R_angle)));
} else {
/* zero roll/pitch rotation */
R_rp = R;
}
/* R_rp and R_des has the same Z axis, calculate yaw error */
math::Vector3 R_des_z(R_des(0, 2), R_des(1, 2), R_des(2, 2));
math::Vector3 R_des_x(R_des(0, 0), R_des(1, 0), R_des(2, 0));
math::Vector3 R_rp_x(R_rp(0, 0), R_rp(1, 0), R_rp(2, 0));
e_R(2) = R_rp_x.cross(R_des_x) * R_des_z;
/* angular rates setpoint*/
math::Vector3 rates_sp = _K * e_R;