diff --git a/libraries/AP_Scheduler/AP_Scheduler.cpp b/libraries/AP_Scheduler/AP_Scheduler.cpp index aa8c62ed1a..ddeecf84c4 100644 --- a/libraries/AP_Scheduler/AP_Scheduler.cpp +++ b/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(), diff --git a/libraries/AP_Scheduler/AP_Scheduler.h b/libraries/AP_Scheduler/AP_Scheduler.h index 167a29ff81..f24e2340e2 100644 --- a/libraries/AP_Scheduler/AP_Scheduler.h +++ b/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; diff --git a/libraries/AP_Scheduler/PerfInfo.cpp b/libraries/AP_Scheduler/PerfInfo.cpp index b68c708c34..77962687b6 100644 --- a/libraries/AP_Scheduler/PerfInfo.cpp +++ b/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()); } diff --git a/libraries/AP_Scheduler/PerfInfo.h b/libraries/AP_Scheduler/PerfInfo.h index cdbf1d6cf1..d8c08db6c2 100644 --- a/libraries/AP_Scheduler/PerfInfo.h +++ b/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;