Merge pull request #71 from CU-Robotics/feature-profiler

Feature profiler

Author: Pandabear1125

src/main.cpp

@@ -3,6 +3,7 @@
 #include "git_info.h"
 
 #include "utils/timing.hpp"
+#include "utils/profiler.hpp"
 #include "comms/rm_can.hpp"
 #include "sensors/dr16.hpp"
 #include "controls/estimator_manager.hpp"
@@ -34,6 +35,8 @@ ACS712 current_sensor;
 ConfigLayer config_layer;
 Config config;
 
+Profiler prof;
+
 Timer loop_timer;
 Timer stall_timer;
 Timer control_input_timer;

src/utils/profiler.cpp

@@ -0,0 +1,70 @@
+#include "profiler.hpp"
+
+void Profiler::begin(const char* name) {
+#ifdef PROFILE
+    // search for a section by name or an empty slot
+    for (uint32_t i = 0; i < PROF_MAX_SECTIONS; i++) {
+        if (strncmp(sections[i].name, name, PROF_MAX_NAME) == 0 || (sections[i].count == 0 && sections[i].overflow == 0)) {
+            // start this section
+            strncpy(sections[i].name, name, PROF_MAX_NAME);
+            sections[i].name[PROF_MAX_NAME] = '\0';  // ensure null termination
+            if (sections[i].count < PROF_MAX_TIMES) {
+                sections[i].start_times[sections[i].count] = micros();
+                sections[i].started = 1;    // set this section as "started"
+            }
+            return;
+        }
+    }
+#endif
+}
+
+void Profiler::end(const char* name) {
+#ifdef PROFILE
+    // find the section by name and add an end time
+    for (uint32_t i = 0; i < PROF_MAX_SECTIONS; i++) {
+        if (strncmp(sections[i].name, name, PROF_MAX_NAME) == 0) {
+            uint32_t count = sections[i].count;
+            if (count < PROF_MAX_TIMES) {
+                sections[i].end_times[count] = micros();
+                sections[i].count++;
+                sections[i].started = 0;    // set this section as "finished"
+                // if this value just overflowed, or it has reached the max time count, 
+                // mark it as overflowed and reset to 0
+                if (sections[i].count == 0 || sections[i].count == PROF_MAX_TIMES) {
+                    sections[i].overflow = 1;
+                    sections[i].count = 0;
+                }
+            }
+            return;
+        }
+    }
+#endif
+}
+
+void Profiler::print(const char* name) {
+#ifdef PROFILE
+    uint32_t min = UINT32_MAX;
+    uint32_t max = 0;
+    uint32_t sum = 0;
+
+    for (uint32_t i = 0; i < PROF_MAX_SECTIONS; i++) {
+        if (strncmp(sections[i].name, name, PROF_MAX_NAME) != 0) continue;
+        // find actual count since this value can overflow/max out
+        uint32_t actual_count = sections[i].overflow ? PROF_MAX_TIMES : sections[i].count;
+        for (unsigned int j = 0; j < actual_count; j++) {
+            // if end() was not called when we decide to print, ignore the "bad" section reading
+            if (sections[i].started && j == sections[i].count) continue;
+
+            uint32_t delta = sections[i].end_times[j] - sections[i].start_times[j];
+            // sum all deltas to be averaged upon printing
+            sum += delta;
+            if (delta < min) min = delta;
+            if (delta > max) max = delta;
+        }
+        // print stats
+        Serial.printf("Profiling for: %s\n  Min: %u us\n  Max: %u us\n  Avg: %u us\n", 
+                        name, min, max, sum / actual_count);
+        return;
+    }
+#endif
+}

src/utils/profiler.hpp

@@ -0,0 +1,51 @@
+#ifndef PROFILER_H
+#define PROFILER_H
+
+// Use this flag to toggle profiling globally.
+#define PROFILE
+
+#include <Arduino.h>
+
+#define PROF_MAX_SECTIONS 4  // max number of active profiling sections
+#define PROF_MAX_NAME 16      // max length of section name
+#define PROF_MAX_TIMES (1 << 20)   // max number of start/end times per section
+
+/// @brief Object for profiling sections of code.
+struct Profiler {
+    /// @brief Data structure for a profiling section. 
+    struct profiler_section_t {
+        /// @brief Start time for each profiling section.
+        uint32_t start_times[PROF_MAX_TIMES] = { 0 };
+        /// @brief End time for each profiling section.
+        uint32_t end_times[PROF_MAX_TIMES] = { 0 };
+        /// @brief Number of start/end times recorded (how many deltas can be calculated).
+        uint16_t count = 0;
+        /// @brief Flag on whether count has overflowed or not
+        uint8_t overflow : 7;
+        /// @brief Flag on whether this section had begin() called on it
+        uint8_t started : 1;
+        /// @brief A unique name to identify the section.
+        char name[PROF_MAX_NAME + 1] = { 0 };  // extra for null terminator
+    };
+
+    // dont allocate this memory if we dont need it
+#ifdef PROFILE
+    /// @brief Array of profiling sections.
+    profiler_section_t sections[PROF_MAX_SECTIONS] = {};
+#endif
+    /// @brief Start a profiling section.
+    /// @param name A unique name to identify the section.
+    void begin(const char* name);
+
+    /// @brief End a profiling section.
+    /// @param name The name of the section to end.
+    void end(const char* name);
+
+    /// @brief Print stats for a particular profiling section.
+    /// @param name The name of the section to print statistics for.
+    void print(const char* name);
+};
+
+extern PROGMEM Profiler prof;  // Global profiler
+
+#endif  // PROFILER_H