forked from Archive/PX4-Autopilot
fw att control: reset integrators when requested via attitude setpoint topic
This commit is contained in:
parent
52d539d3cd
commit
9a911f7388
|
@ -707,14 +707,21 @@ FixedwingAttitudeControl::task_main()
|
||||||
float throttle_sp = 0.0f;
|
float throttle_sp = 0.0f;
|
||||||
|
|
||||||
if (_vcontrol_mode.flag_control_velocity_enabled || _vcontrol_mode.flag_control_position_enabled) {
|
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;
|
roll_sp = _att_sp.roll_body + _parameters.rollsp_offset_rad;
|
||||||
pitch_sp = _att_sp.pitch_body + _parameters.pitchsp_offset_rad;
|
pitch_sp = _att_sp.pitch_body + _parameters.pitchsp_offset_rad;
|
||||||
throttle_sp = _att_sp.thrust;
|
throttle_sp = _att_sp.thrust;
|
||||||
|
|
||||||
/* reset integrals where needed */
|
/* reset integrals where needed */
|
||||||
if (_att_sp.roll_reset_integral)
|
if (_att_sp.roll_reset_integral) {
|
||||||
_roll_ctrl.reset_integrator();
|
_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 {
|
} else {
|
||||||
/*
|
/*
|
||||||
* Scale down roll and pitch as the setpoints are radians
|
* Scale down roll and pitch as the setpoints are radians
|
||||||
|
|
Loading…
Reference in New Issue