From afcd1c6ec326e631592847812a88559f3e95d4b0 Mon Sep 17 00:00:00 2001 From: Leonard Hall Date: Sun, 29 Mar 2015 20:35:23 +1030 Subject: [PATCH] AC_AttitudeControl: sqrt controller on Stab --- .../AC_AttitudeControl/AC_AttitudeControl.cpp | 34 ++++++------------- .../AC_AttitudeControl/AC_AttitudeControl.h | 8 ++--- 2 files changed, 15 insertions(+), 27 deletions(-) diff --git a/libraries/AC_AttitudeControl/AC_AttitudeControl.cpp b/libraries/AC_AttitudeControl/AC_AttitudeControl.cpp index 6446e43fa8..3e0b0293e5 100644 --- a/libraries/AC_AttitudeControl/AC_AttitudeControl.cpp +++ b/libraries/AC_AttitudeControl/AC_AttitudeControl.cpp @@ -6,23 +6,7 @@ // table of user settable parameters const AP_Param::GroupInfo AC_AttitudeControl::var_info[] PROGMEM = { - // @Param: RATE_RP_MAX - // @DisplayName: Angle Rate Roll-Pitch max - // @Description: maximum rotation rate in roll/pitch axis requested by angle controller used in stabilize, loiter, rtl, auto flight modes - // @Units: Centi-Degrees/Sec - // @Range: 9000 36000 - // @Increment: 500 - // @User: Advanced - AP_GROUPINFO("RATE_RP_MAX", 0, AC_AttitudeControl, _angle_rate_rp_max, AC_ATTITUDE_CONTROL_RATE_RP_MAX_DEFAULT), - - // @Param: RATE_Y_MAX - // @DisplayName: Angle Rate Yaw max - // @Description: maximum rotation rate in roll/pitch axis requested by angle controller used in stabilize, loiter, rtl, auto flight modes - // @Units: Centi-Degrees/Sec - // @Range: 4500 18000 - // @Increment: 500 - // @User: Advanced - AP_GROUPINFO("RATE_Y_MAX", 1, AC_AttitudeControl, _angle_rate_y_max, AC_ATTITUDE_CONTROL_RATE_Y_MAX_DEFAULT), + // 0, 1 were RATE_RP_MAX, RATE_Y_MAX // @Param: SLEW_YAW // @DisplayName: Yaw target slew rate @@ -543,25 +527,29 @@ void AC_AttitudeControl::integrate_bf_rate_error_to_angle_errors() // results in centi-degrees/sec put into _rate_bf_target void AC_AttitudeControl::update_rate_bf_targets() { + // stab roll calculation - _rate_bf_target.x = _p_angle_roll.kP() * _angle_bf_error.x; // constrain roll rate request if (_flags.limit_angle_to_rate_request) { - _rate_bf_target.x = constrain_float(_rate_bf_target.x,-_angle_rate_rp_max,_angle_rate_rp_max); + _rate_bf_target.x = sqrt_controller(_angle_bf_error.x, _p_angle_roll.kP(), constrain_float(_accel_roll_max/2.0f, AC_ATTITUDE_ACCEL_RP_CONTROLLER_MIN, AC_ATTITUDE_ACCEL_RP_CONTROLLER_MAX)); + }else{ + _rate_bf_target.x = _p_angle_roll.kP() * _angle_bf_error.x; } // stab pitch calculation - _rate_bf_target.y = _p_angle_pitch.kP() * _angle_bf_error.y; // constrain pitch rate request if (_flags.limit_angle_to_rate_request) { - _rate_bf_target.y = constrain_float(_rate_bf_target.y,-_angle_rate_rp_max,_angle_rate_rp_max); + _rate_bf_target.y = sqrt_controller(_angle_bf_error.y, _p_angle_pitch.kP(), constrain_float(_accel_pitch_max/2.0f, AC_ATTITUDE_ACCEL_RP_CONTROLLER_MIN, AC_ATTITUDE_ACCEL_RP_CONTROLLER_MAX)); + }else{ + _rate_bf_target.y = _p_angle_pitch.kP() * _angle_bf_error.y; } // stab yaw calculation - _rate_bf_target.z = _p_angle_yaw.kP() * _angle_bf_error.z; // constrain yaw rate request if (_flags.limit_angle_to_rate_request) { - _rate_bf_target.z = constrain_float(_rate_bf_target.z,-_angle_rate_y_max,_angle_rate_y_max); + _rate_bf_target.z = sqrt_controller(_angle_bf_error.z, _p_angle_yaw.kP(), constrain_float(_accel_yaw_max/2.0f, AC_ATTITUDE_ACCEL_Y_CONTROLLER_MIN, AC_ATTITUDE_ACCEL_Y_CONTROLLER_MAX)); + }else{ + _rate_bf_target.z = _p_angle_yaw.kP() * _angle_bf_error.z; } _rate_bf_target.x += _angle_bf_error.y * _ahrs.get_gyro().z; diff --git a/libraries/AC_AttitudeControl/AC_AttitudeControl.h b/libraries/AC_AttitudeControl/AC_AttitudeControl.h index c95391949b..53760c83cf 100644 --- a/libraries/AC_AttitudeControl/AC_AttitudeControl.h +++ b/libraries/AC_AttitudeControl/AC_AttitudeControl.h @@ -17,8 +17,10 @@ // To-Do: change the name or move to AP_Math? #define AC_ATTITUDE_CONTROL_DEGX100 5729.57795f // constant to convert from radians to centi-degrees -#define AC_ATTITUDE_CONTROL_RATE_RP_MAX_DEFAULT 18000 // maximum rotation rate in roll/pitch axis requested by angle controller used in stabilize, loiter, rtl, auto flight modes -#define AC_ATTITUDE_CONTROL_RATE_Y_MAX_DEFAULT 9000 // maximum rotation rate on yaw axis requested by angle controller used in stabilize, loiter, rtl, auto flight modes +#define AC_ATTITUDE_ACCEL_RP_CONTROLLER_MIN 36000.0f// minimum body-frame acceleration limit for the stability controller (for roll and pitch axis) +#define AC_ATTITUDE_ACCEL_RP_CONTROLLER_MAX 72000.0f// maximum body-frame acceleration limit for the stability controller (for roll and pitch axis) +#define AC_ATTITUDE_ACCEL_Y_CONTROLLER_MIN 18000.0f// minimum body-frame acceleration limit for the stability controller (for yaw axis) +#define AC_ATTITUDE_ACCEL_Y_CONTROLLER_MAX 36000.0f// maximum body-frame acceleration limit for the stability controller (for yaw axis) #define AC_ATTITUDE_CONTROL_SLEW_YAW_DEFAULT 1000 // constraint on yaw angle error in degrees. This should lead to maximum turn rate of 10deg/sed * Stab Rate P so by default will be 45deg/sec. #define AC_ATTITUDE_CONTROL_ACCEL_RP_MAX_DEFAULT 0 // default maximum acceleration for roll/pitch axis in centi-degrees/sec/sec #define AC_ATTITUDE_CONTROL_ACCEL_Y_MAX_DEFAULT 0 // default maximum acceleration for yaw axis in centi-degrees/sec/sec @@ -246,8 +248,6 @@ protected: AC_PID& _pid_rate_yaw; // parameters - AP_Float _angle_rate_rp_max; // maximum rate request output from the earth-frame angle controller for roll and pitch axis - AP_Float _angle_rate_y_max; // maximum rate request output from the earth-frame angle controller for yaw axis AP_Float _slew_yaw; // maximum rate the yaw target can be updated in Loiter, RTL, Auto flight modes AP_Float _accel_roll_max; // maximum rotation acceleration for earth-frame roll axis AP_Float _accel_pitch_max; // maximum rotation acceleration for earth-frame pitch axis