forked from Archive/PX4-Autopilot
mc_att_control: limit max yaw setpoint offset
This commit is contained in:
parent
c2f825647e
commit
42a7d80a81
|
@ -506,6 +506,14 @@ MulticopterAttitudeControl::control_attitude(float dt)
|
||||||
/* move yaw setpoint */
|
/* move yaw setpoint */
|
||||||
yaw_sp_move_rate = _manual_control_sp.r * _params.man_yaw_max;
|
yaw_sp_move_rate = _manual_control_sp.r * _params.man_yaw_max;
|
||||||
_v_att_sp.yaw_body = _wrap_pi(_v_att_sp.yaw_body + yaw_sp_move_rate * dt);
|
_v_att_sp.yaw_body = _wrap_pi(_v_att_sp.yaw_body + yaw_sp_move_rate * dt);
|
||||||
|
float yaw_offs_max = _params.man_yaw_max / _params.att_p(2);
|
||||||
|
float yaw_offs = _wrap_pi(_v_att_sp.yaw_body - _v_att.yaw);
|
||||||
|
if (yaw_offs < - yaw_offs_max) {
|
||||||
|
_v_att_sp.yaw_body = _wrap_pi(_v_att.yaw - yaw_offs);
|
||||||
|
|
||||||
|
} else if (yaw_offs > yaw_offs_max) {
|
||||||
|
_v_att_sp.yaw_body = _wrap_pi(_v_att.yaw + yaw_offs);
|
||||||
|
}
|
||||||
_v_att_sp.R_valid = false;
|
_v_att_sp.R_valid = false;
|
||||||
publish_att_sp = true;
|
publish_att_sp = true;
|
||||||
}
|
}
|
||||||
|
|
Loading…
Reference in New Issue