Copter: add main loop performance warning

libraries/AP_Arming/AP_Arming.cpp

@@ -51,6 +51,7 @@
 #include <AP_OpenDroneID/AP_OpenDroneID.h>
 #include <AP_SerialManager/AP_SerialManager.h>
 #include <AP_Vehicle/AP_Vehicle_Type.h>
+#include <AP_Scheduler/AP_Scheduler.h>
 
 #if HAL_MAX_CAN_PROTOCOL_DRIVERS
   #include <AP_CANManager/AP_CANManager.h>
@@ -966,6 +967,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()) {

libraries/AP_Logger/LogStructure.h

@@ -537,6 +537,7 @@ struct PACKED log_Rally {
 struct PACKED log_Performance {
     LOG_PACKET_HEADER;
     uint64_t time_us;
+    uint16_t loop_rate;
     uint16_t num_long_running;
     uint16_t num_loops;
     uint32_t max_time;
@@ -943,8 +944,9 @@ struct PACKED log_VER {
 // @LoggerMessage: PM
 // @Description: autopilot system performance and general data dumping ground
 // @Field: TimeUS: Time since system startup
+// @Field: LR: Main loop rate
 // @Field: NLon: Number of long loops detected
-// @Field: NLoop: Number of measurement loops for this message
+// @Field: NL: Number of measurement loops for this message
 // @Field: MaxT: Maximum loop time
 // @Field: Mem: Free memory available
 // @Field: Load: System processor load
@@ -1258,7 +1260,7 @@ LOG_STRUCTURE_FROM_CAMERA \
     LOG_STRUCTURE_FROM_BEACON                                       \
     LOG_STRUCTURE_FROM_PROXIMITY                                    \
     { LOG_PERFORMANCE_MSG, sizeof(log_Performance),                     \
-      "PM",  "QHHIIHHIIIIII", "TimeUS,NLon,NLoop,MaxT,Mem,Load,ErrL,IntE,ErrC,SPIC,I2CC,I2CI,Ex", "s---b%------s", "F---0A------F" }, \
+      "PM",  "QHHHIIHHIIIIII", "TimeUS,LR,NLon,NL,MaxT,Mem,Load,ErrL,IntE,ErrC,SPIC,I2CC,I2CI,Ex", "sz---b%------s", "F----0A------F" }, \
     { LOG_SRTL_MSG, sizeof(log_SRTL), \
       "SRTL", "QBHHBfff", "TimeUS,Active,NumPts,MaxPts,Action,N,E,D", "s----mmm", "F----000" }, \
 LOG_STRUCTURE_FROM_AVOIDANCE \

libraries/AP_Scheduler/AP_Scheduler.cpp

@@ -308,6 +308,10 @@ uint16_t AP_Scheduler::time_available_usec(void) const
  */
 float AP_Scheduler::load_average()
 {
+    // return 1 if filtered main loop rate is 5% below the configured rate
+    if (get_filtered_loop_rate_hz() < get_loop_rate_hz() * 0.95) {
+        return 1.0;
+    }
     if (_spare_ticks == 0) {
         return 0.0f;
     }
@@ -426,6 +430,7 @@ void AP_Scheduler::Log_Write_Performance()
     struct log_Performance pkt = {
         LOG_PACKET_HEADER_INIT(LOG_PERFORMANCE_MSG),
         time_us          : AP_HAL::micros64(),
+        loop_rate        : (uint16_t)(get_filtered_loop_rate_hz() + 0.5f),
         num_long_running : perf_info.get_num_long_running(),
         num_loops        : perf_info.get_num_loops(),
         max_time         : perf_info.get_max_time(),

libraries/AP_Scheduler/AP_Scheduler.h

@@ -162,10 +162,16 @@ class AP_Scheduler
         return _loop_period_s;
     }
 
+    // get the filtered main loop time in seconds
     float get_filtered_loop_time(void) const {
         return perf_info.get_filtered_time();
     }
 
+    // get the filtered active main loop rate
+    float get_filtered_loop_rate_hz() {
+        return perf_info.get_filtered_loop_rate_hz();
+    }
+
     // get the time in seconds that the last loop took
     float get_last_loop_time_s(void) const {
         return _last_loop_time_s;

libraries/AP_Scheduler/PerfInfo.cpp

@@ -183,6 +183,17 @@ float AP::PerfInfo::get_filtered_time() const
     return filtered_loop_time;
 }
 
+// return low pass filtered loop rate in hz
+float AP::PerfInfo::get_filtered_loop_rate_hz() const
+{
+    const float filt_time_s = get_filtered_time();
+    if (filt_time_s <= 0) {
+        return loop_rate_hz;
+    }
+    return 1.0 / filt_time_s;
+}
+
+
 void AP::PerfInfo::update_logging() const
 {
     gcs().send_text(MAV_SEVERITY_INFO,
@@ -191,7 +202,7 @@ void AP::PerfInfo::update_logging() const
                     (unsigned)get_num_loops(),
                     (unsigned long)get_max_time(),
                     (unsigned long)get_min_time(),
-                    (unsigned)(0.5+(1.0f/get_filtered_time())),
+                    (unsigned)(0.5+get_filtered_loop_rate_hz()),
                     (unsigned long)get_stddev_time(),
                     (unsigned long)AP::scheduler().get_extra_loop_us());
 }

libraries/AP_Scheduler/PerfInfo.h

@@ -35,6 +35,7 @@ class PerfInfo {
     uint32_t get_avg_time() const;
     uint32_t get_stddev_time() const;
     float    get_filtered_time() const;
+    float get_filtered_loop_rate_hz() const;
     void set_loop_rate(uint16_t rate_hz);
 
     void update_logging() const;