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

AC_AttitudeControl: Set yaw rate to zero during arming procedure

Browse Source
This commit is contained in:
Leonard Hall 2021-05-24 23:01:40 +09:30 committed by Randy Mackay
parent 8e61de37be
commit b489dd5fe1
2 changed files with 12 additions and 8 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

13
libraries/AC_AttitudeControl/AC_AttitudeControl.cpp
View File

@ -875,13 +875,20 @@ Vector3f AC_AttitudeControl::euler_accel_limit(const Vector3f &euler_rad, const
return rot_accel;
}
// Shifts earth frame yaw target by yaw_shift_cd. yaw_shift_cd should be in centidegrees and is added to the current target heading
void AC_AttitudeControl::shift_ef_yaw_target(float yaw_shift_cd)
// Sets yaw target to vehicle heading and sets yaw rate to zero
void AC_AttitudeControl::set_yaw_target_to_current_heading()
{
float yaw_shift = radians(yaw_shift_cd * 0.01f);
// move attitude target to current heading
float yaw_shift = _ahrs.yaw - _euler_angle_target.z;
Quaternion _attitude_target_update;
_attitude_target_update.from_axis_angle(Vector3f{0.0f, 0.0f, yaw_shift});
_attitude_target = _attitude_target_update * _attitude_target;
// set yaw rate to zero
_euler_rate_target.z = 0.0f;
// Convert euler angle derivative of desired attitude into a body-frame angular velocity vector for feedforward
euler_rate_to_ang_vel(_euler_angle_target, _euler_rate_target, _ang_vel_target);
}
// Shifts the target attitude to maintain the current error in the event of an EKF reset

7
libraries/AC_AttitudeControl/AC_AttitudeControl.h
View File

@ -132,11 +132,8 @@ public:
// Sets attitude target to vehicle attitude
void set_attitude_target_to_current_attitude() { _ahrs.get_quat_body_to_ned(_attitude_target); }
// Sets yaw target to vehicle heading
void set_yaw_target_to_current_heading() { shift_ef_yaw_target(degrees(_ahrs.yaw - _euler_angle_target.z) * 100.0f); }
// Shifts earth frame yaw target by yaw_shift_cd. yaw_shift_cd should be in centidegrees and is added to the current target heading
void shift_ef_yaw_target(float yaw_shift_cd);
// Sets yaw target to vehicle heading and sets yaw rate to zero
void set_yaw_target_to_current_heading();
// handle reset of attitude from EKF since the last iteration
void inertial_frame_reset();