aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Plane: allow fwd motor to run in VTOL land hor positioning

Browse Source 
this fixes an issue with vtol landings below the takeoff point
This commit is contained in:
Andrew Tridgell 2019-02-23 11:20:20 +11:00
parent 2d14d79a65
commit 7908ad437e
1 changed files with 18 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

24
ArduPlane/quadplane.cpp
View File

@ -2638,12 +2638,24 @@ int8_t QuadPlane::forward_throttle_pct(void)
// inhibit reverse throttle and allow petrol engines with min > 0
int8_t fwd_throttle_min = plane.have_reverse_thrust() ? 0 : plane.aparm.throttle_min;
vel_forward.integrator = constrain_float(vel_forward.integrator, fwd_throttle_min, plane.aparm.throttle_max);
// If we are below alt_cutoff then scale down the effect until it turns off at alt_cutoff and decay the integrator
float alt_cutoff = MAX(0,vel_forward_alt_cutoff);
float height_above_ground = plane.relative_ground_altitude(plane.g.rangefinder_landing);
vel_forward.last_pct = linear_interpolate(0, vel_forward.integrator,
height_above_ground, alt_cutoff, alt_cutoff+2);
if (in_vtol_auto() && is_vtol_land(plane.mission.get_current_nav_cmd().id) &&
(poscontrol.state == QPOS_POSITION1 || poscontrol.state == QPOS_POSITION2) &&
motors->get_desired_spool_state() >= AP_Motors::DESIRED_GROUND_IDLE) {
// 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
vel_forward.last_pct = vel_forward.integrator;
} else {
// If we are below alt_cutoff then scale down the effect until
// it turns off at alt_cutoff and decay the integrator
float alt_cutoff = MAX(0,vel_forward_alt_cutoff);
float height_above_ground = plane.relative_ground_altitude(plane.g.rangefinder_landing);
vel_forward.last_pct = linear_interpolate(0, vel_forward.integrator,
height_above_ground, alt_cutoff, alt_cutoff+2);
}
if (vel_forward.last_pct == 0) {
// if the percent is 0 then decay the integrator
vel_forward.integrator *= 0.95f;