Measure accuracy for the waiting at the thread priority you run at

Author: TrentHouliston

src/extension/ChronoController.cpp

@@ -41,24 +41,27 @@ namespace extension {
         : Reactor(std::move(environment)) {
 
         // Estimate the accuracy of our cv wait and precise sleep
-        for (int i = 0; i < 3; ++i) {
-            // Estimate the accuracy of our cv wait
-            std::mutex test;
-            std::unique_lock<std::mutex> lock(test);
-            const auto cv_s = NUClear::clock::now();
-            wait.wait_for(lock, std::chrono::milliseconds(1));
-            const auto cv_e = NUClear::clock::now();
-            const auto cv_a = NUClear::clock::duration(cv_e - cv_s - std::chrono::milliseconds(1));
-
-            // Estimate the accuracy of our precise sleep
-            const auto ns_s = NUClear::clock::now();
-            util::precise_sleep(std::chrono::milliseconds(1));
-            const auto ns_e = NUClear::clock::now();
-            const auto ns_a = NUClear::clock::duration(ns_e - ns_s - std::chrono::milliseconds(1));
-
-            // Use the largest time we have seen
-            cv_accuracy = cv_a > cv_accuracy ? cv_a : cv_accuracy;
-            ns_accuracy = ns_a > ns_accuracy ? ns_a : ns_accuracy;
+        {
+            const util::ThreadPriority priority_lock(PriorityLevel::HIGHEST);
+            for (int i = 0; i < 3; ++i) {
+                // Estimate the accuracy of our cv wait
+                std::mutex test;
+                std::unique_lock<std::mutex> lock(test);
+                const auto cv_s = NUClear::clock::now();
+                wait.wait_for(lock, std::chrono::milliseconds(1));
+                const auto cv_e = NUClear::clock::now();
+                const auto cv_a = NUClear::clock::duration(cv_e - cv_s - std::chrono::milliseconds(1));
+
+                // Estimate the accuracy of our precise sleep
+                const auto ns_s = NUClear::clock::now();
+                util::precise_sleep(std::chrono::milliseconds(1));
+                const auto ns_e = NUClear::clock::now();
+                const auto ns_a = NUClear::clock::duration(ns_e - ns_s - std::chrono::milliseconds(1));
+
+                // Use the largest time we have seen
+                cv_accuracy = cv_a > cv_accuracy ? cv_a : cv_accuracy;
+                ns_accuracy = ns_a > ns_accuracy ? ns_a : ns_accuracy;
+            }
         }
 
         on<Trigger<ChronoTask>>().then("Add Chrono task", [this](const std::shared_ptr<const ChronoTask>& task) {