From 81b519056bb8bb79e2bcfbdf0352f899eab06ba6 Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <andrew@tridgell.net>
Date: Mon, 5 Dec 2022 15:06:45 +1100
Subject: [PATCH] AP_AHRS: implement timing jitter in SITL

make the AHRS update take a random amount of extra CPU time
---
 libraries/AP_AHRS/AP_AHRS.cpp | 10 ++++++++++
 1 file changed, 10 insertions(+)

diff --git a/libraries/AP_AHRS/AP_AHRS.cpp b/libraries/AP_AHRS/AP_AHRS.cpp
index 8f21e7a6dd..9de7af6c29 100644
--- a/libraries/AP_AHRS/AP_AHRS.cpp
+++ b/libraries/AP_AHRS/AP_AHRS.cpp
@@ -420,6 +420,16 @@ void AP_AHRS::update(bool skip_ins_update)
         }
         GCS_SEND_TEXT(MAV_SEVERITY_INFO, "AHRS: %s active", shortname);
     }
+
+#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
+    /*
+      add timing jitter to simulate slow EKF response
+     */
+    const auto *sitl = AP::sitl();
+    if (sitl->loop_time_jitter_us > 0) {
+        hal.scheduler->delay_microseconds(random() % sitl->loop_time_jitter_us);
+    }
+#endif
 }
 
 /*