From 17c9db08f37c48cb89d17151cedcb4064494d71a Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Mon, 15 Feb 2016 15:26:24 +0900 Subject: [PATCH] AC_AttControl: add angle and rate PIDs --- .../AC_AttitudeControl/AC_AttitudeControl.cpp | 77 +++++++++++-------- .../AC_AttitudeControl/AC_AttitudeControl.h | 75 +++++++++++++----- 2 files changed, 102 insertions(+), 50 deletions(-) diff --git a/libraries/AC_AttitudeControl/AC_AttitudeControl.cpp b/libraries/AC_AttitudeControl/AC_AttitudeControl.cpp index 2bda03d43e..625254e113 100644 --- a/libraries/AC_AttitudeControl/AC_AttitudeControl.cpp +++ b/libraries/AC_AttitudeControl/AC_AttitudeControl.cpp @@ -66,26 +66,39 @@ const AP_Param::GroupInfo AC_AttitudeControl::var_info[] = { // @User: Advanced AP_GROUPINFO("ANGLE_BOOST", 12, AC_AttitudeControl, _angle_boost_enabled, 1), + // @Param: ANG_RLL_P + // @DisplayName: Roll axis angle controller P gain + // @Description: Roll axis angle controller P gain. Converts the error between the desired roll angle and actual angle to a desired roll rate + // @Range: 3.000 12.000 + // @User: Standard + AP_SUBGROUPINFO(_p_angle_roll, "ANG_RLL_", 13, AC_AttitudeControl, AC_P), + + // @Param: ANG_PIT_P + // @DisplayName: Pitch axis angle controller P gain + // @Description: Pitch axis angle controller P gain. Converts the error between the desired pitch angle and actual angle to a desired pitch rate + // @Range: 3.000 12.000 + // @User: Standard + AP_SUBGROUPINFO(_p_angle_pitch, "ANG_PIT_", 14, AC_AttitudeControl, AC_P), + + // @Param: ANG_YAW_P + // @DisplayName: Yaw axis angle controller P gain + // @Description: Yaw axis angle controller P gain. Converts the error between the desired yaw angle and actual angle to a desired yaw rate + // @Range: 3.000 6.000 + // @User: Standard + AP_SUBGROUPINFO(_p_angle_yaw, "ANG_YAW_", 15, AC_AttitudeControl, AC_P), + AP_GROUPEND }; -void AC_AttitudeControl::set_dt(float delta_sec) -{ - _dt = delta_sec; - _pid_rate_roll.set_dt(_dt); - _pid_rate_pitch.set_dt(_dt); - _pid_rate_yaw.set_dt(_dt); -} - void AC_AttitudeControl::relax_bf_rate_controller() { // Set reference angular velocity used in angular velocity controller equal // to the input angular velocity and reset the angular velocity integrators. // This zeros the output of the angular velocity controller. _ang_vel_target_rads = _ahrs.get_gyro(); - _pid_rate_roll.reset_I(); - _pid_rate_pitch.reset_I(); - _pid_rate_yaw.reset_I(); + get_rate_roll_pid().reset_I(); + get_rate_pitch_pid().reset_I(); + get_rate_yaw_pid().reset_I(); // Write euler derivatives derived from vehicle angular velocity to // _att_target_euler_rate_rads. This resets the state of the input shapers. @@ -503,18 +516,18 @@ float AC_AttitudeControl::rate_bf_to_motor_roll(float rate_target_rads) float rate_error_rads = rate_target_rads - current_rate_rads; // For legacy reasons, we convert to centi-degrees before inputting to the PID - _pid_rate_roll.set_input_filter_d(degrees(rate_error_rads)*100.0f); - _pid_rate_roll.set_desired_rate(degrees(rate_target_rads)*100.0f); + get_rate_roll_pid().set_input_filter_d(degrees(rate_error_rads)*100.0f); + get_rate_roll_pid().set_desired_rate(degrees(rate_target_rads)*100.0f); - float integrator = _pid_rate_roll.get_integrator(); + float integrator = get_rate_roll_pid().get_integrator(); // Ensure that integrator can only be reduced if the output is saturated if (!_motors.limit.roll_pitch || ((integrator > 0 && rate_error_rads < 0) || (integrator < 0 && rate_error_rads > 0))) { - integrator = _pid_rate_roll.get_i(); + integrator = get_rate_roll_pid().get_i(); } // Compute output in range -1 ~ +1 - float output = (_pid_rate_roll.get_p() + integrator + _pid_rate_roll.get_d()) / 4500.0f; + float output = (get_rate_roll_pid().get_p() + integrator + get_rate_roll_pid().get_d()) / 4500.0f; // Constrain output return constrain_float(output, -1.0f, 1.0f); @@ -526,18 +539,18 @@ float AC_AttitudeControl::rate_bf_to_motor_pitch(float rate_target_rads) float rate_error_rads = rate_target_rads - current_rate_rads; // For legacy reasons, we convert to centi-degrees before inputting to the PID - _pid_rate_pitch.set_input_filter_d(degrees(rate_error_rads)*100.0f); - _pid_rate_pitch.set_desired_rate(degrees(rate_target_rads)*100.0f); + get_rate_pitch_pid().set_input_filter_d(degrees(rate_error_rads)*100.0f); + get_rate_pitch_pid().set_desired_rate(degrees(rate_target_rads)*100.0f); - float integrator = _pid_rate_pitch.get_integrator(); + float integrator = get_rate_pitch_pid().get_integrator(); // Ensure that integrator can only be reduced if the output is saturated if (!_motors.limit.roll_pitch || ((integrator > 0 && rate_error_rads < 0) || (integrator < 0 && rate_error_rads > 0))) { - integrator = _pid_rate_pitch.get_i(); + integrator = get_rate_pitch_pid().get_i(); } // Compute output in range -1 ~ +1 - float output = (_pid_rate_pitch.get_p() + integrator + _pid_rate_pitch.get_d()) / 4500.0f; + float output = (get_rate_pitch_pid().get_p() + integrator + get_rate_pitch_pid().get_d()) / 4500.0f; // Constrain output return constrain_float(output, -1.0f, 1.0f); @@ -549,18 +562,18 @@ float AC_AttitudeControl::rate_bf_to_motor_yaw(float rate_target_rads) float rate_error_rads = rate_target_rads - current_rate_rads; // For legacy reasons, we convert to centi-degrees before inputting to the PID - _pid_rate_yaw.set_input_filter_all(degrees(rate_error_rads)*100.0f); - _pid_rate_yaw.set_desired_rate(degrees(rate_target_rads)*100.0f); + get_rate_yaw_pid().set_input_filter_all(degrees(rate_error_rads)*100.0f); + get_rate_yaw_pid().set_desired_rate(degrees(rate_target_rads)*100.0f); - float integrator = _pid_rate_yaw.get_integrator(); + float integrator = get_rate_yaw_pid().get_integrator(); // Ensure that integrator can only be reduced if the output is saturated if (!_motors.limit.yaw || ((integrator > 0 && rate_error_rads < 0) || (integrator < 0 && rate_error_rads > 0))) { - integrator = _pid_rate_yaw.get_i(); + integrator = get_rate_yaw_pid().get_i(); } // Compute output in range -1 ~ +1 - float output = (_pid_rate_yaw.get_p() + integrator + _pid_rate_yaw.get_d()) / 4500.0f; + float output = (get_rate_yaw_pid().get_p() + integrator + get_rate_yaw_pid().get_d()) / 4500.0f; // Constrain output return constrain_float(output, -1.0f, 1.0f); @@ -671,21 +684,21 @@ void AC_AttitudeControl::get_rotation_reference_to_vehicle(Matrix3f& m) float AC_AttitudeControl::max_rate_step_bf_roll() { - float alpha = _pid_rate_roll.get_filt_alpha(); + float alpha = get_rate_roll_pid().get_filt_alpha(); float alpha_remaining = 1-alpha; - return AC_ATTITUDE_RATE_RP_CONTROLLER_OUT_MAX/((alpha_remaining*alpha_remaining*alpha_remaining*alpha*_pid_rate_roll.kD())/_dt + _pid_rate_roll.kP()); + return AC_ATTITUDE_RATE_RP_CONTROLLER_OUT_MAX/((alpha_remaining*alpha_remaining*alpha_remaining*alpha*get_rate_roll_pid().kD())/_dt + get_rate_roll_pid().kP()); } float AC_AttitudeControl::max_rate_step_bf_pitch() { - float alpha = _pid_rate_pitch.get_filt_alpha(); + float alpha = get_rate_pitch_pid().get_filt_alpha(); float alpha_remaining = 1-alpha; - return AC_ATTITUDE_RATE_RP_CONTROLLER_OUT_MAX/((alpha_remaining*alpha_remaining*alpha_remaining*alpha*_pid_rate_pitch.kD())/_dt + _pid_rate_pitch.kP()); + return AC_ATTITUDE_RATE_RP_CONTROLLER_OUT_MAX/((alpha_remaining*alpha_remaining*alpha_remaining*alpha*get_rate_pitch_pid().kD())/_dt + get_rate_pitch_pid().kP()); } float AC_AttitudeControl::max_rate_step_bf_yaw() { - float alpha = _pid_rate_yaw.get_filt_alpha(); + float alpha = get_rate_yaw_pid().get_filt_alpha(); float alpha_remaining = 1-alpha; - return AC_ATTITUDE_RATE_RP_CONTROLLER_OUT_MAX/((alpha_remaining*alpha_remaining*alpha_remaining*alpha*_pid_rate_yaw.kD())/_dt + _pid_rate_yaw.kP()); + return AC_ATTITUDE_RATE_RP_CONTROLLER_OUT_MAX/((alpha_remaining*alpha_remaining*alpha_remaining*alpha*get_rate_yaw_pid().kD())/_dt + get_rate_yaw_pid().kP()); } diff --git a/libraries/AC_AttitudeControl/AC_AttitudeControl.h b/libraries/AC_AttitudeControl/AC_AttitudeControl.h index afcb98e7cd..b3ce06842f 100644 --- a/libraries/AC_AttitudeControl/AC_AttitudeControl.h +++ b/libraries/AC_AttitudeControl/AC_AttitudeControl.h @@ -13,9 +13,43 @@ #include #include +// default rate controller PID gains +#ifndef AC_ATC_MULTI_RATE_RP_P + # define AC_ATC_MULTI_RATE_RP_P 0.150f +#endif +#ifndef AC_ATC_MULTI_RATE_RP_I + # define AC_ATC_MULTI_RATE_RP_I 0.100f +#endif +#ifndef AC_ATC_MULTI_RATE_RP_D + # define AC_ATC_MULTI_RATE_RP_D 0.004f +#endif +#ifndef AC_ATC_MULTI_RATE_RP_IMAX + # define AC_ATC_MULTI_RATE_RP_IMAX 2000.0f +#endif +#ifndef AC_ATC_MULTI_RATE_RP_FILT_HZ + # define AC_ATC_MULTI_RATE_RP_FILT_HZ 20.0f +#endif +#ifndef AC_ATC_MULTI_RATE_YAW_P + # define AC_ATC_MULTI_RATE_YAW_P 0.200f +#endif +#ifndef AC_ATC_MULTI_RATE_YAW_I + # define AC_ATC_MULTI_RATE_YAW_I 0.020f +#endif +#ifndef AC_ATC_MULTI_RATE_YAW_D + # define AC_ATC_MULTI_RATE_YAW_D 0.0f +#endif +#ifndef AC_ATC_MULTI_RATE_YAW_IMAX + # define AC_ATC_MULTI_RATE_YAW_IMAX 1000.0f +#endif +#ifndef AC_ATC_MULTI_RATE_YAW_FILT_HZ + # define AC_ATC_MULTI_RATE_YAW_FILT_HZ 5.0f +#endif + // TODO: change the name or move to AP_Math? eliminate in favor of degrees(100)? #define AC_ATTITUDE_CONTROL_DEGX100 5729.57795f // constant to convert from radians to centidegrees +#define AC_ATTITUDE_CONTROL_ANGLE_P 4.5f // default angle P gain for roll, pitch and yaw + #define AC_ATTITUDE_ACCEL_RP_CONTROLLER_MIN_RADSS radians(40.0f) // minimum body-frame acceleration limit for the stability controller (for roll and pitch axis) #define AC_ATTITUDE_ACCEL_RP_CONTROLLER_MAX_RADSS radians(720.0f) // maximum body-frame acceleration limit for the stability controller (for roll and pitch axis) #define AC_ATTITUDE_ACCEL_Y_CONTROLLER_MIN_RADSS radians(10.0f) // minimum body-frame acceleration limit for the stability controller (for yaw axis) @@ -45,22 +79,20 @@ public: AC_AttitudeControl( AP_AHRS &ahrs, const AP_Vehicle::MultiCopter &aparm, AP_Motors& motors, - AC_P& pi_angle_roll, AC_P& pi_angle_pitch, AC_P& pi_angle_yaw, - AC_PID& pid_rate_roll, AC_PID& pid_rate_pitch, AC_PID& pid_rate_yaw - ) : - _dt(AC_ATTITUDE_400HZ_DT), + float dt) : + _p_angle_roll(AC_ATTITUDE_CONTROL_ANGLE_P), + _p_angle_pitch(AC_ATTITUDE_CONTROL_ANGLE_P), + _p_angle_yaw(AC_ATTITUDE_CONTROL_ANGLE_P), + _dt(dt), _angle_boost(0), _att_ctrl_use_accel_limit(true), _throttle_in_filt(AC_ATTITUDE_CONTROL_ALTHOLD_LEANANGLE_FILT_HZ), _ahrs(ahrs), _aparm(aparm), _motors(motors), - _p_angle_roll(pi_angle_roll), - _p_angle_pitch(pi_angle_pitch), - _p_angle_yaw(pi_angle_yaw), - _pid_rate_roll(pid_rate_roll), - _pid_rate_pitch(pid_rate_pitch), - _pid_rate_yaw(pid_rate_yaw) + _pid_rate_roll(AC_ATC_MULTI_RATE_RP_P, AC_ATC_MULTI_RATE_RP_I, AC_ATC_MULTI_RATE_RP_D, AC_ATC_MULTI_RATE_RP_IMAX, AC_ATC_MULTI_RATE_RP_FILT_HZ, dt), + _pid_rate_pitch(AC_ATC_MULTI_RATE_RP_P, AC_ATC_MULTI_RATE_RP_I, AC_ATC_MULTI_RATE_RP_D, AC_ATC_MULTI_RATE_RP_IMAX, AC_ATC_MULTI_RATE_RP_FILT_HZ, dt), + _pid_rate_yaw(AC_ATC_MULTI_RATE_YAW_P, AC_ATC_MULTI_RATE_YAW_I, AC_ATC_MULTI_RATE_YAW_D, AC_ATC_MULTI_RATE_YAW_IMAX, AC_ATC_MULTI_RATE_YAW_FILT_HZ, dt) { AP_Param::setup_object_defaults(this, var_info); } @@ -68,8 +100,13 @@ public: // Empty destructor to suppress compiler warning virtual ~AC_AttitudeControl() {} - // Set_dt - sets time delta in seconds for all controllers (i.e. 100hz = 0.01, 400hz = 0.0025) - void set_dt(float delta_sec); + // pid accessors + AC_P& get_angle_roll_p() { return _p_angle_roll; } + AC_P& get_angle_pitch_p() { return _p_angle_pitch; } + AC_P& get_angle_yaw_p() { return _p_angle_yaw; } + virtual AC_PID& get_rate_roll_pid() = 0; + virtual AC_PID& get_rate_pitch_pid() = 0; + virtual AC_PID& get_rate_yaw_pid() = 0; // Gets the roll acceleration limit in centidegrees/s/s float get_accel_roll_max() { return _accel_roll_max; } @@ -301,6 +338,14 @@ protected: // Enable/Disable angle boost AP_Int8 _angle_boost_enabled; + // angle controller P objects + AC_P _p_angle_roll; + AC_P _p_angle_pitch; + AC_P _p_angle_yaw; + AC_PID _pid_rate_roll; + AC_PID _pid_rate_pitch; + AC_PID _pid_rate_yaw; + // Intersampling period in seconds float _dt; @@ -346,12 +391,6 @@ protected: const AP_AHRS& _ahrs; const AP_Vehicle::MultiCopter &_aparm; AP_Motors& _motors; - AC_P& _p_angle_roll; - AC_P& _p_angle_pitch; - AC_P& _p_angle_yaw; - AC_PID& _pid_rate_roll; - AC_PID& _pid_rate_pitch; - AC_PID& _pid_rate_yaw; }; #define AC_ATTITUDE_CONTROL_LOG_FORMAT(msg) { msg, sizeof(AC_AttitudeControl::log_Attitude), \