|
|
|
@ -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 |
|
|
|
|