mc_att_control: allow commanding a yaw rate with zero throttle

Still avoiding to build up absolute yaw error in that case.
This commit is contained in:
Matthias Grob 2023-02-02 16:57:07 +01:00 committed by Silvan Fuhrer
parent 2b0f7879bc
commit f887ad6ebf
1 changed files with 4 additions and 5 deletions

View File

@ -112,12 +112,11 @@ MulticopterAttitudeControl::generate_attitude_setpoint(const Quatf &q, float dt,
vehicle_attitude_setpoint_s attitude_setpoint{};
const float yaw = Eulerf(q).psi();
// Avoid giving yaw rate setpoint with arming stick gesture
if ((_manual_control_setpoint.throttle > -.9f) || (_param_mc_airmode.get() == 2)) {
attitude_setpoint.yaw_sp_move_rate = _manual_control_setpoint.yaw * math::radians(_param_mpc_man_y_max.get());
} else {
attitude_setpoint.yaw_sp_move_rate = 0.f;
// Avoid accumulating absolute yaw error with arming stick gesture in case heading_good_for_control stays true
if ((_manual_control_setpoint.throttle < -.9f) && (_param_mc_airmode.get() != 2)) {
reset_yaw_sp = true;
}
// Make sure not absolute heading error builds up