AP_Scheduler: add get_filtered_loop_rate_hz

libraries/AP_Scheduler/AP_Scheduler.cpp

@@ -426,6 +426,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

@@ -163,10 +163,16 @@ public:
         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

@@ -36,6 +36,7 @@ public:
     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;