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

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
This commit is contained in:
Andrew Tridgell 2022-10-11 09:14:35 +11:00
parent 01d006ebd1
commit bd1be3b41c
2 changed files with 37 additions and 8 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

26
libraries/AC_AttitudeControl/AC_AttitudeControl.cpp
View File

@ -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;
}

19
libraries/AC_AttitudeControl/AC_AttitudeControl.h
View File

@ -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;