From ee82943f854e7a14e037545fbc4ad3617356c7da Mon Sep 17 00:00:00 2001 From: Leonard Hall Date: Sat, 11 Nov 2017 01:52:57 +1030 Subject: [PATCH] AC_AttitudeControl: add input_shaping_rate_predictor --- .../AC_AttitudeControl/AC_AttitudeControl.cpp | 20 +++++++++++++++++++ .../AC_AttitudeControl/AC_AttitudeControl.h | 4 ++++ 2 files changed, 24 insertions(+) diff --git a/libraries/AC_AttitudeControl/AC_AttitudeControl.cpp b/libraries/AC_AttitudeControl/AC_AttitudeControl.cpp index 8f2bc864ac..1dc714ef14 100644 --- a/libraries/AC_AttitudeControl/AC_AttitudeControl.cpp +++ b/libraries/AC_AttitudeControl/AC_AttitudeControl.cpp @@ -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) { diff --git a/libraries/AC_AttitudeControl/AC_AttitudeControl.h b/libraries/AC_AttitudeControl/AC_AttitudeControl.h index 58de2292ea..69d5e64418 100644 --- a/libraries/AC_AttitudeControl/AC_AttitudeControl.h +++ b/libraries/AC_AttitudeControl/AC_AttitudeControl.h @@ -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);