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

mc_att_control_vector, multirotor_pos_control: fixed

Browse Source
This commit is contained in:
Anton Babushkin
2013-12-22 12:20:06 +04:00
parent ef5aa697c7
commit fae6c69423
2 changed files with 10 additions and 9 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
+1 -1
View File
@@ -630,7 +630,7 @@ MulticopterAttitudeControl::task_main()
math::Vector3 rates_sp = _K * e_R;
/* feed forward yaw setpoint rate */
rates_sp(2) += yaw_sp_move_rate;
rates_sp(2) += yaw_sp_move_rate * yaw_w;
math::Vector3 control = _K_rate_p * (rates_sp - rates) + _K_rate_d * (_rates_prev - rates) / fmaxf(dt, 0.003f);
_rates_prev = rates;
src/modules/multirotor_pos_control/multirotor_pos_control.c
+9 -8
View File
@@ -764,12 +764,13 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[])
float body_y[3];
float body_z[3];
if (thrust_abs > 0.0f) {
if (thrust_abs > SIGMA) {
body_z[0] = -thrust_sp[0] / thrust_abs;
body_z[1] = -thrust_sp[1] / thrust_abs;
body_z[2] = -thrust_sp[2] / thrust_abs;
} else {
/* no thrust, set Z axis to safe value */
body_z[0] = 0.0f;
body_z[1] = 0.0f;
body_z[2] = 1.0f;
@@ -781,13 +782,7 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[])
y_C[1] = cosf(att_sp.yaw_body);
y_C[2] = 0;
if (fabsf(body_z[2]) < SIGMA) {
/* desired thrust is in XY plane, set X downside to construct correct matrix,
* but yaw component will not be used actually */
body_x[0] = -body_x[0];
body_x[1] = -body_x[1];
body_x[2] = -body_x[2];
} else {
if (fabsf(body_z[2]) > SIGMA) {
/* desired body_x axis = cross(y_C, body_z) */
cross3(y_C, body_z, body_x);
@@ -798,6 +793,12 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[])
body_x[2] = -body_x[2];
}
normalize3(body_x);
} else {
/* desired thrust is in XY plane, set X downside to construct correct matrix,
* but yaw component will not be used actually */
body_x[0] = 0.0f;
body_x[1] = 0.0f;
body_x[2] = 1.0f;
}
/* desired body_y axis = cross(body_z, body_x) */