diff options
author | Thomas Gubler <thomasgubler@gmail.com> | 2014-06-24 17:40:56 +0200 |
---|---|---|
committer | Thomas Gubler <thomasgubler@gmail.com> | 2014-06-24 17:40:56 +0200 |
commit | 9a911f7388e1a48630469fd2e875f00a119c829a (patch) | |
tree | 2321c8b649dbd14f08233da6b62d4c7b65acfa40 /src/modules | |
parent | 52d539d3cdf56eb4145dbf29c407bb0a1a7eebe5 (diff) | |
download | px4-firmware-9a911f7388e1a48630469fd2e875f00a119c829a.tar.gz px4-firmware-9a911f7388e1a48630469fd2e875f00a119c829a.tar.bz2 px4-firmware-9a911f7388e1a48630469fd2e875f00a119c829a.zip |
fw att control: reset integrators when requested via attitude setpoint topic
Diffstat (limited to 'src/modules')
-rw-r--r-- | src/modules/fw_att_control/fw_att_control_main.cpp | 11 |
1 files changed, 9 insertions, 2 deletions
diff --git a/src/modules/fw_att_control/fw_att_control_main.cpp b/src/modules/fw_att_control/fw_att_control_main.cpp index 178b590ae..3cd4ce928 100644 --- a/src/modules/fw_att_control/fw_att_control_main.cpp +++ b/src/modules/fw_att_control/fw_att_control_main.cpp @@ -707,14 +707,21 @@ FixedwingAttitudeControl::task_main() float throttle_sp = 0.0f; if (_vcontrol_mode.flag_control_velocity_enabled || _vcontrol_mode.flag_control_position_enabled) { + /* read in attitude setpoint from attitude setpoint uorb topic */ roll_sp = _att_sp.roll_body + _parameters.rollsp_offset_rad; pitch_sp = _att_sp.pitch_body + _parameters.pitchsp_offset_rad; throttle_sp = _att_sp.thrust; /* reset integrals where needed */ - if (_att_sp.roll_reset_integral) + if (_att_sp.roll_reset_integral) { _roll_ctrl.reset_integrator(); - + } + if (_att_sp.pitch_reset_integral) { + _pitch_ctrl.reset_integrator(); + } + if (_att_sp.yaw_reset_integral) { + _yaw_ctrl.reset_integrator(); + } } else { /* * Scale down roll and pitch as the setpoints are radians |