ArduCopter: added AP_PerfMon library

libraries/AP_PerfMon/AP_PerfMon.cpp

+
+#include <math.h>
+#include <FastSerial.h>
+#include "Arduino.h"
+#include "AP_PerfMon.h"
+
+// static class variable definitions
+uint8_t AP_PerfMon::nextFuncNum;
+char AP_PerfMon::functionNames[PERFMON_MAX_FUNCTIONS][PERFMON_FUNCTION_NAME_LENGTH];
+uint32_t AP_PerfMon::time[PERFMON_MAX_FUNCTIONS];
+uint32_t AP_PerfMon::maxTime[PERFMON_MAX_FUNCTIONS];
+uint32_t AP_PerfMon::numCalls[PERFMON_MAX_FUNCTIONS];
+uint32_t AP_PerfMon::allStartTime;
+uint32_t AP_PerfMon::allEndTime;
+AP_PerfMon* AP_PerfMon::lastCreated = NULL;
+bool AP_PerfMon::_enabled = true;
+
+// constructor
+AP_PerfMon::AP_PerfMon(uint8_t funcNum) : _funcNum(funcNum), _time_this_iteration(0)
+{
+    // exit immediately if we are disabled
+    if( !_enabled ) {
+        return;
+    }
+
+    // check global start time
+    if( allStartTime == 0 ) {
+        allStartTime = micros();
+    }
+
+    // stop recording time from parent
+    _parent = lastCreated;  // add pointer to parent
+	if( _parent != NULL ) {
+        _parent->stop();
+    }
+
+    // record myself as the last created instance
+    lastCreated = this;
+
+    numCalls[_funcNum]++;   // record that this function has been called
+    start();  				// start recording time spent in this function
+}
+
+// destructor
+AP_PerfMon::~AP_PerfMon()
+{
+    // exit immediately if we are disabled
+    if( !_enabled ) {
+        return;
+    }
+
+    stop();   // stop recording time spent in this function
+    lastCreated = _parent;  // make my parent the last created instance
+
+    // calculate max time spent in this function
+    if( _time_this_iteration > maxTime[_funcNum] ) {
+        maxTime[_funcNum] = _time_this_iteration;
+    }
+
+    // restart recording time for parent
+    if( _parent != NULL )
+        _parent->start();
+}
+
+// record function name in static list
+uint8_t AP_PerfMon::recordFunctionName(const char funcName[])
+{
+    uint8_t nextNum = nextFuncNum++;
+    uint8_t i;
+
+    // clear existing function name (if any)
+    functionNames[nextNum][0] = 0;
+
+    // store function name
+    for( i=0; i<PERFMON_FUNCTION_NAME_LENGTH-1 && funcName[i] != 0; i++ ) {
+        functionNames[nextNum][i] = funcName[i];
+    }
+    functionNames[nextNum][i] = 0;
+
+    return nextNum;
+}
+
+// stop recording time
+void AP_PerfMon::start()
+{
+    _startTime = micros();  // start recording time spent in this function
+}
+
+// stop recording time
+void AP_PerfMon::stop()
+{
+    uint32_t temp_time = micros()-_startTime;
+    _time_this_iteration += temp_time;
+    time[_funcNum] += temp_time;
+}
+
+// ClearAll - clears all data from static members
+void AP_PerfMon::ClearAll()
+{
+    uint8_t i;
+    AP_PerfMon *p = lastCreated;
+
+    for(i=0; i<PERFMON_MAX_FUNCTIONS; i++) {
+        time[i] = 0;        // reset times
+        numCalls[i] = 0;    // reset num times called
+        maxTime[i] = 0;     // reset maximum time
+    }
+
+    // reset start time to now
+    allStartTime = micros();
+    allEndTime = 0;
+
+    // reset start times of any active counters
+    while( p != NULL ) {
+        p->_startTime = allStartTime;
+        p = p->_parent;
+    }
+}
+
+// DisplayResults - displays table of timing results
+void AP_PerfMon::DisplayResults()
+{
+    uint8_t i,j,changed;
+    float hz;
+    float pct;
+    uint32_t totalTime;
+    uint32_t avgTime;
+    uint32_t sumOfTime = 0;
+    uint32_t unExplainedTime;
+    uint8_t order[PERFMON_MAX_FUNCTIONS];
+    bool blocking_writes;
+
+    // record end time
+    if( allEndTime == 0 ) {
+        allEndTime = micros();
+    }
+
+    // turn off any time recording
+    if( lastCreated != NULL ) {
+        lastCreated->stop();
+    }
+    _enabled = false;
+
+    // reorder results
+    for(i=0; i<nextFuncNum; i++) {
+        order[i] = i;
+    }
+    changed=0;
+    do{
+        changed = 0;
+        for(i=0; i<nextFuncNum-1; i++)
+            if(time[order[i]]<time[order[i+1]]) {
+                j = order[i];
+                order[i] = order[i+1];
+                order[i+1] = j;
+                changed = 1;
+            }
+	}while(changed != 0);
+
+    // calculate time elapsed
+    totalTime = allEndTime - allStartTime;
+
+    // ensure serial is blocking
+    blocking_writes = Serial.get_blocking_writes();
+    Serial.set_blocking_writes(true);
+    Serial.printf_P(PSTR("blocking was:%d\n"),(int)blocking_writes);
+
+    // print table of results
+    Serial.printf_P(PSTR("\nPerfMon elapsed:%lu(ms)\n"),(unsigned long)totalTime/1000);
+    Serial.printf_P(PSTR("Fn:\t\tcpu\ttot(ms)\tavg(ms)\tmax(ms)\t#calls\tHz\n"));
+    for( i=0; i<nextFuncNum; i++ ) {
+        j=order[i];
+        sumOfTime += time[j];
+
+        // calculate average execution time
+        if( numCalls[j] > 0 ) {
+            avgTime = time[j] / numCalls[j];
+        }else{
+            avgTime = 0;
+        }
+
+        hz = numCalls[j]/(totalTime/1000000);
+        pct = ((float)time[j] / (float)totalTime) * 100.0;
+        Serial.printf_P(PSTR("%-10s\t%4.2f\t%lu\t%4.3f\t%4.3f\t%lu\t%4.1f\n"),
+            functionNames[j],
+            pct,
+            (unsigned long)time[j]/1000,
+            (float)avgTime/1000.0,
+            (float)maxTime[j]/1000.0,
+            numCalls[j],
+            hz);
+    }
+    // display unexplained time
+    if( sumOfTime >= totalTime ) {
+        unExplainedTime = 0;
+    } else {
+        unExplainedTime = totalTime - sumOfTime;
+    }
+    pct = ((float)unExplainedTime / (float)totalTime) * 100.0;
+    Serial.printf_P(PSTR("unexpl:\t\t%4.2f\t%lu\n"),pct,(unsigned long)unExplainedTime/1000);
+
+    // restore to blocking writes if necessary
+    Serial.set_blocking_writes(blocking_writes);
+
+    // turn back on any time recording
+    if( lastCreated != NULL ) {
+        lastCreated->start();
+    }
+    _enabled = true;
+}
+
+// DisplayAndClear - will display results after this many milliseconds.  should be called regularly
+void AP_PerfMon::DisplayAndClear(uint32_t display_after_seconds)
+{
+    if( (micros() - allStartTime) > (uint32_t)(display_after_seconds * 1000000) ) {
+        DisplayResults();
+        ClearAll();
+    }
+}
\ No newline at end of file

libraries/AP_PerfMon/AP_PerfMon.h

+#ifndef AP_PERFMON_H
+#define AP_PERFMON_H
+
+// macros to make integrating into code easier
+#define AP_PERFMON_REGISTER static uint8_t myFunc = AP_PerfMon::recordFunctionName(__func__); AP_PerfMon perfMon(myFunc);
+#define AP_PERFMON_REGISTER_NAME(functionName) static uint8_t myFunc = AP_PerfMon::recordFunctionName(functionName); AP_PerfMon perfMon(myFunc);
+
+#define PERFMON_MAX_FUNCTIONS 11
+#define PERFMON_FUNCTION_NAME_LENGTH 10
+
+#include <AP_Common.h>
+#include "HardwareSerial.h"
+
+class AP_PerfMon
+{
+    public:
+		// static methods
+        static uint8_t recordFunctionName(const char funcName[]);
+        static void DisplayResults();
+		static void ClearAll();
+        static void DisplayAndClear(uint32_t display_after_seconds);  // will display results after this many milliseconds.  should be called regularly
+
+		// public methods
+		AP_PerfMon(uint8_t funcNum);    // Constructor - records function start time
+		~AP_PerfMon();                  // Destructor - records function end time
+		void stop();                    // stops recording time spent in this function - meant to be called by a child.
+		void start();                   // restarts recording time spent in this function
+
+    private:
+        // static variables
+        static uint8_t nextFuncNum;
+		static char functionNames[PERFMON_MAX_FUNCTIONS][PERFMON_FUNCTION_NAME_LENGTH];
+        static uint32_t time[PERFMON_MAX_FUNCTIONS];
+		static uint32_t numCalls[PERFMON_MAX_FUNCTIONS];
+        static uint32_t maxTime[PERFMON_MAX_FUNCTIONS];
+		static uint32_t allStartTime;
+		static uint32_t allEndTime;
+		static AP_PerfMon* lastCreated;
+        static bool _enabled;
+
+        // instance variables
+        uint8_t _funcNum;
+        unsigned long _startTime;
+        unsigned long _time_this_iteration;
+		AP_PerfMon* _parent;
+};
+
+#endif  // AP_PERFMON_H
\ No newline at end of file

libraries/AP_PerfMon/AP_PerfMon_test/AP_PerfMon_test.pde

+/*
+  AP_PerfMon
+  Code by Randy Mackay
+*/
+
+#include <FastSerial.h>
+#include <AP_Common.h>
+#include <AP_PerfMon.h>        // PerfMonitor library
+
+FastSerialPort0(Serial);
+
+void setup()
+{
+  AP_PERFMON_REGISTER_NAME("setupA")
+  Serial.begin(115200);
+
+  Serial.println("Performance Monitor test v1.1");
+  // let tx buffer clear & display available space
+  delay(100);
+  Serial.printf_P(PSTR("Tx buf:%d available:%d\n"),Serial.txspace(),Serial.available());
+  delay(100);
+  Serial.set_blocking_writes(false);
+}
+
+void loop()
+{
+    AP_PERFMON_REGISTER
+
+    int16_t i = 0;
+
+    for( i=0; i<10; i++ ) {
+        testFn();
+    }
+
+    //AP_PerfMon::DisplayAndClear(5);
+    AP_PerfMon::DisplayResults();
+    AP_PerfMon::ClearAll();
+
+    delay(10000);
+}
+
+void testFn()
+{
+    AP_PERFMON_REGISTER
+    //delay(10);
+    //testFn2();
+    //delay(10);
+    Serial.printf_P(PSTR("1234567890"));
+}
+
+void testFn2()
+{
+    AP_PERFMON_REGISTER
+    delay(10);
+}