Plane: fixed height_above_ground() for case when rangefinder is below min

this prevents a rangefinder that goes below min distance from causing
the calculations that depend on height above ground to fail
This commit is contained in:
Andrew Tridgell 2019-09-11 17:20:04 +10:00
parent 34b0967d09
commit 5c43b18672
3 changed files with 22 additions and 2 deletions

View File

@ -126,7 +126,14 @@ float Plane::relative_ground_altitude(bool use_rangefinder_if_available)
{
if (use_rangefinder_if_available && rangefinder_state.in_range) {
return rangefinder_state.height_estimate;
}
}
if (use_rangefinder_if_available && quadplane.in_vtol_land_final() &&
rangefinder.status_orient(ROTATION_PITCH_270) == RangeFinder::RangeFinder_OutOfRangeLow) {
// a special case for quadplane landing when rangefinder goes
// below minimum. Consider our height above ground to be zero
return 0;
}
#if AP_TERRAIN_AVAILABLE
float altitude;

View File

@ -2795,7 +2795,7 @@ int8_t QuadPlane::forward_throttle_pct(void)
// 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 (in_vtol_land_descent() && poscontrol.state == QPOS_LAND_FINAL && motors->limit.throttle_lower) {
} else if (in_vtol_land_final() && motors->limit.throttle_lower) {
// we're in the settling phase of landing, disable fwd motor
vel_forward.last_pct = 0;
vel_forward.integrator = 0;
@ -3109,3 +3109,11 @@ bool QuadPlane::in_vtol_land_descent(void) const
}
return false;
}
/*
see if we are in the final phase of a VTOL landing
*/
bool QuadPlane::in_vtol_land_final(void) const
{
return in_vtol_land_descent() && poscontrol.state == QPOS_LAND_FINAL;
}

View File

@ -540,6 +540,11 @@ private:
are we in the descent phase of a VTOL landing?
*/
bool in_vtol_land_descent(void) const;
/*
are we in the final landing phase of a VTOL landing?
*/
bool in_vtol_land_final(void) const;
public:
void motor_test_output();