From bdf440e37596e0ff756b48b31591d1e1defe3c64 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Tue, 21 Jan 2014 19:02:03 +0100 Subject: [PATCH] mc_att_control: reset integrals when disarmed --- src/modules/mc_att_control/mc_att_control_main.cpp | 8 ++++++++ 1 file changed, 8 insertions(+) diff --git a/src/modules/mc_att_control/mc_att_control_main.cpp b/src/modules/mc_att_control/mc_att_control_main.cpp index 91eabdada3..245ac024b8 100644 --- a/src/modules/mc_att_control/mc_att_control_main.cpp +++ b/src/modules/mc_att_control/mc_att_control_main.cpp @@ -638,6 +638,14 @@ MulticopterAttitudeControl::task_main() /* feed forward yaw setpoint rate */ 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> control = _params.rate_p.emult(rates_err) + _params.rate_d.emult(_rates_prev - rates) / fmaxf(dt, 0.003f) + rates_int; _rates_prev = rates;