From 5afb5b49563d7d495284617e444885a9ae0e3d85 Mon Sep 17 00:00:00 2001 From: Leonard Hall Date: Tue, 9 Jul 2019 22:12:16 +0930 Subject: [PATCH] AC_AttitudeControl: Prevent loss of yaw control during large angle recovery. --- libraries/AC_AttitudeControl/AC_AttitudeControl.cpp | 9 +++++---- libraries/AC_AttitudeControl/AC_AttitudeControl.h | 3 +++ .../AC_AttitudeControl/AC_AttitudeControl_Multi.cpp | 2 +- 3 files changed, 9 insertions(+), 5 deletions(-) diff --git a/libraries/AC_AttitudeControl/AC_AttitudeControl.cpp b/libraries/AC_AttitudeControl/AC_AttitudeControl.cpp index c24277b12a..d975bf2f1f 100644 --- a/libraries/AC_AttitudeControl/AC_AttitudeControl.cpp +++ b/libraries/AC_AttitudeControl/AC_AttitudeControl.cpp @@ -669,14 +669,15 @@ void AC_AttitudeControl::attitude_controller_run_quat() Quaternion desired_ang_vel_quat = to_to_from_quat.inverse() * attitude_target_ang_vel_quat * to_to_from_quat; // Correct the thrust vector and smoothly add feedforward and yaw input + _feedforward_scalar = 1.0f; if (_thrust_error_angle > AC_ATTITUDE_THRUST_ERROR_ANGLE * 2.0f) { _rate_target_ang_vel.z = _ahrs.get_gyro().z; } else if (_thrust_error_angle > AC_ATTITUDE_THRUST_ERROR_ANGLE) { - float feedforward_scalar = (1.0f - (_thrust_error_angle - AC_ATTITUDE_THRUST_ERROR_ANGLE) / AC_ATTITUDE_THRUST_ERROR_ANGLE); - _rate_target_ang_vel.x += desired_ang_vel_quat.q2 * feedforward_scalar; - _rate_target_ang_vel.y += desired_ang_vel_quat.q3 * feedforward_scalar; + _feedforward_scalar = (1.0f - (_thrust_error_angle - AC_ATTITUDE_THRUST_ERROR_ANGLE) / AC_ATTITUDE_THRUST_ERROR_ANGLE); + _rate_target_ang_vel.x += desired_ang_vel_quat.q2 * _feedforward_scalar; + _rate_target_ang_vel.y += desired_ang_vel_quat.q3 * _feedforward_scalar; _rate_target_ang_vel.z += desired_ang_vel_quat.q4; - _rate_target_ang_vel.z = _ahrs.get_gyro().z * (1.0 - feedforward_scalar) + _rate_target_ang_vel.z * feedforward_scalar; + _rate_target_ang_vel.z = _ahrs.get_gyro().z * (1.0 - _feedforward_scalar) + _rate_target_ang_vel.z * _feedforward_scalar; } else { _rate_target_ang_vel.x += desired_ang_vel_quat.q2; _rate_target_ang_vel.y += desired_ang_vel_quat.q3; diff --git a/libraries/AC_AttitudeControl/AC_AttitudeControl.h b/libraries/AC_AttitudeControl/AC_AttitudeControl.h index af857c1e85..a4e143338f 100644 --- a/libraries/AC_AttitudeControl/AC_AttitudeControl.h +++ b/libraries/AC_AttitudeControl/AC_AttitudeControl.h @@ -440,6 +440,9 @@ protected: // mix between throttle and hover throttle for 0 to 1 and ratio above hover throttle for >1 float _throttle_rpy_mix; + // Yaw feed forward percent to allow zero yaw actuator output during extreme roll and pitch corrections + float _feedforward_scalar = 1.0f; + // References to external libraries const AP_AHRS_View& _ahrs; const AP_Vehicle::MultiCopter &_aparm; diff --git a/libraries/AC_AttitudeControl/AC_AttitudeControl_Multi.cpp b/libraries/AC_AttitudeControl/AC_AttitudeControl_Multi.cpp index d25998f3e6..4069a2bdcb 100644 --- a/libraries/AC_AttitudeControl/AC_AttitudeControl_Multi.cpp +++ b/libraries/AC_AttitudeControl/AC_AttitudeControl_Multi.cpp @@ -338,7 +338,7 @@ void AC_AttitudeControl_Multi::rate_controller_run() _motors.set_pitch_ff(get_rate_pitch_pid().get_ff()); _motors.set_yaw(get_rate_yaw_pid().update_all(_rate_target_ang_vel.z, gyro_latest.z, _motors.limit.yaw) + _actuator_sysid.z); - _motors.set_yaw_ff(get_rate_yaw_pid().get_ff()); + _motors.set_yaw_ff(get_rate_yaw_pid().get_ff()*_feedforward_scalar); _rate_sysid_ang_vel.zero(); _actuator_sysid.zero();