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

mc_att_control: reset integrals when disarmed

This commit is contained in:
Anton Babushkin 2014-01-21 19:02:03 +01:00
parent 29759ce092
commit bdf440e375
1 changed files with 8 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -638,6 +638,14 @@ MulticopterAttitudeControl::task_main()
/* feed forward yaw setpoint rate */ /* feed forward yaw setpoint rate */
rates_sp(2) += yaw_sp_move_rate * yaw_w; rates_sp(2) += yaw_sp_move_rate * yaw_w;
/* reset integral if disarmed */
// TODO add LANDED flag here
if (!_arming.armed) {
rates_int.zero();
}
/* rate controller */
math::Vector<3> rates_err = rates_sp - rates; math::Vector<3> rates_err = rates_sp - rates;
math::Vector<3> control = _params.rate_p.emult(rates_err) + _params.rate_d.emult(_rates_prev - rates) / fmaxf(dt, 0.003f) + rates_int; math::Vector<3> control = _params.rate_p.emult(rates_err) + _params.rate_d.emult(_rates_prev - rates) / fmaxf(dt, 0.003f) + rates_int;
_rates_prev = rates; _rates_prev = rates;