Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AC_AttitudeControl: limit gyro correction to 45 degrees error

This commit is contained in:
Leonard Hall 2018-08-18 21:39:24 +09:30 committed by Randy Mackay
parent acaefe9316
commit 96a8bcf641
1 changed files with 2 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
libraries/AC_AttitudeControl/AC_AttitudeControl.cpp
View File

@ -548,8 +548,8 @@ void AC_AttitudeControl::attitude_controller_run_quat()
// Add feedforward term that attempts to ensure that roll and pitch errors rotate with the body frame rather than the reference frame. // Add feedforward term that attempts to ensure that roll and pitch errors rotate with the body frame rather than the reference frame.
// todo: this should probably be a matrix that couples yaw as well. // todo: this should probably be a matrix that couples yaw as well.
_rate_target_ang_vel.x += attitude_error_vector.y * _ahrs.get_gyro().z; _rate_target_ang_vel.x += constrain_float(attitude_error_vector.y, -M_PI/4, M_PI/4) * _ahrs.get_gyro().z;
_rate_target_ang_vel.y += -attitude_error_vector.x * _ahrs.get_gyro().z; _rate_target_ang_vel.y += -constrain_float(attitude_error_vector.x, -M_PI/4, M_PI/4) * _ahrs.get_gyro().z;
ang_vel_limit(_rate_target_ang_vel, radians(_ang_vel_roll_max), radians(_ang_vel_pitch_max), radians(_ang_vel_yaw_max)); ang_vel_limit(_rate_target_ang_vel, radians(_ang_vel_roll_max), radians(_ang_vel_pitch_max), radians(_ang_vel_yaw_max));