Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-09 17:38:32 -04:00
Code Issues 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 7a33d949ca
commit c755deff21
1 changed files with 17 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

13
ArduPlane/quadplane.cpp
View File

@ -2405,11 +2405,22 @@ int8_t QuadPlane::forward_throttle_pct(void)
int8_t fwd_throttle_min = (plane.aparm.throttle_min <= 0) ? 0 : plane.aparm.throttle_min; int8_t fwd_throttle_min = (plane.aparm.throttle_min <= 0) ? 0 : plane.aparm.throttle_min;
vel_forward.integrator = constrain_float(vel_forward.integrator, fwd_throttle_min, plane.aparm.throttle_max); 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 if (in_vtol_auto() && is_vtol_land(plane.mission.get_current_nav_cmd().id) &&
(poscontrol.state == QPOS_POSITION1 || poscontrol.state == QPOS_POSITION2)) {
// 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 alt_cutoff = MAX(0,vel_forward_alt_cutoff);
float height_above_ground = plane.relative_ground_altitude(plane.g.rangefinder_landing); float height_above_ground = plane.relative_ground_altitude(plane.g.rangefinder_landing);
vel_forward.last_pct = linear_interpolate(0, vel_forward.integrator, vel_forward.last_pct = linear_interpolate(0, vel_forward.integrator,
height_above_ground, alt_cutoff, alt_cutoff+2); height_above_ground, alt_cutoff, alt_cutoff+2);
}
if (vel_forward.last_pct == 0) { if (vel_forward.last_pct == 0) {
// if the percent is 0 then decay the integrator // if the percent is 0 then decay the integrator
vel_forward.integrator *= 0.95f; vel_forward.integrator *= 0.95f;