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

AC_AttitudeControl: ensure the rate and attitude controllers can't interfere with the target at the same time

Browse Source
This commit is contained in:
Andy Piper 2024-05-15 20:51:14 +01:00 committed by Peter Barker
parent c28e38e9b5
commit ff002cba3f
3 changed files with 31 additions and 21 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

37
libraries/AC_AttitudeControl/AC_AttitudeControl.cpp
View File

@ -188,15 +188,16 @@ float AC_AttitudeControl::get_slew_yaw_max_degs() const
// Ensure attitude controller have zero errors to relax rate controller output
void AC_AttitudeControl::relax_attitude_controllers()
{
// take a copy of the last gyro used by the rate controller before using it
Vector3f gyro = _rate_gyro;
// Initialize the attitude variables to the current attitude
_ahrs.get_quat_body_to_ned(_attitude_target);
_attitude_target.to_euler(_euler_angle_target);
_attitude_ang_error.initialise();
// Initialize the angular rate variables to the current rate
_ang_vel_target = _ahrs.get_gyro();
_ang_vel_target = gyro;
ang_vel_to_euler_rate(_attitude_target, _ang_vel_target, _euler_rate_target);
_ang_vel_body = _ahrs.get_gyro();
// Initialize remaining variables
_thrust_error_angle = 0.0f;
@ -208,6 +209,8 @@ void AC_AttitudeControl::relax_attitude_controllers()
// Reset the I terms
reset_rate_controller_I_terms();
// finally update the attitude target
_ang_vel_body = gyro;
}
void AC_AttitudeControl::reset_rate_controller_I_terms()
@ -531,6 +534,7 @@ void AC_AttitudeControl::input_rate_bf_roll_pitch_yaw_2(float roll_rate_bf_cds,
_attitude_target.to_euler(_euler_angle_target);
// Convert body-frame angular velocity into euler angle derivative of desired attitude
ang_vel_to_euler_rate(_attitude_target, _ang_vel_target, _euler_rate_target);
// finally update the attitude target
_ang_vel_body = _ang_vel_target;
}
@ -554,7 +558,7 @@ void AC_AttitudeControl::input_rate_bf_roll_pitch_yaw_3(float roll_rate_bf_cds,
_attitude_ang_error.from_axis_angle(attitude_error);
}
Vector3f gyro_latest = _ahrs.get_gyro_latest();
Vector3f gyro_latest = _rate_gyro;
attitude_ang_error_update_quat.from_axis_angle((_ang_vel_target - gyro_latest) * _dt);
_attitude_ang_error = attitude_ang_error_update_quat * _attitude_ang_error;
_attitude_ang_error.normalize();
@ -582,8 +586,11 @@ void AC_AttitudeControl::input_rate_bf_roll_pitch_yaw_3(float roll_rate_bf_cds,
// Compute the angular velocity target from the integrated rate error
_attitude_ang_error.to_axis_angle(attitude_error);
_ang_vel_body = update_ang_vel_target_from_att_error(attitude_error);
_ang_vel_body += _ang_vel_target;
Vector3f ang_vel_body = update_ang_vel_target_from_att_error(attitude_error);
ang_vel_body += _ang_vel_target;
// finally update the attitude target
_ang_vel_body = ang_vel_body;
}
// Command an angular step (i.e change) in body frame angle
@ -788,33 +795,35 @@ void AC_AttitudeControl::attitude_controller_run_quat()
thrust_heading_rotation_angles(_attitude_target, attitude_body, attitude_error, _thrust_angle, _thrust_error_angle);
// Compute the angular velocity corrections in the body frame from the attitude error
_ang_vel_body = update_ang_vel_target_from_att_error(attitude_error);
Vector3f ang_vel_body = update_ang_vel_target_from_att_error(attitude_error);
// ensure angular velocity does not go over configured limits
ang_vel_limit(_ang_vel_body, radians(_ang_vel_roll_max), radians(_ang_vel_pitch_max), radians(_ang_vel_yaw_max));
ang_vel_limit(ang_vel_body, radians(_ang_vel_roll_max), radians(_ang_vel_pitch_max), radians(_ang_vel_yaw_max));
// rotation from the target frame to the body frame
Quaternion rotation_target_to_body = attitude_body.inverse() * _attitude_target;
// target angle velocity vector in the body frame
Vector3f ang_vel_body_feedforward = rotation_target_to_body * _ang_vel_target;
Vector3f gyro = _rate_gyro;
// Correct the thrust vector and smoothly add feedforward and yaw input
_feedforward_scalar = 1.0f;
if (_thrust_error_angle > AC_ATTITUDE_THRUST_ERROR_ANGLE * 2.0f) {
_ang_vel_body.z = _ahrs.get_gyro().z;
ang_vel_body.z = gyro.z;
} else if (_thrust_error_angle > AC_ATTITUDE_THRUST_ERROR_ANGLE) {
_feedforward_scalar = (1.0f - (_thrust_error_angle - AC_ATTITUDE_THRUST_ERROR_ANGLE) / AC_ATTITUDE_THRUST_ERROR_ANGLE);
_ang_vel_body.x += ang_vel_body_feedforward.x * _feedforward_scalar;
_ang_vel_body.y += ang_vel_body_feedforward.y * _feedforward_scalar;
_ang_vel_body.z += ang_vel_body_feedforward.z;
_ang_vel_body.z = _ahrs.get_gyro().z * (1.0 - _feedforward_scalar) + _ang_vel_body.z * _feedforward_scalar;
ang_vel_body.x += ang_vel_body_feedforward.x * _feedforward_scalar;
ang_vel_body.y += ang_vel_body_feedforward.y * _feedforward_scalar;
ang_vel_body.z += ang_vel_body_feedforward.z;
ang_vel_body.z = gyro.z * (1.0 - _feedforward_scalar) + ang_vel_body.z * _feedforward_scalar;
} else {
_ang_vel_body += ang_vel_body_feedforward;
ang_vel_body += ang_vel_body_feedforward;
}
// Record error to handle EKF resets
_attitude_ang_error = attitude_body.inverse() * _attitude_target;
// finally update the attitude target
_ang_vel_body = ang_vel_body;
}
// thrust_heading_rotation_angles - calculates two ordered rotations to move the attitude_body quaternion to the attitude_target quaternion.

3
libraries/AC_AttitudeControl/AC_AttitudeControl.h
View File

@ -352,9 +352,6 @@ public:
// Calculates the body frame angular velocities to follow the target attitude
void update_attitude_target();
// Calculates the body frame angular velocities to follow the target attitude
void attitude_controller_run_quat();
// thrust_heading_rotation_angles - calculates two ordered rotations to move the attitude_body quaternion to the attitude_target quaternion.
// The maximum error in the yaw axis is limited based on the angle yaw P value and acceleration.
void thrust_heading_rotation_angles(Quaternion& attitude_target, const Quaternion& attitude_body, Vector3f& attitude_error, float& thrust_angle, float& thrust_error_angle) const;

12
libraries/AC_AttitudeControl/AC_AttitudeControl_Multi.cpp
View File

@ -441,24 +441,28 @@ void AC_AttitudeControl_Multi::update_throttle_rpy_mix()
void AC_AttitudeControl_Multi::rate_controller_run_dt(float dt, const Vector3f& gyro)
{
_rate_gyro = gyro;
// take a copy of the target so that it can't be changed from under us.
Vector3f ang_vel_body = _ang_vel_body;
// boost angle_p/pd each cycle on high throttle slew
update_throttle_gain_boost();
// move throttle vs attitude mixing towards desired (called from here because this is conveniently called on every iteration)
update_throttle_rpy_mix();
_ang_vel_body += _sysid_ang_vel_body;
ang_vel_body += _sysid_ang_vel_body;
_rate_gyro = gyro;
_rate_gyro_time_us = AP_HAL::micros64();
_motors.set_roll(get_rate_roll_pid().update_all(_ang_vel_body.x, gyro.x, dt, _motors.limit.roll, _pd_scale.x) + _actuator_sysid.x);
_motors.set_roll(get_rate_roll_pid().update_all(ang_vel_body.x, gyro.x, dt, _motors.limit.roll, _pd_scale.x) + _actuator_sysid.x);
_motors.set_roll_ff(get_rate_roll_pid().get_ff());
_motors.set_pitch(get_rate_pitch_pid().update_all(_ang_vel_body.y, gyro.y, dt, _motors.limit.pitch, _pd_scale.y) + _actuator_sysid.y);
_motors.set_pitch(get_rate_pitch_pid().update_all(ang_vel_body.y, gyro.y, dt, _motors.limit.pitch, _pd_scale.y) + _actuator_sysid.y);
_motors.set_pitch_ff(get_rate_pitch_pid().get_ff());
_motors.set_yaw(get_rate_yaw_pid().update_all(_ang_vel_body.z, gyro.z, dt, _motors.limit.yaw, _pd_scale.z) + _actuator_sysid.z);
_motors.set_yaw(get_rate_yaw_pid().update_all(ang_vel_body.z, gyro.z, dt, _motors.limit.yaw, _pd_scale.z) + _actuator_sysid.z);
_motors.set_yaw_ff(get_rate_yaw_pid().get_ff()*_feedforward_scalar);
_sysid_ang_vel_body.zero();