From ccc5cfd466b213d09450531456e7c11182da74dc Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <andrew@tridgell.net>
Date: Mon, 26 Aug 2024 20:37:43 +1000
Subject: [PATCH] HAL: adjust CondMutex test

---
 .../AP_HAL/examples/CondMutex/CondMutex.cpp   | 32 +++++++++----------
 1 file changed, 15 insertions(+), 17 deletions(-)

diff --git a/libraries/AP_HAL/examples/CondMutex/CondMutex.cpp b/libraries/AP_HAL/examples/CondMutex/CondMutex.cpp
index 7c1341a35ef12..e31a335681dc4 100644
--- a/libraries/AP_HAL/examples/CondMutex/CondMutex.cpp
+++ b/libraries/AP_HAL/examples/CondMutex/CondMutex.cpp
@@ -14,7 +14,7 @@ void loop();
 
 const AP_HAL::HAL& hal = AP_HAL::get_HAL();
 
-#define USE_COND_MUTEX
+//#define USE_COND_MUTEX
 
 class ProducerConsumerTest {
 public:
@@ -27,6 +27,7 @@ class ProducerConsumerTest {
     void producer_tick(void);
     void consumer_tick(void);
     bool update();
+    void busy_wait(uint32_t us);
     void update_sent();
     bool check() { return bsize>0; }
 
@@ -39,6 +40,15 @@ class ProducerConsumerTest {
     uint32_t delay_count;
 };
 
+/*
+  simulate use of CPU processing a sample
+ */
+void ProducerConsumerTest::busy_wait(uint32_t us)
+{
+    const uint32_t start_us = AP_HAL::micros();
+    while (AP_HAL::micros() - start_us < us) ;
+}
+
 void ProducerConsumerTest::setup(void)
 {
     hal.scheduler->thread_create(
@@ -61,25 +71,13 @@ void ProducerConsumerTest::consumer_tick(void)
     cmtx.lock_and_wait(FUNCTOR_BIND_MEMBER(&ProducerConsumerTest::check, bool));
     update();
     cmtx.unlock();
+    busy_wait(100);
 #else
-    if (!check()) {
-        sem1.wait_blocking();
-    }
-    // we are avoiding wait_blocking() here to cope with double notifications
-    // however it also means that, pre-existing notifications will not
-    // be waited for, this means that we can exhaust the pending data
-    // and the wait_blocking() above will immediately return. This is why
-    // the availability of data must also be checked inside the lock
-    // it also means you have to go around the loop twice to get to a blocking state
-    // when going from some data to no data
-    if (!update()) {
-        // we thought we had a sample, but concurrency means we actually do not
-        // the pattern requires that we are able to exit early here without processing
-        // it should only ever happen two cycles at a time so is not a busy wait
-        return; 
+    sem1.wait_blocking();
+    while (update()) {
+        busy_wait(100);
     }
 #endif
-    hal.scheduler->delay_microseconds(100); // simluate processing delay
 }
 
 void ProducerConsumerTest::producer(void)