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

AC_AttitudeControl: Set rates to zero during arming procedure for acro

This commit is contained in:
Leonard Hall 2021-05-25 09:08:16 +09:30 committed by Randy Mackay
parent dd0b303ec2
commit fb83f98b77
2 changed files with 13 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

11
libraries/AC_AttitudeControl/AC_AttitudeControl.cpp
View File

@ -875,6 +875,17 @@ Vector3f AC_AttitudeControl::euler_accel_limit(const Vector3f &euler_rad, const
return rot_accel;
}
// Sets attitude target to vehicle attitude and sets all rates to zero
void AC_AttitudeControl::set_attitude_target_to_current_attitude()
{
// move attitude target to current attitude
_ahrs.get_quat_body_to_ned(_attitude_target);
// Convert euler angle derivative of desired attitude into a body-frame angular velocity vector for feedforward
_ang_vel_target.zero();
_euler_angle_target.zero();
}
// Sets yaw target to vehicle heading and sets yaw rate to zero
void AC_AttitudeControl::reset_yaw_target_and_rate()
{

4
libraries/AC_AttitudeControl/AC_AttitudeControl.h
View File

@ -129,8 +129,8 @@ public:
// reset rate controller I terms smoothly to zero in 0.5 seconds
void reset_rate_controller_I_terms_smoothly();
// Sets attitude target to vehicle attitude
void set_attitude_target_to_current_attitude() { _ahrs.get_quat_body_to_ned(_attitude_target); }
// Sets attitude target to vehicle attitude and sets all rates to zero
void set_attitude_target_to_current_attitude();
// Sets yaw target to vehicle heading and sets yaw rate to zero
void reset_yaw_target_and_rate();