mirror of https://github.com/ArduPilot/ardupilot
AC_AttControl: get_max_rate_step_bf_roll, pitch and yaw
This commit is contained in:
parent
f00025e5c9
commit
9833c91b2b
|
@ -715,3 +715,27 @@ float AC_AttitudeControl::sqrt_controller(float error, float p, float second_ord
|
||||||
return error*p;
|
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());
|
||||||
|
}
|
||||||
|
|
|
@ -137,6 +137,20 @@ public:
|
||||||
void rate_bf_pitch_target(float rate_cds) { _rate_bf_target.y = 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; }
|
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)
|
// rate_ef_targets - returns rate controller body-frame targets (for reporting)
|
||||||
const Vector3f& rate_bf_targets() const { return _rate_bf_target; }
|
const Vector3f& rate_bf_targets() const { return _rate_bf_target; }
|
||||||
|
|
||||||
|
|
Loading…
Reference in New Issue