diff --git a/libraries/AP_Scheduler/AP_Scheduler.cpp b/libraries/AP_Scheduler/AP_Scheduler.cpp index 65260b80cc..6a8d34e5e6 100644 --- a/libraries/AP_Scheduler/AP_Scheduler.cpp +++ b/libraries/AP_Scheduler/AP_Scheduler.cpp @@ -240,3 +240,10 @@ void AP_Scheduler::loop() #endif perf_info.check_loop_time(AP_HAL::micros() - loop_start); } + +void AP_Scheduler::update_logging() +{ + if (debug()) { + perf_info.update_logging(); + } +} diff --git a/libraries/AP_Scheduler/AP_Scheduler.h b/libraries/AP_Scheduler/AP_Scheduler.h index 82f722e9a6..34b9bf2c75 100644 --- a/libraries/AP_Scheduler/AP_Scheduler.h +++ b/libraries/AP_Scheduler/AP_Scheduler.h @@ -78,6 +78,9 @@ public: // that function does void loop(); + // call to update any logging the scheduler might do; call at 1Hz + void update_logging(); + // call when one tick has passed void tick(void); diff --git a/libraries/AP_Scheduler/PerfInfo.cpp b/libraries/AP_Scheduler/PerfInfo.cpp index 2b6c8e626e..7b77ae256b 100644 --- a/libraries/AP_Scheduler/PerfInfo.cpp +++ b/libraries/AP_Scheduler/PerfInfo.cpp @@ -1,6 +1,7 @@ #include "PerfInfo.h" #include +#include // // high level performance monitoring @@ -93,3 +94,15 @@ uint32_t AP::PerfInfo::get_stddev_time() const { return sqrt((sigmasquared_time - (sigma_time*sigma_time)/loop_count) / loop_count); } + +void AP::PerfInfo::update_logging() +{ + gcs().send_text(MAV_SEVERITY_WARNING, + "PERF: %u/%u max=%lu min=%lu avg=%lu sd=%lu", + (unsigned)get_num_long_running(), + (unsigned)get_num_loops(), + (unsigned long)get_max_time(), + (unsigned long)get_min_time(), + (unsigned long)get_avg_time(), + (unsigned long)get_stddev_time()); +} diff --git a/libraries/AP_Scheduler/PerfInfo.h b/libraries/AP_Scheduler/PerfInfo.h index c1984f86d1..d2f728f404 100644 --- a/libraries/AP_Scheduler/PerfInfo.h +++ b/libraries/AP_Scheduler/PerfInfo.h @@ -28,6 +28,8 @@ public: overtime_threshold_micros = 1000000/rate_hz * 1.2f; } + void update_logging(); + private: uint16_t overtime_threshold_micros; uint16_t loop_count;