diff --git a/ArduPlane/quadplane.cpp b/ArduPlane/quadplane.cpp index 919a4ae8b9..61e0450c3b 100644 --- a/ArduPlane/quadplane.cpp +++ b/ArduPlane/quadplane.cpp @@ -2985,13 +2985,18 @@ void QuadPlane::assign_tilt_to_fwd_thr(void) { float fwd_tilt_rad = radians(constrain_float(-0.01f * (float)plane.nav_pitch_cd, 0.0f, 45.0f)); q_fwd_throttle = MIN(q_fwd_thr_gain * tanf(fwd_tilt_rad), 1.0f); - // Slowly relax forward tilt limit if the position controller is saturating in the forward direction because + // Relax forward tilt limit if the position controller is saturating in the forward direction because // the forward thrust motor could be failed const float fwd_tilt_range_cd = (float)aparm.angle_max - 100.0f * q_fwd_pitch_lim; if (is_positive(fwd_tilt_range_cd)) { + // rate limit the forward tilt change to slew between the motor good and motor failed + // value over 10 seconds const float fwd_pitch_lim_cd_tgt = plane.quadplane.pos_control->get_fwd_pitch_is_limited() ? (float)aparm.angle_max : 100.0f * q_fwd_pitch_lim; const float delta_max = 0.1f * fwd_tilt_range_cd * plane.G_Dt; q_fwd_pitch_lim_cd += constrain_float((fwd_pitch_lim_cd_tgt - q_fwd_pitch_lim_cd), -delta_max, delta_max); + // Don't let the forward pitch limit be more than the forward pitch demand before limiting to + // avoid opening up the limit more than necessary + q_fwd_pitch_lim_cd = MIN(q_fwd_pitch_lim_cd, MAX(-(float)plane.nav_pitch_cd, 100.0f * q_fwd_pitch_lim)); } else { // take the lesser of the two limits q_fwd_pitch_lim_cd = (float)aparm.angle_max; @@ -3017,7 +3022,7 @@ void QuadPlane::assign_tilt_to_fwd_thr(void) { // Diagnostics logging - remove when feature is fully flight tested. AP::logger().WriteStreaming("FWDT", - "TimeUS,fts,qfpcd,npllcd,npcd,qft", // labels + "TimeUS,fts,qfplcd,npllcd,npcd,qft", // labels "Qfffff", // fmt AP_HAL::micros64(), (double)fwd_thr_scaler,