AP_Scheduler: fixed example test to pass/fail

ArduPilot · Feb 23, 2024 · 9a0d2a6 · 9a0d2a6
1 parent c0914f4
commit 9a0d2a6
Showing 1 changed file with 24 additions and 4 deletions.
diff --git a/libraries/AP_Scheduler/examples/Scheduler_test/Scheduler_test.cpp b/libraries/AP_Scheduler/examples/Scheduler_test/Scheduler_test.cpp
@@ -26,13 +26,14 @@ class SchedTest {
 
 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 +82,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 +90,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 +116,6 @@ void SchedTest::loop(void)
 void SchedTest::ins_update(void)
 {
     ins_counter++;
-    ins.update();
 }
 
 /*
@@ -108,6 +124,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 +133,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 +146,10 @@ void setup(void)
 {
     schedtest.setup();
 }
+
 void loop(void)
 {
     schedtest.loop();
 }
+
 AP_HAL_MAIN();