mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-03-13 10:03:57 -03:00
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:
parent
34b0967d09
commit
5c43b18672
@ -126,7 +126,14 @@ float Plane::relative_ground_altitude(bool use_rangefinder_if_available)
|
|||||||
{
|
{
|
||||||
if (use_rangefinder_if_available && rangefinder_state.in_range) {
|
if (use_rangefinder_if_available && rangefinder_state.in_range) {
|
||||||
return rangefinder_state.height_estimate;
|
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
|
#if AP_TERRAIN_AVAILABLE
|
||||||
float altitude;
|
float altitude;
|
||||||
|
@ -2795,7 +2795,7 @@ int8_t QuadPlane::forward_throttle_pct(void)
|
|||||||
// lidar could cause the aircraft not to be able to
|
// lidar could cause the aircraft not to be able to
|
||||||
// approach the landing point when landing below the takeoff point
|
// approach the landing point when landing below the takeoff point
|
||||||
vel_forward.last_pct = vel_forward.integrator;
|
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
|
// we're in the settling phase of landing, disable fwd motor
|
||||||
vel_forward.last_pct = 0;
|
vel_forward.last_pct = 0;
|
||||||
vel_forward.integrator = 0;
|
vel_forward.integrator = 0;
|
||||||
@ -3109,3 +3109,11 @@ bool QuadPlane::in_vtol_land_descent(void) const
|
|||||||
}
|
}
|
||||||
return false;
|
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;
|
||||||
|
}
|
||||||
|
@ -540,6 +540,11 @@ private:
|
|||||||
are we in the descent phase of a VTOL landing?
|
are we in the descent phase of a VTOL landing?
|
||||||
*/
|
*/
|
||||||
bool in_vtol_land_descent(void) const;
|
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:
|
public:
|
||||||
void motor_test_output();
|
void motor_test_output();
|
||||||
|
Loading…
Reference in New Issue
Block a user