From 527e621a576404d3b85038297b18ce18f5f0c857 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Mon, 29 Apr 2024 14:30:27 +1000 Subject: [PATCH] AP_Baro: accumulate altitude drift in place of raw calc currently we set drift based on current boot time. If you want to introduce it at some stage in a flight the baro alt will suddenly jump based on how long you've been up. Instead, accumulate a delta --- libraries/AP_Baro/AP_Baro_SITL.cpp | 6 +++++- libraries/AP_Baro/AP_Baro_SITL.h | 2 ++ 2 files changed, 7 insertions(+), 1 deletion(-) 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