|
|
@ -133,11 +133,17 @@ bool Plane::suppress_throttle(void) |
|
|
|
// if we have an airspeed sensor, then check it too, and
|
|
|
|
// if we have an airspeed sensor, then check it too, and
|
|
|
|
// require 5m/s. This prevents throttle up due to spiky GPS
|
|
|
|
// require 5m/s. This prevents throttle up due to spiky GPS
|
|
|
|
// groundspeed with bad GPS reception
|
|
|
|
// groundspeed with bad GPS reception
|
|
|
|
|
|
|
|
#if AP_AIRSPEED_ENABLED |
|
|
|
if ((!ahrs.airspeed_sensor_enabled()) || airspeed.get_airspeed() >= 5) { |
|
|
|
if ((!ahrs.airspeed_sensor_enabled()) || airspeed.get_airspeed() >= 5) { |
|
|
|
// we're moving at more than 5 m/s
|
|
|
|
// we're moving at more than 5 m/s
|
|
|
|
throttle_suppressed = false; |
|
|
|
throttle_suppressed = false; |
|
|
|
return false;
|
|
|
|
return false;
|
|
|
|
} |
|
|
|
} |
|
|
|
|
|
|
|
#else |
|
|
|
|
|
|
|
// no airspeed sensor, so we trust that the GPS's movement is truthful
|
|
|
|
|
|
|
|
throttle_suppressed = false; |
|
|
|
|
|
|
|
return false; |
|
|
|
|
|
|
|
#endif |
|
|
|
} |
|
|
|
} |
|
|
|
|
|
|
|
|
|
|
|
#if HAL_QUADPLANE_ENABLED |
|
|
|
#if HAL_QUADPLANE_ENABLED |
|
|
|