Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-26 17:53:59 -04:00
Code Issues Packages Projects Releases Wiki Activity

AC_AttControl: add RATE_RP_MAX, RATE_Y_MAX params

Browse Source 
These replace the ANGLE_RATE_MAX parameter from the main code
This commit is contained in:
Randy Mackay 2014-02-01 15:27:58 +09:00 committed by Andrew Tridgell
parent c2efb91ece
commit 2dae0d68c5
2 changed files with 48 additions and 25 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

54
libraries/AC_AttitudeControl/AC_AttitudeControl.cpp
View File

@ -8,12 +8,21 @@ extern const AP_HAL::HAL& hal;
// table of user settable parameters
const AP_Param::GroupInfo AC_AttitudeControl::var_info[] PROGMEM = {
// @Param: DUMMY
// @DisplayName: Dummy parameter
// @Description: This is the dummy parameters
// @Increment: 0.1
// @User: User
AP_GROUPINFO("DUMMY", 0, AC_AttitudeControl, _dummy_param, 0),
// @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
// @Range: 90000 250000
// @Increment: 500
// @User: Advanced
AP_GROUPINFO("RATE_RP_MAX", 0, AC_AttitudeControl, _angle_rate_rp_max, AC_ATTITUDE_CONTROL_ANGLE_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
// @Range: 90000 250000
// @Increment: 500
// @User: Advanced
AP_GROUPINFO("RATE_Y_MAX", 1, AC_AttitudeControl, _angle_rate_y_max, AC_ATTITUDE_CONTROL_ANGLE_RATE_Y_MAX_DEFAULT),
AP_GROUPEND
};
@ -122,14 +131,14 @@ void AC_AttitudeControl::angle_to_rate_ef_roll()
// To-Do: is this being converted to int32_t as part of wrap_180_cd?
float angle_error_cd = wrap_180_cd(_angle_ef_target.x - _ahrs.roll_sensor);
// limit the error we're feeding to the PID
// Does this make any sense to limit the angular error by the maximum lean angle of the copter? Is it the responsibility of the rate controller to know it's own limits?
// To-Do: move aparm.angle_max into the AP_Vehicles class?
angle_error_cd = constrain_float(angle_error_cd, -AC_ATTITUDE_ANGLE_CONTROLLER_ANGLE_MAX, AC_ATTITUDE_ANGLE_CONTROLLER_ANGLE_MAX);
// convert to desired earth-frame rate
// To-Do: replace PI controller with just a single gain?
_rate_ef_target.x = _pi_angle_roll.kP() * angle_error_cd;
// constrain rate request
if (_flags.limit_angle_to_rate_request) {
_rate_ef_target.x = constrain_float(_rate_ef_target.x,-_angle_rate_rp_max,_angle_rate_rp_max);
}
}
// angle_to_rate_ef_pitch - ask the angle controller to calculate the earth frame rate targets for pitch
@ -139,14 +148,14 @@ void AC_AttitudeControl::angle_to_rate_ef_pitch()
// To-Do: is this being converted to int32_t as part of wrap_180_cd?
float angle_error_cd = wrap_180_cd(_angle_ef_target.y - _ahrs.pitch_sensor);
// limit the error we're feeding to the PID
// Does this make any sense to limit the angular error by the maximum lean angle of the copter? Is it the responsibility of the rate controller to know it's own limits?
// To-Do: move aparm.angle_max into the AP_Vehicles class?
angle_error_cd = constrain_float(angle_error_cd, -AC_ATTITUDE_ANGLE_CONTROLLER_ANGLE_MAX, AC_ATTITUDE_ANGLE_CONTROLLER_ANGLE_MAX);
// convert to desired earth-frame rate
// To-Do: replace PI controller with just a single gain?
_rate_ef_target.y = _pi_angle_pitch.kP() * angle_error_cd;
// constrain rate request
if (_flags.limit_angle_to_rate_request) {
_rate_ef_target.y = constrain_float(_rate_ef_target.y,-_angle_rate_rp_max,_angle_rate_rp_max);
}
}
// angle_to_rate_ef_yaw - ask the angle controller to calculate the earth-frame yaw rate in centi-degrees/second
@ -156,16 +165,16 @@ void AC_AttitudeControl::angle_to_rate_ef_yaw()
// To-Do: is this being converted to int32_t as part of wrap_180_cd?
float angle_error_cd = wrap_180_cd(_angle_ef_target.z - _ahrs.yaw_sensor);
// limit the error we're feeding to the PID
angle_error_cd = constrain_float(angle_error_cd, -AC_ATTITUDE_ANGLE_YAW_CONTROLLER_OUT_MAX, AC_ATTITUDE_ANGLE_YAW_CONTROLLER_OUT_MAX);
// convert to desired earth-frame rate in centi-degrees/second
// To-Do: replace PI controller with just a single gain?
_rate_ef_target.z = _pi_angle_yaw.kP() * angle_error_cd;
// To-Do: deal with trad helicopter which do not use yaw rate controllers if using external gyros
// constrain rate request
if (_flags.limit_angle_to_rate_request) {
_rate_ef_target.y = constrain_float(_rate_ef_target.y,-_angle_rate_y_max,_angle_rate_y_max);
}
// To-Do: allow logging of PIDs?
// To-Do: deal with trad helicopter which do not use yaw rate controllers if using external gyros
}
//
@ -341,9 +350,6 @@ void AC_AttitudeControl::rate_controller_run()
_motor_roll = rate_bf_to_motor_roll(_rate_bf_target.x);
_motor_pitch = rate_bf_to_motor_pitch(_rate_bf_target.y);
_motor_yaw = rate_bf_to_motor_yaw(_rate_bf_target.z);
// To-Do: what about throttle?
//_motor_
}
//

19
libraries/AC_AttitudeControl/AC_AttitudeControl.h
View File

@ -18,6 +18,8 @@
// 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_ANGLE_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_ANGLE_RATE_Y_MAX_DEFAULT 18000 // maximum rotation rate on yaw axis requested by angle controller used in stabilize, loiter, rtl, auto flight modes
#define AC_ATTITUDE_RATE_CONTROLLER_TIMEOUT 1.0f // body-frame rate controller timeout in seconds
#define AC_ATTITUDE_RATE_RP_CONTROLLER_OUT_MAX 5000.0f // body-frame rate controller maximum output (for roll-pitch axis)
#define AC_ATTITUDE_RATE_YAW_CONTROLLER_OUT_MAX 4500.0f // body-frame rate controller maximum output (for yaw axis)
@ -63,6 +65,9 @@ public:
_sin_pitch(0.0)
{
AP_Param::setup_object_defaults(this, var_info);
// initialise flags
_flags.limit_angle_to_rate_request = true;
}
//
@ -88,6 +93,9 @@ public:
// ratebf_rpy - attempts to maintain a roll, pitch and yaw rate (all body frame)
void ratebf_rpy(float roll_rate_bf, float pitch_rate_bf, float yaw_rate_bf);
// limit_angle_to_rate_request
void limit_angle_to_rate_request(bool limit_request) { _flags.limit_angle_to_rate_request = limit_request; }
//
// angle controller (earth-frame) methods
//
@ -158,6 +166,9 @@ public:
// To-Do: can we return these Vector3fs by reference? i.e. using "&"?
Vector3f rate_bf_targets() const { return _rate_bf_target; }
void rate_bf_targets(Vector3f& rates_cds) { _rate_bf_target = rates_cds; }
void rate_bf_roll_target(float rate_cds) { _rate_bf_target.x = rate_cds; }
void rate_bf_pitch_target(float rate_cds) { _rate_bf_target.y = rate_cds; }
void rate_bf_yaw_target(float rate_cds) { _rate_bf_target.z = rate_cds; }
// rate_controller_run - run lowest level body-frame rate controller and send outputs to the motors
// should be called at 100hz or more
@ -195,6 +206,11 @@ public:
protected:
// attitude control flags
struct AttControlFlags {
uint8_t limit_angle_to_rate_request : 1; // 1 if the earth frame angle controller is limiting it's maximum rate request
} _flags;
//
// body-frame rate controller
//
@ -246,7 +262,8 @@ protected:
int16_t& _motor_throttle; // g.rc_3.servo_out
// parameters
AP_Float _dummy_param;
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
// internal variables
// To-Do: make rate targets a typedef instead of Vector3f?