From 6713caba55a1c8f41d29ed52728f18963d0d1453 Mon Sep 17 00:00:00 2001
From: Randy Mackay <rmackay9@yahoo.com>
Date: Mon, 5 Dec 2022 12:55:07 +0900
Subject: [PATCH] AP_Arming: add system check of main loop rate

---
 libraries/AP_Arming/AP_Arming.cpp | 11 +++++++++++
 1 file changed, 11 insertions(+)

diff --git a/libraries/AP_Arming/AP_Arming.cpp b/libraries/AP_Arming/AP_Arming.cpp
index f6643b1896..ab8c3bf1cc 100644
--- a/libraries/AP_Arming/AP_Arming.cpp
+++ b/libraries/AP_Arming/AP_Arming.cpp
@@ -50,6 +50,7 @@
 #include <AP_Mount/AP_Mount.h>
 #include <AP_OpenDroneID/AP_OpenDroneID.h>
 #include <AP_SerialManager/AP_SerialManager.h>
+#include <AP_Scheduler/AP_Scheduler.h>
 
 #if HAL_MAX_CAN_PROTOCOL_DRIVERS
   #include <AP_CANManager/AP_CANManager.h>
@@ -960,6 +961,16 @@ bool AP_Arming::system_checks(bool report)
             check_failed(ARMING_CHECK_SYSTEM, report, "Param storage failed");
             return false;
         }
+
+        // check main loop rate is at least 90% of expected value
+        const float actual_loop_rate = AP::scheduler().get_filtered_loop_rate_hz();
+        const uint16_t expected_loop_rate = AP::scheduler().get_loop_rate_hz();
+        const float loop_rate_pct =  actual_loop_rate / expected_loop_rate;
+        if (loop_rate_pct < 0.90) {
+            check_failed(ARMING_CHECK_SYSTEM, report, "Main loop slow (%uHz < %uHz)", (unsigned)actual_loop_rate, (unsigned)expected_loop_rate);
+            return false;
+        }
+
 #if AP_TERRAIN_AVAILABLE
         const AP_Terrain *terrain = AP_Terrain::get_singleton();
         if ((terrain != nullptr) && terrain->init_failed()) {