SITL: prevent wind effects at negative altitudes

this prevents crashes on takeoff with barometer noise
This commit is contained in:
Andrew Tridgell 2014-04-09 07:06:39 +10:00
parent e7d73aa856
commit a15e4633b7

View File

@ -495,6 +495,9 @@ void SITL_State::_simulator_output(void)
// setup wind control
float wind_speed = _sitl->wind_speed * 100;
float altitude = _barometer?_barometer->get_altitude():0;
if (altitude < 0) {
altitude = 0;
}
if (altitude < 60) {
wind_speed *= altitude / 60.0f;
}