dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

mc_pos_control: AWU fixed

This commit is contained in:
Anton Babushkin 2014-01-20 10:26:43 +01:00
parent f4edb448dd
commit 9c6cc7a36b
1 changed files with 10 additions and 8 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

18
src/modules/mc_pos_control/mc_pos_control_main.cpp
View File

@ -828,16 +828,18 @@ MulticopterPositionControl::task_main()
} }
/* update integrals */ /* update integrals */
math::Vector<3> m; if (_control_mode.flag_control_velocity_enabled && !saturation_xy) {
m(0) = (_control_mode.flag_control_velocity_enabled && !saturation_xy) ? 1.0f : 0.0f; thrust_int(0) += vel_err(0) * _params.vel_i(0) * dt;
m(1) = m(0); thrust_int(1) += vel_err(1) * _params.vel_i(1) * dt;
m(2) = (_control_mode.flag_control_climb_rate_enabled && !saturation_z) ? 1.0f : 0.0f; }
thrust_int += vel_err.emult(_params.vel_i) * dt; if (_control_mode.flag_control_climb_rate_enabled && !saturation_z) {
thrust_int(2) += vel_err(2) * _params.vel_i(2) * dt;
/* protection against flipping on ground when landing */ /* protection against flipping on ground when landing */
if (thrust_int(2) > 0.0f) if (thrust_int(2) > 0.0f)
thrust_int(2) = 0.0f; thrust_int(2) = 0.0f;
}
/* calculate attitude and thrust from thrust vector */ /* calculate attitude and thrust from thrust vector */
if (_control_mode.flag_control_velocity_enabled) { if (_control_mode.flag_control_velocity_enabled) {