From b3a1807349024748eb36f848f6fe91d090f5a5c1 Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Wed, 30 Aug 2023 20:52:40 +1000 Subject: [PATCH] Plane: Fix use of q_fwd_nav_pitch_lim_cd class variable --- ArduPlane/quadplane.cpp | 19 ++++++++++--------- ArduPlane/quadplane.h | 1 - 2 files changed, 10 insertions(+), 10 deletions(-) diff --git a/ArduPlane/quadplane.cpp b/ArduPlane/quadplane.cpp index fbf2440603..64e972fd78 100644 --- a/ArduPlane/quadplane.cpp +++ b/ArduPlane/quadplane.cpp @@ -655,7 +655,6 @@ bool QuadPlane::setup(void) } q_fwd_throttle = 0.0f; - q_fwd_nav_pitch_lim_cd = -aparm.angle_max; /* cope with upgrade from old AP_Motors values for frame_class @@ -2931,22 +2930,24 @@ void QuadPlane::assign_tilt_to_fwd_thr(void) { // Handle the case where we are limiting the forward pitch angle to prevent negative wing lift // and are using the forward thrust motor or tilting rotors to provide the forward acceleration float fwd_tilt_rad = radians(constrain_float(-0.01f * (float)plane.nav_pitch_cd, 0.0f, 45.0f)); - q_fwd_nav_pitch_lim_cd = (int32_t)(-100.0f * q_fwd_pitch_lim); - plane.nav_pitch_cd = MAX(plane.nav_pitch_cd, q_fwd_nav_pitch_lim_cd); q_fwd_throttle = MIN(q_fwd_thr_gain * tanf(fwd_tilt_rad), 1.0f); + int32_t fwd_pitch_lim_cd = (int32_t)(-100.0f * q_fwd_pitch_lim); - // To prevent forward motor prop strike, reduce throttle to zero when close to ground - // When we are doing horizontal positioning in a VTOL land - // we always allow the fwd motor to run. Otherwise a bad - // lidar could cause the aircraft not to be able to - // approach the landing point when landing below the takeoff point if (!in_vtol_land_approach()) { + // To prevent forward motor prop strike, reduce throttle to zero when close to ground. + // When we are doing horizontal positioning in a VTOL land we always allow the fwd motor + // to run. Otherwise a bad height above landing point estimate could cause the aircraft + // not to be able to approach the landing point float alt_cutoff = MAX(0,vel_forward_alt_cutoff); float height_above_ground = plane.relative_ground_altitude(plane.g.rangefinder_landing); float fwd_thr_scaler = linear_interpolate(0.0f, 1.0f, height_above_ground, alt_cutoff, alt_cutoff+2); q_fwd_throttle *= fwd_thr_scaler; - q_fwd_nav_pitch_lim_cd = - (int32_t)((float)aparm.angle_max * (1.0f - fwd_thr_scaler) + 100.0f * q_fwd_pitch_lim * fwd_thr_scaler); + // When reducing forward throttle use, relax forward pitch limit to maintain forward + // acceleration capability. + fwd_pitch_lim_cd = - (int32_t)((float)aparm.angle_max * (1.0f - fwd_thr_scaler) + 100.0f * q_fwd_pitch_lim * fwd_thr_scaler); } + + plane.nav_pitch_cd = MAX(plane.nav_pitch_cd, fwd_pitch_lim_cd); } /* diff --git a/ArduPlane/quadplane.h b/ArduPlane/quadplane.h index c9d6e8ef51..6274985560 100644 --- a/ArduPlane/quadplane.h +++ b/ArduPlane/quadplane.h @@ -418,7 +418,6 @@ private: Location last_auto_target; float q_fwd_throttle; // forward throttle used in q modes - int32_t q_fwd_nav_pitch_lim_cd; // forward tilt limit used in q modes in centi-degrees // when did we last run the attitude controller? uint32_t last_att_control_ms;