Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-03 06:28:27 -04:00
Code Issues Packages Projects Releases Wiki Activity

Plane: Fix use of q_fwd_nav_pitch_lim_cd class variable

Browse Source
This commit is contained in:
Paul Riseborough 2023-08-30 20:52:40 +10:00 committed by Andrew Tridgell
parent 16895003df
commit b3a1807349
2 changed files with 10 additions and 10 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

19
ArduPlane/quadplane.cpp
View File

@ -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);
}
/*

1
ArduPlane/quadplane.h
View File

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