mirror of https://github.com/ArduPilot/ardupilot
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
|
@ -128,6 +128,13 @@ float Plane::relative_ground_altitude(bool use_rangefinder_if_available)
|
|||
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;
|
||||
if (target_altitude.terrain_following &&
|
||||
|
|
|
@ -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;
|
||||
}
|
||||
|
|
|
@ -541,6 +541,11 @@ private:
|
|||
*/
|
||||
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();
|
||||
MAV_RESULT mavlink_motor_test_start(mavlink_channel_t chan, uint8_t motor_seq, uint8_t throttle_type,
|
||||
|
|
Loading…
Reference in New Issue