Browse Source

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
master
Andrew Tridgell 5 years ago
parent
commit
5c43b18672
  1. 9
      ArduPlane/altitude.cpp
  2. 10
      ArduPlane/quadplane.cpp
  3. 5
      ArduPlane/quadplane.h

9
ArduPlane/altitude.cpp

@ -126,7 +126,14 @@ float Plane::relative_ground_altitude(bool use_rangefinder_if_available) @@ -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;

10
ArduPlane/quadplane.cpp

@ -2795,7 +2795,7 @@ int8_t QuadPlane::forward_throttle_pct(void) @@ -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 @@ -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;
}

5
ArduPlane/quadplane.h

@ -540,6 +540,11 @@ private: @@ -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();

Loading…
Cancel
Save