From f4d349eff8e438b9a1bed547974570ba5ff77d70 Mon Sep 17 00:00:00 2001 From: Ben Nizette Date: Sun, 24 Nov 2013 20:18:33 +1100 Subject: [PATCH] SITL: Wire in random noise and fixed drift for the SITL barometer --- libraries/AP_HAL_AVR_SITL/sitl_barometer.cpp | 12 +++++++++--- 1 file changed, 9 insertions(+), 3 deletions(-) diff --git a/libraries/AP_HAL_AVR_SITL/sitl_barometer.cpp b/libraries/AP_HAL_AVR_SITL/sitl_barometer.cpp index d1d2920868..6e5410833c 100644 --- a/libraries/AP_HAL_AVR_SITL/sitl_barometer.cpp +++ b/libraries/AP_HAL_AVR_SITL/sitl_barometer.cpp @@ -31,18 +31,24 @@ void SITL_State::_update_barometer(float altitude) { static uint32_t last_update; + float sim_alt = altitude; + if (_barometer == NULL) { // this sketch doesn't use a barometer return; } // 80Hz, to match the real APM2 barometer - if (hal.scheduler->millis() - last_update < 12) { + uint32_t now = hal.scheduler->millis(); + if ((now - last_update) < 12) { return; } - last_update = hal.scheduler->millis(); + last_update = now; - _barometer->setHIL(altitude); + sim_alt += _sitl->baro_drift * now / 1000; + sim_alt += _sitl->baro_noise * _rand_float(); + + _barometer->setHIL(sim_alt); } #endif