diff options
| author | Anton Babushkin <anton.babushkin@me.com> | 2014-01-21 19:02:03 +0100 |
|---|---|---|
| committer | Anton Babushkin <anton.babushkin@me.com> | 2014-01-21 19:02:03 +0100 |
| commit | bdf440e37596e0ff756b48b31591d1e1defe3c64 (patch) | |
| tree | 5f4f801a57fbbc2570e5e201f5eb355d53b75817 /src/modules/mc_att_control/mc_att_control_main.cpp | |
| parent | 29759ce0925c7218458523df568a75a6cd073d0d (diff) | |
| download | px4-firmware-bdf440e37596e0ff756b48b31591d1e1defe3c64.tar.gz px4-firmware-bdf440e37596e0ff756b48b31591d1e1defe3c64.tar.bz2 px4-firmware-bdf440e37596e0ff756b48b31591d1e1defe3c64.zip | |
mc_att_control: reset integrals when disarmed
Diffstat (limited to 'src/modules/mc_att_control/mc_att_control_main.cpp')
| -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; |