mirror of https://github.com/ArduPilot/ardupilot
AC_AttitudeControl: Allow yaw rate reset to be de-selected
This commit is contained in:
parent
2d2f1dd23d
commit
902560953b
|
@ -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
|
||||
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
|
||||
_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();
|
||||
if (reset_rate) {
|
||||
// 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()
|
||||
// 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
|
||||
float yaw_shift = _ahrs.yaw - _euler_angle_target.z;
|
||||
|
@ -895,11 +899,13 @@ void AC_AttitudeControl::reset_yaw_target_and_rate()
|
|||
_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;
|
||||
if (reset_rate) {
|
||||
// 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);
|
||||
// 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
|
||||
|
|
|
@ -133,10 +133,12 @@ public:
|
|||
void reset_rate_controller_I_terms_smoothly();
|
||||
|
||||
// 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
|
||||
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
|
||||
void inertial_frame_reset();
|
||||
|
|
Loading…
Reference in New Issue