From ced4cce35801c6a2d3bf2229d16b39ef8290fea3 Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <andrew@tridgell.net>
Date: Thu, 21 Apr 2016 14:50:15 +1000
Subject: [PATCH] AP_Scheduler: added optional perf counters at SCHED_DEBUG >=
 4

---
 libraries/AP_Scheduler/AP_Scheduler.cpp | 40 +++++++++++++++++--------
 libraries/AP_Scheduler/AP_Scheduler.h   |  4 +++
 2 files changed, 32 insertions(+), 12 deletions(-)

diff --git a/libraries/AP_Scheduler/AP_Scheduler.cpp b/libraries/AP_Scheduler/AP_Scheduler.cpp
index 67e9bd9eea..a2a950a5d1 100644
--- a/libraries/AP_Scheduler/AP_Scheduler.cpp
+++ b/libraries/AP_Scheduler/AP_Scheduler.cpp
@@ -24,6 +24,7 @@
 #include <AP_HAL/AP_HAL.h>
 #include <AP_Param/AP_Param.h>
 #include <AP_Vehicle/AP_Vehicle.h>
+#include <stdio.h>
 
 #if APM_BUILD_TYPE(APM_BUILD_ArduCopter)
 #define SCHEDULER_DEFAULT_LOOP_RATE 400
@@ -99,6 +100,15 @@ void AP_Scheduler::run(uint16_t time_available)
     uint32_t run_started_usec = AP_HAL::micros();
     uint32_t now = run_started_usec;
 
+    if (_debug > 3 && _perf_counters == nullptr) {
+        _perf_counters = new AP_HAL::Util::perf_counter_t[_num_tasks];
+        if (_perf_counters != nullptr) {
+            for (uint8_t i=0; i<_num_tasks; i++) {
+                _perf_counters[i] = hal.util->perf_alloc(AP_HAL::Util::PC_ELAPSED, _tasks[i].name);
+            }
+        }
+    }
+    
     for (uint8_t i=0; i<_num_tasks; i++) {
         uint16_t dt = _tick_counter - _last_run[i];
         uint16_t interval_ticks = _loop_rate_hz / _tasks[i].rate_hz;
@@ -112,12 +122,12 @@ void AP_Scheduler::run(uint16_t time_available)
             if (dt >= interval_ticks*2) {
                 // we've slipped a whole run of this task!
                 if (_debug > 1) {
-                    hal.console->printf("Scheduler slip task[%u-%s] (%u/%u/%u)\n",
-                                          (unsigned)i,
-                                          _tasks[i].name,
-                                          (unsigned)dt,
-                                          (unsigned)interval_ticks,
-                                          (unsigned)_task_time_allowed);
+                    ::printf("Scheduler slip task[%u-%s] (%u/%u/%u)\n",
+                             (unsigned)i,
+                             _tasks[i].name,
+                             (unsigned)dt,
+                             (unsigned)interval_ticks,
+                             (unsigned)_task_time_allowed);
                 }
             }
 
@@ -125,7 +135,13 @@ void AP_Scheduler::run(uint16_t time_available)
                 // run it
                 _task_time_started = now;
                 current_task = i;
+                if (_debug > 3 && _perf_counters && _perf_counters[i]) {
+                    hal.util->perf_begin(_perf_counters[i]);
+                }
                 _tasks[i].function();
+                if (_debug > 3 && _perf_counters && _perf_counters[i]) {
+                    hal.util->perf_end(_perf_counters[i]);
+                }
                 current_task = -1;
 
                 // record the tick counter when we ran. This drives
@@ -138,12 +154,12 @@ void AP_Scheduler::run(uint16_t time_available)
 
                 if (time_taken > _task_time_allowed) {
                     // the event overran!
-                    if (_debug > 2) {
-                        hal.console->printf("Scheduler overrun task[%u-%s] (%u/%u)\n",
-                                              (unsigned)i,
-                                              _tasks[i].name,
-                                              (unsigned)time_taken,
-                                              (unsigned)_task_time_allowed);
+                    if (_debug > 4) {
+                        ::printf("Scheduler overrun task[%u-%s] (%u/%u)\n",
+                                 (unsigned)i,
+                                 _tasks[i].name,
+                                 (unsigned)time_taken,
+                                 (unsigned)_task_time_allowed);
                     }
                 }
                 if (time_taken >= time_available) {
diff --git a/libraries/AP_Scheduler/AP_Scheduler.h b/libraries/AP_Scheduler/AP_Scheduler.h
index 25c3eca510..ff7e0fc643 100644
--- a/libraries/AP_Scheduler/AP_Scheduler.h
+++ b/libraries/AP_Scheduler/AP_Scheduler.h
@@ -22,6 +22,7 @@
 #pragma once
 
 #include <AP_Param/AP_Param.h>
+#include <AP_HAL/Util.h>
 
 #define AP_SCHEDULER_NAME_INITIALIZER(_name) .name = #_name,
 
@@ -126,4 +127,7 @@ private:
 
     // number of ticks that _spare_micros is counted over
     uint8_t _spare_ticks;
+
+    // performance counters
+    AP_HAL::Util::perf_counter_t *_perf_counters;
 };