diff options
Diffstat (limited to 'src/modules/mc_att_control')
| -rw-r--r-- | src/modules/mc_att_control/mc_att_control_main.cpp | 8 |
1 files changed, 8 insertions, 0 deletions
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 91eabdada..245ac024b 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; |