aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

AC_AttitudeControl: Prevent loss of yaw control during large angle recovery.

Browse Source
This commit is contained in:
Leonard Hall 2019-07-09 22:12:16 +09:30 committed by Randy Mackay
parent 668b30e8e9
commit 2038a6a61b
3 changed files with 9 additions and 5 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

9
libraries/AC_AttitudeControl/AC_AttitudeControl.cpp
View File

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

3
libraries/AC_AttitudeControl/AC_AttitudeControl.h
View File

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

2
libraries/AC_AttitudeControl/AC_AttitudeControl_Multi.cpp
View File

@ -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();