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

AC_AttitudeControl: move input_shaping_rate into input_shaping_ang_vel

This commit is contained in:
Bill Geyer 2022-06-23 23:11:32 -04:00 committed by Randy Mackay
parent be65358cc1
commit 2377d7a2c3
2 changed files with 25 additions and 81 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

99
libraries/AC_AttitudeControl/AC_AttitudeControl.cpp
View File

@ -287,11 +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
// the output rate towards the input rate.
if (!is_positive(_rate_y_tc)) {
_euler_rate_target.z = input_shaping_ang_vel(_euler_rate_target.z, euler_yaw_rate, euler_accel.z, _dt);
} else {
_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);
}
_euler_rate_target.z = input_shaping_ang_vel(_euler_rate_target.z, euler_yaw_rate, euler_accel.z, _dt, _rate_y_tc);
// 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);
@ -392,18 +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
// the output rate towards the input rate.
if (!is_positive(_rate_rp_tc)) {
_euler_rate_target.x = input_shaping_ang_vel(_euler_rate_target.x, euler_roll_rate, euler_accel.x, _dt);
_euler_rate_target.y = input_shaping_ang_vel(_euler_rate_target.y, euler_pitch_rate, euler_accel.y, _dt);
} else {
_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_rate((euler_pitch_rate - _euler_rate_target.y), _rate_rp_tc, euler_accel.y, _euler_rate_target.y, _dt);
}
if (!is_positive(_rate_y_tc)) {
_euler_rate_target.z = input_shaping_ang_vel(_euler_rate_target.z, euler_yaw_rate, euler_accel.z, _dt);
} else {
_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);
}
_euler_rate_target.x = input_shaping_ang_vel(_euler_rate_target.x, euler_roll_rate, euler_accel.x, _dt, _rate_rp_tc);
_euler_rate_target.y = input_shaping_ang_vel(_euler_rate_target.y, euler_pitch_rate, euler_accel.y, _dt, _rate_rp_tc);
_euler_rate_target.z = input_shaping_ang_vel(_euler_rate_target.z, euler_yaw_rate, euler_accel.z, _dt, _rate_y_tc);
// 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);
@ -441,18 +428,9 @@ void AC_AttitudeControl::input_rate_bf_roll_pitch_yaw(float roll_rate_bf_cds, fl
// Compute acceleration-limited body frame rates
// When acceleration limiting is enabled, the input shaper constrains angular acceleration about the axis, slewing
// the output rate towards the input rate.
if (!is_positive(_rate_rp_tc)) {
_ang_vel_target.x = input_shaping_ang_vel(_ang_vel_target.x, roll_rate_rads, get_accel_roll_max_radss(), _dt);
_ang_vel_target.y = input_shaping_ang_vel(_ang_vel_target.y, pitch_rate_rads, get_accel_pitch_max_radss(), _dt);
} else {
_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_rate((pitch_rate_rads - _ang_vel_target.y), _rate_rp_tc, get_accel_pitch_max_radss(), _ang_vel_target.y, _dt);
}
if (!is_positive(_rate_y_tc)) {
_ang_vel_target.z = input_shaping_ang_vel(_ang_vel_target.z, yaw_rate_rads, get_accel_yaw_max_radss(), _dt);
} else {
_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);
}
_ang_vel_target.x = input_shaping_ang_vel(_ang_vel_target.x, roll_rate_rads, get_accel_roll_max_radss(), _dt, _rate_rp_tc);
_ang_vel_target.y = input_shaping_ang_vel(_ang_vel_target.y, pitch_rate_rads, get_accel_pitch_max_radss(), _dt, _rate_rp_tc);
_ang_vel_target.z = input_shaping_ang_vel(_ang_vel_target.z, yaw_rate_rads, get_accel_yaw_max_radss(), _dt, _rate_y_tc);
// 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);
@ -483,18 +461,9 @@ void AC_AttitudeControl::input_rate_bf_roll_pitch_yaw_2(float roll_rate_bf_cds,
// Compute acceleration-limited body frame rates
// When acceleration limiting is enabled, the input shaper constrains angular acceleration about the axis, slewing
// the output rate towards the input rate.
if (!is_positive(_rate_rp_tc)) {
_ang_vel_target.x = input_shaping_ang_vel(_ang_vel_target.x, roll_rate_rads, get_accel_roll_max_radss(), _dt);
_ang_vel_target.y = input_shaping_ang_vel(_ang_vel_target.y, pitch_rate_rads, get_accel_pitch_max_radss(), _dt);
} else {
_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_rate((pitch_rate_rads - _ang_vel_target.y), _rate_rp_tc, get_accel_pitch_max_radss(), _ang_vel_target.y, _dt);
}
if (!is_positive(_rate_y_tc)) {
_ang_vel_target.z = input_shaping_ang_vel(_ang_vel_target.z, yaw_rate_rads, get_accel_yaw_max_radss(), _dt);
} else {
_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);
}
_ang_vel_target.x = input_shaping_ang_vel(_ang_vel_target.x, roll_rate_rads, get_accel_roll_max_radss(), _dt, _rate_rp_tc);
_ang_vel_target.y = input_shaping_ang_vel(_ang_vel_target.y, pitch_rate_rads, get_accel_pitch_max_radss(), _dt, _rate_rp_tc);
_ang_vel_target.z = input_shaping_ang_vel(_ang_vel_target.z, yaw_rate_rads, get_accel_yaw_max_radss(), _dt, _rate_y_tc);
// Update the unused targets attitude based on current attitude to condition mode change
_ahrs.get_quat_body_to_ned(_attitude_target);
@ -531,18 +500,9 @@ void AC_AttitudeControl::input_rate_bf_roll_pitch_yaw_3(float roll_rate_bf_cds,
// Compute acceleration-limited body frame rates
// When acceleration limiting is enabled, the input shaper constrains angular acceleration about the axis, slewing
// the output rate towards the input rate.
if (!is_positive(_rate_rp_tc)) {
_ang_vel_target.x = input_shaping_ang_vel(_ang_vel_target.x, roll_rate_rads, get_accel_roll_max_radss(), _dt);
_ang_vel_target.y = input_shaping_ang_vel(_ang_vel_target.y, pitch_rate_rads, get_accel_pitch_max_radss(), _dt);
} else {
_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_rate((pitch_rate_rads - _ang_vel_target.y), _rate_rp_tc, get_accel_pitch_max_radss(), _ang_vel_target.y, _dt);
}
if (!is_positive(_rate_y_tc)) {
_ang_vel_target.z = input_shaping_ang_vel(_ang_vel_target.z, yaw_rate_rads, get_accel_yaw_max_radss(), _dt);
} else {
_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);
}
_ang_vel_target.x = input_shaping_ang_vel(_ang_vel_target.x, roll_rate_rads, get_accel_roll_max_radss(), _dt, _rate_rp_tc);
_ang_vel_target.y = input_shaping_ang_vel(_ang_vel_target.y, pitch_rate_rads, get_accel_pitch_max_radss(), _dt, _rate_rp_tc);
_ang_vel_target.z = input_shaping_ang_vel(_ang_vel_target.z, yaw_rate_rads, get_accel_yaw_max_radss(), _dt, _rate_y_tc);
// Retrieve quaternion body attitude
Quaternion attitude_body;
@ -624,11 +584,7 @@ void AC_AttitudeControl::input_thrust_vector_rate_heading(const Vector3f& thrust
// 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.
if (!is_positive(_rate_y_tc)) {
_ang_vel_target.z = input_shaping_ang_vel(_ang_vel_target.z, heading_rate, get_accel_yaw_max_radss(), _dt);
} else {
_ang_vel_target.z = input_shaping_rate((heading_rate - _ang_vel_target.z), _rate_y_tc, get_accel_yaw_max_radss(), _ang_vel_target.z, _dt);
}
_ang_vel_target.z = input_shaping_ang_vel(_ang_vel_target.z, heading_rate, get_accel_yaw_max_radss(), _dt, _rate_y_tc);
// Limit the angular velocity
ang_vel_limit(_ang_vel_target, radians(_ang_vel_roll_max), radians(_ang_vel_pitch_max), radians(_ang_vel_yaw_max));
@ -863,12 +819,18 @@ float AC_AttitudeControl::input_shaping_angle(float error_angle, float input_tc,
}
// Acceleration is limited directly to smooth the beginning of the curve.
return input_shaping_ang_vel(target_ang_vel, desired_ang_vel, accel_max, dt);
return input_shaping_ang_vel(target_ang_vel, desired_ang_vel, accel_max, dt, 0.0f);
}
// limits the acceleration and deceleration of a velocity request
float AC_AttitudeControl::input_shaping_ang_vel(float target_ang_vel, float desired_ang_vel, float accel_max, float dt)
// Shapes the velocity request based on a rate time constant. The angular acceleration and deceleration is limited.
float AC_AttitudeControl::input_shaping_ang_vel(float target_ang_vel, float desired_ang_vel, float accel_max, float dt, float input_tc)
{
if (is_positive(input_tc)) {
// Calculate the acceleration to smoothly achieve rate. Jerk is not limited.
float error_rate = desired_ang_vel - target_ang_vel;
float desired_ang_accel = sqrt_controller(error_rate, 1.0f / MAX(input_tc, 0.01f), 0.0f, dt);
desired_ang_vel = target_ang_vel + desired_ang_accel * dt;
}
// Acceleration is limited directly to smooth the beginning of the curve.
if (is_positive(accel_max)) {
float delta_ang_vel = accel_max * dt;
@ -878,21 +840,6 @@ float AC_AttitudeControl::input_shaping_ang_vel(float target_ang_vel, float desi
}
}
// calculates the accleration correction from an rate error. The angular acceleration and deceleration is limited.
float AC_AttitudeControl::input_shaping_rate(float error_rate, float input_tc, float accel_max, float target_ang_vel, float dt)
{
// Calculate the acceleration to smoothly achieve rate. Jerk is not limited.
float desired_ang_accel = sqrt_controller(error_rate, 1.0f / MAX(input_tc, 0.01f), 0.0f, dt);
// limit acceleration or deceleration
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.
// 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

7
libraries/AC_AttitudeControl/AC_AttitudeControl.h
View File

@ -306,11 +306,8 @@ public:
static float input_shaping_angle(float error_angle, float input_tc, float accel_max, float target_ang_vel, float desired_ang_vel, float max_ang_vel, float dt);
static float input_shaping_angle(float error_angle, float input_tc, float accel_max, float target_ang_vel, float dt){ return input_shaping_angle(error_angle, input_tc, accel_max, target_ang_vel, 0.0f, 0.0f, dt); }
// 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);
// calculates the accleration correction from an rate error. The angular acceleration and deceleration is limited.
static float input_shaping_rate(float error_rate, float input_tc, float accel_max, float target_ang_vel, float dt);
// Shapes the velocity request based on a rate time constant. The angular acceleration and deceleration is limited.
static float input_shaping_ang_vel(float target_ang_vel, float desired_ang_vel, float accel_max, float dt, float input_tc);
// 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.