From 09500df4a85c751c4eaadef59128065ce2d475e2 Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <andrew@tridgell.net>
Date: Fri, 23 Feb 2024 11:59:56 +1100
Subject: [PATCH] AP_Scheduler: fixed example test to pass/fail

---
 .../Scheduler_test/Scheduler_test.cpp         | 29 ++++++++++++++++---
 1 file changed, 25 insertions(+), 4 deletions(-)

diff --git a/libraries/AP_Scheduler/examples/Scheduler_test/Scheduler_test.cpp b/libraries/AP_Scheduler/examples/Scheduler_test/Scheduler_test.cpp
index 5a9eb79d20..b0cc45e2fb 100644
--- a/libraries/AP_Scheduler/examples/Scheduler_test/Scheduler_test.cpp
+++ b/libraries/AP_Scheduler/examples/Scheduler_test/Scheduler_test.cpp
@@ -9,6 +9,7 @@
 #include <AP_BoardConfig/AP_BoardConfig.h>
 #include <AP_Logger/AP_Logger.h>
 #include <GCS_MAVLink/GCS_Dummy.h>
+#include <stdio.h>
 
 const struct AP_Param::GroupInfo        GCS_MAVLINK_Parameters::var_info[] = {
     AP_GROUPEND
@@ -26,13 +27,14 @@ public:
 
 private:
 
-    AP_InertialSensor ins;
 #if HAL_EXTERNAL_AHRS_ENABLED
     AP_ExternalAHRS eAHRS;
 #endif // HAL_EXTERNAL_AHRS_ENABLED
     AP_Scheduler scheduler;
 
     uint32_t ins_counter;
+    uint32_t count_5s;
+    uint32_t count_1s;
     static const AP_Scheduler::Task scheduler_tasks[];
 
     void ins_update(void);
@@ -81,8 +83,6 @@ void SchedTest::setup(void)
 
     board_config.init();
 
-    ins.init(scheduler.get_loop_rate_hz());
-
     // initialise the scheduler
     scheduler.init(&scheduler_tasks[0], ARRAY_SIZE(scheduler_tasks), (uint32_t)-1);
 }
@@ -91,6 +91,24 @@ void SchedTest::loop(void)
 {
     // run all tasks
     scheduler.loop();
+    if (ins_counter == 1000) {
+        bool ok = true;
+        if (count_5s != 4) {
+            ::printf("ERROR: count_5s=%u\n", (unsigned)count_5s);
+            ok = false;
+        }
+        if (count_1s != 20) {
+            ::printf("ERROR: count_1s=%u\n", (unsigned)count_1s);
+            ok = false;
+        }
+        if (!ok) {
+            ::printf("Test FAILED\n");
+            exit(1);
+        } else {
+            ::printf("Test PASSED\n");
+            exit(0);
+        }
+    }
 }
 
 /*
@@ -99,7 +117,6 @@ void SchedTest::loop(void)
 void SchedTest::ins_update(void)
 {
     ins_counter++;
-    ins.update();
 }
 
 /*
@@ -108,6 +125,7 @@ void SchedTest::ins_update(void)
 void SchedTest::one_hz_print(void)
 {
     hal.console->printf("one_hz: t=%lu\n", (unsigned long)AP_HAL::millis());
+    count_1s++;
 }
 
 /*
@@ -116,6 +134,7 @@ void SchedTest::one_hz_print(void)
 void SchedTest::five_second_call(void)
 {
     hal.console->printf("five_seconds: t=%lu ins_counter=%u\n", (unsigned long)AP_HAL::millis(), (unsigned)ins_counter);
+    count_5s++;
 }
 
 /*
@@ -128,8 +147,10 @@ void setup(void)
 {
     schedtest.setup();
 }
+
 void loop(void)
 {
     schedtest.loop();
 }
+
 AP_HAL_MAIN();