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

AC_AttitudeControl: Allow yaw rate reset to be de-selected

This commit is contained in:
Leonard Hall 2021-07-09 21:41:33 +09:30 committed by Andrew Tridgell
parent 2d2f1dd23d
commit 902560953b
2 changed files with 19 additions and 11 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

10
libraries/AC_AttitudeControl/AC_AttitudeControl.cpp
View File

@ -876,18 +876,22 @@ Vector3f AC_AttitudeControl::euler_accel_limit(const Vector3f &euler_rad, const
} }
// Sets attitude target to vehicle attitude and sets all rates to zero // Sets attitude target to vehicle attitude and sets all rates to zero
void AC_AttitudeControl::reset_target_and_rate() // If reset_rate is false rates are not reset to allow the rate controllers to run
void AC_AttitudeControl::reset_target_and_rate(bool reset_rate)
{ {
// move attitude target to current attitude // move attitude target to current attitude
_ahrs.get_quat_body_to_ned(_attitude_target); _ahrs.get_quat_body_to_ned(_attitude_target);
if (reset_rate) {
// Convert euler angle derivative of desired attitude into a body-frame angular velocity vector for feedforward // Convert euler angle derivative of desired attitude into a body-frame angular velocity vector for feedforward
_ang_vel_target.zero(); _ang_vel_target.zero();
_euler_angle_target.zero(); _euler_angle_target.zero();
} }
}
// Sets yaw target to vehicle heading and sets yaw rate to zero // Sets yaw target to vehicle heading and sets yaw rate to zero
void AC_AttitudeControl::reset_yaw_target_and_rate() // If reset_rate is false rates are not reset to allow the rate controllers to run
void AC_AttitudeControl::reset_yaw_target_and_rate(bool reset_rate)
{ {
// move attitude target to current heading // move attitude target to current heading
float yaw_shift = _ahrs.yaw - _euler_angle_target.z; float yaw_shift = _ahrs.yaw - _euler_angle_target.z;
@ -895,12 +899,14 @@ void AC_AttitudeControl::reset_yaw_target_and_rate()
_attitude_target_update.from_axis_angle(Vector3f{0.0f, 0.0f, yaw_shift}); _attitude_target_update.from_axis_angle(Vector3f{0.0f, 0.0f, yaw_shift});
_attitude_target = _attitude_target_update * _attitude_target; _attitude_target = _attitude_target_update * _attitude_target;
if (reset_rate) {
// set yaw rate to zero // set yaw rate to zero
_euler_rate_target.z = 0.0f; _euler_rate_target.z = 0.0f;
// Convert euler angle derivative of desired attitude into a body-frame angular velocity vector for feedforward // 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); 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 // Shifts the target attitude to maintain the current error in the event of an EKF reset
void AC_AttitudeControl::inertial_frame_reset() void AC_AttitudeControl::inertial_frame_reset()

6
libraries/AC_AttitudeControl/AC_AttitudeControl.h
View File

@ -133,10 +133,12 @@ public:
void reset_rate_controller_I_terms_smoothly(); void reset_rate_controller_I_terms_smoothly();
// Sets attitude target to vehicle attitude and sets all rates to zero // Sets attitude target to vehicle attitude and sets all rates to zero
void reset_target_and_rate(); // If reset_rate is false rates are not reset to allow the rate controllers to run
void reset_target_and_rate(bool reset_rate = true);
// Sets yaw target to vehicle heading and sets yaw rate to zero // Sets yaw target to vehicle heading and sets yaw rate to zero
void reset_yaw_target_and_rate(); // If reset_rate is false rates are not reset to allow the rate controllers to run
void reset_yaw_target_and_rate(bool reset_rate = true);
// handle reset of attitude from EKF since the last iteration // handle reset of attitude from EKF since the last iteration
void inertial_frame_reset(); void inertial_frame_reset();