AP_Scheduler: added optional perf counters at SCHED_DEBUG >= 4

hughhugh · Apr 21, 2016 · ced4cce · ced4cce
1 parent a7006a7
commit ced4cce
Show file tree

Hide file tree

Showing 2 changed files with 32 additions and 12 deletions.
diff --git 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,20 +122,26 @@ 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);
                 }
             }
 
             if (_task_time_allowed <= 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
@@ -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 @@ class AP_Scheduler
 
     // number of ticks that _spare_micros is counted over
     uint8_t _spare_ticks;
+
+    // performance counters
+    AP_HAL::Util::perf_counter_t *_perf_counters;
 };