Browse Source

fw att control: reset integrators when requested via attitude setpoint topic

sbg
Thomas Gubler 11 years ago
parent
commit
9a911f7388
  1. 11
      src/modules/fw_att_control/fw_att_control_main.cpp

11
src/modules/fw_att_control/fw_att_control_main.cpp

@ -707,14 +707,21 @@ FixedwingAttitudeControl::task_main() @@ -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

Loading…
Cancel
Save