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

AC_AttControl: get_max_rate_step_bf_roll, pitch and yaw

This commit is contained in:
Leonard Hall 2015-02-11 22:03:36 +09:00 committed by Randy Mackay
parent f00025e5c9
commit 9833c91b2b
2 changed files with 38 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

24
libraries/AC_AttitudeControl/AC_AttitudeControl.cpp
View File

@ -715,3 +715,27 @@ float AC_AttitudeControl::sqrt_controller(float error, float p, float second_ord
return error*p;
}
}
// Maximum roll rate step size that results in maximum output after 4 time steps
float AC_AttitudeControl::max_rate_step_bf_roll()
{
float alpha = _pid_rate_roll.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());
}
// Maximum pitch rate step size that results in maximum output after 4 time steps
float AC_AttitudeControl::max_rate_step_bf_pitch()
{
float alpha = _pid_rate_pitch.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());
}
// Maximum yaw rate step size that results in maximum output after 4 time steps
float AC_AttitudeControl::max_rate_step_bf_yaw()
{
float alpha = _pid_rate_yaw.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());
}

14
libraries/AC_AttitudeControl/AC_AttitudeControl.h
View File

@ -137,6 +137,20 @@ public:
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; }
// Maximum roll rate step size that results in maximum output after 4 time steps
float max_rate_step_bf_roll();
// Maximum pitch rate step size that results in maximum output after 4 time steps
float max_rate_step_bf_pitch();
// Maximum yaw rate step size that results in maximum output after 4 time steps
float max_rate_step_bf_yaw();
// Maximum roll step size that results in maximum output after 4 time steps
float max_angle_step_bf_roll() { return max_rate_step_bf_roll()/_p_angle_roll.kP(); }
// Maximum pitch step size that results in maximum output after 4 time steps
float max_angle_step_bf_pitch() { return max_rate_step_bf_pitch()/_p_angle_pitch.kP(); }
// Maximum yaw step size that results in maximum output after 4 time steps
float max_angle_step_bf_yaw() { return max_rate_step_bf_yaw()/_p_angle_yaw.kP(); }
// rate_ef_targets - returns rate controller body-frame targets (for reporting)
const Vector3f& rate_bf_targets() const { return _rate_bf_target; }