Browse Source

Heli: Prevent loss of yaw control during large angle recovery

zr-v5.1
Leonard Hall 5 years ago committed by Randy Mackay
parent
commit
368a8028ae
  1. 2
      libraries/AC_AttitudeControl/AC_AttitudeControl_Heli.cpp

2
libraries/AC_AttitudeControl/AC_AttitudeControl_Heli.cpp

@ -358,7 +358,7 @@ float AC_AttitudeControl_Heli::rate_target_to_motor_yaw(float rate_yaw_actual_ra @@ -358,7 +358,7 @@ float AC_AttitudeControl_Heli::rate_target_to_motor_yaw(float rate_yaw_actual_ra
float pid = _pid_rate_yaw.update_all(rate_target_rads, rate_yaw_actual_rads, _flags_heli.limit_yaw) + _actuator_sysid.z;
// use pid library to calculate ff
float vff = _pid_rate_yaw.get_ff();
float vff = _pid_rate_yaw.get_ff()*_feedforward_scalar;
// add feed forward
float yaw_out = pid + vff;

Loading…
Cancel
Save