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

AC_AttitudeControl: add input_shaping_rate_predictor

Browse Source
This commit is contained in:
Leonard Hall 2017-11-11 01:52:57 +10:30 committed by Randy Mackay
parent 6175a896ee
commit ee82943f85
2 changed files with 24 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

20
libraries/AC_AttitudeControl/AC_AttitudeControl.cpp
View File

@ -481,6 +481,12 @@ void AC_AttitudeControl::thrust_heading_rotation_angles(Quaternion& att_to_quat,
heading_quat.to_axis_angle(rotation);
att_diff_angle.z = rotation.z;
// Todo: Limit roll an pitch error based on output saturation and maximum error.
// Limit Yaw Error based on maximum acceleration - Update to include output saturation and maximum error.
// Currently the limit is based on the maximum acceleration using the linear part of the SQRT controller.
// This should be updated to be based on an angle limit, saturation, or unlimited based on user defined parameters.
if(!is_zero(_p_angle_yaw.kP()) && fabsf(att_diff_angle.z) > AC_ATTITUDE_ACCEL_Y_CONTROLLER_MAX_RADSS/_p_angle_yaw.kP()){
att_diff_angle.z = constrain_float(wrap_PI(att_diff_angle.z), -AC_ATTITUDE_ACCEL_Y_CONTROLLER_MAX_RADSS/_p_angle_yaw.kP(), AC_ATTITUDE_ACCEL_Y_CONTROLLER_MAX_RADSS/_p_angle_yaw.kP());
heading_quat.from_axis_angle(Vector3f(0.0f,0.0f,att_diff_angle.z));
@ -504,6 +510,20 @@ float AC_AttitudeControl::input_shaping_angle(float error_angle, float smoothing
}
}
// 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(Vector2f error_angle, Vector2f& target_ang_vel, float dt) const
{
if (_rate_bf_ff_enabled) {
// translate the roll pitch and yaw acceleration limits to the euler axis
target_ang_vel.x = input_shaping_angle(wrap_PI(error_angle.x), _smoothing_gain, get_accel_roll_max_radss(), target_ang_vel.x, dt);
target_ang_vel.y = input_shaping_angle(wrap_PI(error_angle.y), _smoothing_gain, get_accel_pitch_max_radss(), target_ang_vel.y, dt);
} else {
target_ang_vel.x = _p_angle_roll.get_p(wrap_PI(error_angle.x));
target_ang_vel.y = _p_angle_pitch.get_p(wrap_PI(error_angle.y));
}
}
// 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)
{

4
libraries/AC_AttitudeControl/AC_AttitudeControl.h
View File

@ -235,6 +235,10 @@ public:
// deceleration limits including basic jerk limiting using smoothing_gain
static float input_shaping_angle(float error_angle, float smoothing_gain, 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.
// This function can be used to predict the delay associated with angle requests.
void input_shaping_rate_predictor(Vector2f error_angle, Vector2f& target_ang_vel, float dt) const;
// limits the acceleration and deceleration of a velocity request
float input_shaping_ang_vel(float target_ang_vel, float desired_ang_vel, float accel_max);