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

AC_AttitudeControl: sqrt controller on Stab

This commit is contained in:
Leonard Hall 2015-03-29 20:35:23 +10:30 committed by Randy Mackay
parent ae77c18a19
commit afcd1c6ec3
2 changed files with 15 additions and 27 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

34
libraries/AC_AttitudeControl/AC_AttitudeControl.cpp
View File

@ -6,23 +6,7 @@
// table of user settable parameters // table of user settable parameters
const AP_Param::GroupInfo AC_AttitudeControl::var_info[] PROGMEM = { const AP_Param::GroupInfo AC_AttitudeControl::var_info[] PROGMEM = {
// @Param: RATE_RP_MAX // 0, 1 were RATE_RP_MAX, RATE_Y_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),
// @Param: SLEW_YAW // @Param: SLEW_YAW
// @DisplayName: Yaw target slew rate // @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 // results in centi-degrees/sec put into _rate_bf_target
void AC_AttitudeControl::update_rate_bf_targets() void AC_AttitudeControl::update_rate_bf_targets()
{ {
// stab roll calculation // stab roll calculation
_rate_bf_target.x = _p_angle_roll.kP() * _angle_bf_error.x;
// constrain roll rate request // constrain roll rate request
if (_flags.limit_angle_to_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 // stab pitch calculation
_rate_bf_target.y = _p_angle_pitch.kP() * _angle_bf_error.y;
// constrain pitch rate request // constrain pitch rate request
if (_flags.limit_angle_to_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 // stab yaw calculation
_rate_bf_target.z = _p_angle_yaw.kP() * _angle_bf_error.z;
// constrain yaw rate request // constrain yaw rate request
if (_flags.limit_angle_to_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; _rate_bf_target.x += _angle_bf_error.y * _ahrs.get_gyro().z;

8
libraries/AC_AttitudeControl/AC_AttitudeControl.h
View File

@ -17,8 +17,10 @@
// To-Do: change the name or move to AP_Math? // 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_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_ACCEL_RP_CONTROLLER_MIN 36000.0f// minimum body-frame acceleration limit for the stability controller (for roll and pitch axis)
#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_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_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_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 #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; AC_PID& _pid_rate_yaw;
// parameters // 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 _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_roll_max; // maximum rotation acceleration for earth-frame roll axis
AP_Float _accel_pitch_max; // maximum rotation acceleration for earth-frame pitch axis AP_Float _accel_pitch_max; // maximum rotation acceleration for earth-frame pitch axis