diff --git a/libraries/AP_Baro/AP_Baro_SITL.cpp b/libraries/AP_Baro/AP_Baro_SITL.cpp index 9b41e33645..fc26899485 100644 --- a/libraries/AP_Baro/AP_Baro_SITL.cpp +++ b/libraries/AP_Baro/AP_Baro_SITL.cpp @@ -66,7 +66,11 @@ void AP_Baro_SITL::_timer() return; } - sim_alt += _sitl->baro[_instance].drift * now * 0.001f; + const auto drift_delta_t_ms = now - last_drift_delta_t_ms; + last_drift_delta_t_ms = now; + total_alt_drift += _sitl->baro[_instance].drift * drift_delta_t_ms * 0.001f; + + sim_alt += total_alt_drift; sim_alt += _sitl->baro[_instance].noise * rand_float(); // add baro glitch diff --git a/libraries/AP_Baro/AP_Baro_SITL.h b/libraries/AP_Baro/AP_Baro_SITL.h index 68ba04d016..7d724aeaeb 100644 --- a/libraries/AP_Baro/AP_Baro_SITL.h +++ b/libraries/AP_Baro/AP_Baro_SITL.h @@ -48,5 +48,7 @@ private: float _recent_press; float _last_altitude; + uint32_t last_drift_delta_t_ms; // allows for integration of drift over time + float total_alt_drift; // integrated altitude drift in metres }; #endif // AP_SIM_BARO_ENABLED