From 71ad3587dd6fa64b763210e8fdb0d240321e42aa Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Tue, 11 Oct 2022 09:14:35 +1100 Subject: [PATCH] AC_AttitudeControl: added single loop override of angle P gains this is used by quadplanes in back-transiton to prevent oscillation caused by driving the fixed wing controller too fast --- .../AC_AttitudeControl/AC_AttitudeControl.cpp | 26 +++++++++++++------ .../AC_AttitudeControl/AC_AttitudeControl.h | 19 ++++++++++++++ 2 files changed, 37 insertions(+), 8 deletions(-) diff --git a/libraries/AC_AttitudeControl/AC_AttitudeControl.cpp b/libraries/AC_AttitudeControl/AC_AttitudeControl.cpp index 9143e85cbe..5b013531ce 100644 --- a/libraries/AC_AttitudeControl/AC_AttitudeControl.cpp +++ b/libraries/AC_AttitudeControl/AC_AttitudeControl.cpp @@ -849,8 +849,10 @@ void AC_AttitudeControl::input_shaping_rate_predictor(const Vector2f &error_angl target_ang_vel.x = input_shaping_angle(wrap_PI(error_angle.x), _input_tc, get_accel_roll_max_radss(), target_ang_vel.x, dt); target_ang_vel.y = input_shaping_angle(wrap_PI(error_angle.y), _input_tc, 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)); + const float angleP_roll = _p_angle_roll.kP() * _angle_P_scale.x; + const float angleP_pitch = _p_angle_pitch.kP() * _angle_P_scale.y; + target_ang_vel.x = angleP_roll * wrap_PI(error_angle.x); + target_ang_vel.y = angleP_pitch * wrap_PI(error_angle.y); } // Limit the angular velocity correction Vector3f ang_vel(target_ang_vel.x, target_ang_vel.y, 0.0f); @@ -988,25 +990,33 @@ Vector3f AC_AttitudeControl::update_ang_vel_target_from_att_error(const Vector3f { Vector3f rate_target_ang_vel; // Compute the roll angular velocity demand from the roll angle error + const float angleP_roll = _p_angle_roll.kP() * _angle_P_scale.x; if (_use_sqrt_controller && !is_zero(get_accel_roll_max_radss())) { - rate_target_ang_vel.x = sqrt_controller(attitude_error_rot_vec_rad.x, _p_angle_roll.kP(), constrain_float(get_accel_roll_max_radss() / 2.0f, AC_ATTITUDE_ACCEL_RP_CONTROLLER_MIN_RADSS, AC_ATTITUDE_ACCEL_RP_CONTROLLER_MAX_RADSS), _dt); + rate_target_ang_vel.x = sqrt_controller(attitude_error_rot_vec_rad.x, angleP_roll, constrain_float(get_accel_roll_max_radss() / 2.0f, AC_ATTITUDE_ACCEL_RP_CONTROLLER_MIN_RADSS, AC_ATTITUDE_ACCEL_RP_CONTROLLER_MAX_RADSS), _dt); } else { - rate_target_ang_vel.x = _p_angle_roll.kP() * attitude_error_rot_vec_rad.x; + rate_target_ang_vel.x = angleP_roll * attitude_error_rot_vec_rad.x; } // Compute the pitch angular velocity demand from the pitch angle error + const float angleP_pitch = _p_angle_pitch.kP() * _angle_P_scale.y; if (_use_sqrt_controller && !is_zero(get_accel_pitch_max_radss())) { - rate_target_ang_vel.y = sqrt_controller(attitude_error_rot_vec_rad.y, _p_angle_pitch.kP(), constrain_float(get_accel_pitch_max_radss() / 2.0f, AC_ATTITUDE_ACCEL_RP_CONTROLLER_MIN_RADSS, AC_ATTITUDE_ACCEL_RP_CONTROLLER_MAX_RADSS), _dt); + rate_target_ang_vel.y = sqrt_controller(attitude_error_rot_vec_rad.y, angleP_pitch, constrain_float(get_accel_pitch_max_radss() / 2.0f, AC_ATTITUDE_ACCEL_RP_CONTROLLER_MIN_RADSS, AC_ATTITUDE_ACCEL_RP_CONTROLLER_MAX_RADSS), _dt); } else { - rate_target_ang_vel.y = _p_angle_pitch.kP() * attitude_error_rot_vec_rad.y; + rate_target_ang_vel.y = angleP_pitch * attitude_error_rot_vec_rad.y; } // Compute the yaw angular velocity demand from the yaw angle error + const float angleP_yaw = _p_angle_yaw.kP() * _angle_P_scale.z; if (_use_sqrt_controller && !is_zero(get_accel_yaw_max_radss())) { - rate_target_ang_vel.z = sqrt_controller(attitude_error_rot_vec_rad.z, _p_angle_yaw.kP(), constrain_float(get_accel_yaw_max_radss() / 2.0f, AC_ATTITUDE_ACCEL_Y_CONTROLLER_MIN_RADSS, AC_ATTITUDE_ACCEL_Y_CONTROLLER_MAX_RADSS), _dt); + rate_target_ang_vel.z = sqrt_controller(attitude_error_rot_vec_rad.z, angleP_yaw, constrain_float(get_accel_yaw_max_radss() / 2.0f, AC_ATTITUDE_ACCEL_Y_CONTROLLER_MIN_RADSS, AC_ATTITUDE_ACCEL_Y_CONTROLLER_MAX_RADSS), _dt); } else { - rate_target_ang_vel.z = _p_angle_yaw.kP() * attitude_error_rot_vec_rad.z; + rate_target_ang_vel.z = angleP_yaw * attitude_error_rot_vec_rad.z; } + + // reset angle P scaling, saving used value for logging + _angle_P_scale_used = _angle_P_scale; + _angle_P_scale = Vector3f{1,1,1}; + return rate_target_ang_vel; } diff --git a/libraries/AC_AttitudeControl/AC_AttitudeControl.h b/libraries/AC_AttitudeControl/AC_AttitudeControl.h index a30974d545..ef4b1615a2 100644 --- a/libraries/AC_AttitudeControl/AC_AttitudeControl.h +++ b/libraries/AC_AttitudeControl/AC_AttitudeControl.h @@ -376,6 +376,19 @@ public: // Sets the yaw rate shaping time constant void set_yaw_rate_tc(float input_tc) { _rate_y_tc = input_tc; } + // setup a one loop angle P scale multiplier. This replaces any previous scale applied + // so should only be used when only one source of scaling is needed + void set_angle_P_scale(const Vector3f &angle_P_scale) { _angle_P_scale = angle_P_scale; } + + // setup a one loop angle P scale multiplier, multiplying by any + // previously applied scale from this loop. This allows for more + // than one type of scale factor to be applied for different + // purposes + void set_angle_P_scale_mult(const Vector3f &angle_P_scale) { _angle_P_scale *= angle_P_scale; } + + // get the value of the angle P scale that was used in the last loop, for logging + const Vector3f &get_angle_P_scale_logging(void) const { return _angle_P_scale_used; } + // User settable parameters static const struct AP_Param::GroupInfo var_info[]; @@ -495,6 +508,12 @@ protected: float _rate_rp_tc; float _rate_y_tc; + // angle P scaling vector for roll, pitch, yaw + Vector3f _angle_P_scale{1,1,1}; + + // angle scale used for last loop, used for logging + Vector3f _angle_P_scale_used; + // References to external libraries const AP_AHRS_View& _ahrs; const AP_Vehicle::MultiCopter &_aparm;