AC_AttControl: get_max_rate_step_bf_roll, pitch and yaw

libraries/AC_AttitudeControl/AC_AttitudeControl.cpp

@@ -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());
+}

libraries/AC_AttitudeControl/AC_AttitudeControl.h

@@ -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; }