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

AC_AttitudeControl: incorporate sqrt controller in rate shaping

Browse Source
This commit is contained in:
bnsgeyer 2022-03-14 13:04:37 -04:00 committed by Randy Mackay
parent 554f20ddb1
commit 7594f7a558
2 changed files with 38 additions and 13 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

39
libraries/AC_AttitudeControl/AC_AttitudeControl.cpp
View File

@ -287,7 +287,7 @@ void AC_AttitudeControl::input_euler_angle_roll_pitch_euler_rate_yaw(float euler
// When yaw acceleration limiting is enabled, the yaw input shaper constrains angular acceleration about the yaw axis, slewing // When yaw acceleration limiting is enabled, the yaw input shaper constrains angular acceleration about the yaw axis, slewing
// the output rate towards the input rate. // the output rate towards the input rate.
_euler_rate_target.z = input_shaping_ang_vel(_euler_rate_target.z, euler_yaw_rate, euler_accel.z, _dt); _euler_rate_target.z = input_shaping_rate((euler_yaw_rate - _euler_rate_target.z), _rate_y_tc, euler_accel.z, _euler_rate_target.z, _dt);
// 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);
@ -388,9 +388,9 @@ void AC_AttitudeControl::input_euler_rate_roll_pitch_yaw(float euler_roll_rate_c
// When acceleration limiting is enabled, the input shaper constrains angular acceleration, slewing // When acceleration limiting is enabled, the input shaper constrains angular acceleration, slewing
// the output rate towards the input rate. // the output rate towards the input rate.
_euler_rate_target.x = input_shaping_ang_vel(_euler_rate_target.x, euler_roll_rate, euler_accel.x, _dt); _euler_rate_target.x = input_shaping_rate((euler_roll_rate - _euler_rate_target.x), _rate_rp_tc, euler_accel.x, _euler_rate_target.x, _dt);
_euler_rate_target.y = input_shaping_ang_vel(_euler_rate_target.y, euler_pitch_rate, euler_accel.y, _dt); _euler_rate_target.y = input_shaping_rate((euler_pitch_rate - _euler_rate_target.y), _rate_rp_tc, euler_accel.y, _euler_rate_target.y, _dt);
_euler_rate_target.z = input_shaping_ang_vel(_euler_rate_target.z, euler_yaw_rate, euler_accel.z, _dt); _euler_rate_target.z = input_shaping_rate((euler_yaw_rate - _euler_rate_target.z), _rate_y_tc, euler_accel.z, _euler_rate_target.z, _dt);
// 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);
@ -428,9 +428,9 @@ void AC_AttitudeControl::input_rate_bf_roll_pitch_yaw(float roll_rate_bf_cds, fl
// Compute acceleration-limited body frame rates // Compute acceleration-limited body frame rates
// When acceleration limiting is enabled, the input shaper constrains angular acceleration about the axis, slewing // When acceleration limiting is enabled, the input shaper constrains angular acceleration about the axis, slewing
// the output rate towards the input rate. // the output rate towards the input rate.
_ang_vel_target.x = input_shaping_ang_vel(_ang_vel_target.x, roll_rate_rads, get_accel_roll_max_radss(), _dt); _ang_vel_target.x = input_shaping_rate((roll_rate_rads - _ang_vel_target.x), _rate_rp_tc, get_accel_roll_max_radss(), _ang_vel_target.x, _dt);
_ang_vel_target.y = input_shaping_ang_vel(_ang_vel_target.y, pitch_rate_rads, get_accel_pitch_max_radss(), _dt); _ang_vel_target.y = input_shaping_rate((pitch_rate_rads - _ang_vel_target.y), _rate_rp_tc, get_accel_pitch_max_radss(), _ang_vel_target.y, _dt);
_ang_vel_target.z = input_shaping_ang_vel(_ang_vel_target.z, yaw_rate_rads, get_accel_yaw_max_radss(), _dt); _ang_vel_target.z = input_shaping_rate((yaw_rate_rads - _ang_vel_target.z), _rate_y_tc, get_accel_yaw_max_radss(), _ang_vel_target.z, _dt);
// Convert body-frame angular velocity into euler angle derivative of desired attitude // Convert body-frame angular velocity into euler angle derivative of desired attitude
ang_vel_to_euler_rate(_euler_angle_target, _ang_vel_target, _euler_rate_target); ang_vel_to_euler_rate(_euler_angle_target, _ang_vel_target, _euler_rate_target);
@ -461,9 +461,9 @@ void AC_AttitudeControl::input_rate_bf_roll_pitch_yaw_2(float roll_rate_bf_cds,
// Compute acceleration-limited body frame rates // Compute acceleration-limited body frame rates
// When acceleration limiting is enabled, the input shaper constrains angular acceleration about the axis, slewing // When acceleration limiting is enabled, the input shaper constrains angular acceleration about the axis, slewing
// the output rate towards the input rate. // the output rate towards the input rate.
_ang_vel_target.x = input_shaping_ang_vel(_ang_vel_target.x, roll_rate_rads, get_accel_roll_max_radss(), _dt); _ang_vel_target.x = input_shaping_rate((roll_rate_rads - _ang_vel_target.x), _rate_rp_tc, get_accel_roll_max_radss(), _ang_vel_target.x, _dt);
_ang_vel_target.y = input_shaping_ang_vel(_ang_vel_target.y, pitch_rate_rads, get_accel_pitch_max_radss(), _dt); _ang_vel_target.y = input_shaping_rate((pitch_rate_rads - _ang_vel_target.y), _rate_rp_tc, get_accel_pitch_max_radss(), _ang_vel_target.y, _dt);
_ang_vel_target.z = input_shaping_ang_vel(_ang_vel_target.z, yaw_rate_rads, get_accel_yaw_max_radss(), _dt); _ang_vel_target.z = input_shaping_rate((yaw_rate_rads - _ang_vel_target.z), _rate_y_tc, get_accel_yaw_max_radss(), _ang_vel_target.z, _dt);
// Update the unused targets attitude based on current attitude to condition mode change // Update the unused targets attitude based on current attitude to condition mode change
_ahrs.get_quat_body_to_ned(_attitude_target); _ahrs.get_quat_body_to_ned(_attitude_target);
@ -500,9 +500,9 @@ void AC_AttitudeControl::input_rate_bf_roll_pitch_yaw_3(float roll_rate_bf_cds,
// Compute acceleration-limited body frame rates // Compute acceleration-limited body frame rates
// When acceleration limiting is enabled, the input shaper constrains angular acceleration about the axis, slewing // When acceleration limiting is enabled, the input shaper constrains angular acceleration about the axis, slewing
// the output rate towards the input rate. // the output rate towards the input rate.
_ang_vel_target.x = input_shaping_ang_vel(_ang_vel_target.x, roll_rate_rads, get_accel_roll_max_radss(), _dt); _ang_vel_target.x = input_shaping_rate((roll_rate_rads - _ang_vel_target.x), _rate_rp_tc, get_accel_roll_max_radss(), _ang_vel_target.x, _dt);
_ang_vel_target.y = input_shaping_ang_vel(_ang_vel_target.y, pitch_rate_rads, get_accel_pitch_max_radss(), _dt); _ang_vel_target.y = input_shaping_rate((pitch_rate_rads - _ang_vel_target.y), _rate_rp_tc, get_accel_pitch_max_radss(), _ang_vel_target.y, _dt);
_ang_vel_target.z = input_shaping_ang_vel(_ang_vel_target.z, yaw_rate_rads, get_accel_yaw_max_radss(), _dt); _ang_vel_target.z = input_shaping_rate((yaw_rate_rads - _ang_vel_target.z), _rate_y_tc, get_accel_yaw_max_radss(), _ang_vel_target.z, _dt);
// Retrieve quaternion body attitude // Retrieve quaternion body attitude
Quaternion attitude_body; Quaternion attitude_body;
@ -834,6 +834,19 @@ float AC_AttitudeControl::input_shaping_ang_vel(float target_ang_vel, float desi
} }
} }
// calculates the accleration correction from an rate error. The angular acceleration is limited.
float AC_AttitudeControl::input_shaping_rate(float error_rate, float input_tc, float accel_max, float target_ang_vel, float dt)
{
float desired_ang_accel = sqrt_controller(error_rate, 1.0f / MAX(input_tc, 0.01f), 0.0f, dt);
if (is_positive(accel_max)) {
desired_ang_accel = constrain_float(desired_ang_accel, -accel_max, accel_max);
}
target_ang_vel += desired_ang_accel * dt;
return target_ang_vel;
}
// calculates the expected angular velocity correction from an angle error based on the AC_AttitudeControl settings. // calculates the expected angular velocity correction from an angle error based on the AC_AttitudeControl settings.
// This function can be used to predict the delay associated with angle requests. // This function can be used to predict the delay associated with angle requests.
void AC_AttitudeControl::input_shaping_rate_predictor(const Vector2f &error_angle, Vector2f& target_ang_vel, float dt) const void AC_AttitudeControl::input_shaping_rate_predictor(const Vector2f &error_angle, Vector2f& target_ang_vel, float dt) const

12
libraries/AC_AttitudeControl/AC_AttitudeControl.h
View File

@ -309,6 +309,8 @@ public:
// limits the acceleration and deceleration of a velocity request // limits the acceleration and deceleration of a velocity request
static float input_shaping_ang_vel(float target_ang_vel, float desired_ang_vel, float accel_max, float dt); static float input_shaping_ang_vel(float target_ang_vel, float desired_ang_vel, float accel_max, float dt);
static float input_shaping_rate(float error_rate, float input_tc, float accel_max, float target_ang_vel, float dt);
// calculates the expected angular velocity correction from an angle error based on the AC_AttitudeControl settings. // calculates the expected angular velocity correction from an angle error based on the AC_AttitudeControl settings.
// This function can be used to predict the delay associated with angle requests. // This function can be used to predict the delay associated with angle requests.
void input_shaping_rate_predictor(const Vector2f &error_angle, Vector2f& target_ang_vel, float dt) const; void input_shaping_rate_predictor(const Vector2f &error_angle, Vector2f& target_ang_vel, float dt) const;
@ -370,6 +372,12 @@ public:
// get the slew rate value for roll, pitch and yaw, for oscillation detection in lua scripts // get the slew rate value for roll, pitch and yaw, for oscillation detection in lua scripts
void get_rpy_srate(float &roll_srate, float &pitch_srate, float &yaw_srate); void get_rpy_srate(float &roll_srate, float &pitch_srate, float &yaw_srate);
// Sets the roll and pitch rate shaping time constant
void set_roll_pitch_rate_tc(float input_tc) { _rate_rp_tc = input_tc; }
// Sets the yaw rate shaping time constant
void set_yaw_rate_tc(float input_tc) { _rate_y_tc = input_tc; }
// User settable parameters // User settable parameters
static const struct AP_Param::GroupInfo var_info[]; static const struct AP_Param::GroupInfo var_info[];
@ -485,6 +493,10 @@ protected:
// Yaw feed forward percent to allow zero yaw actuator output during extreme roll and pitch corrections // Yaw feed forward percent to allow zero yaw actuator output during extreme roll and pitch corrections
float _feedforward_scalar = 1.0f; float _feedforward_scalar = 1.0f;
// rate controller input smoothing time constant
float _rate_rp_tc;
float _rate_y_tc;
// References to external libraries // References to external libraries
const AP_AHRS_View& _ahrs; const AP_AHRS_View& _ahrs;
const AP_Vehicle::MultiCopter &_aparm; const AP_Vehicle::MultiCopter &_aparm;