OP-1365 Flight code instrumentation API: PiOS Implementation

flight/pios/common/pios_instrumentation.c

@@ -0,0 +1,73 @@
+/**
+ ******************************************************************************
+ *
+ * @file       pios_instrumentation.c
+ * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2014.
+ * @brief      PiOS instrumentation infrastructure
+ *             Allow to collects performance indexes and execution times
+ * @see        The GNU Public License (GPL) Version 3
+ *
+ *****************************************************************************/
+/*
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
+ * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
+ * for more details.
+ *
+ * You should have received a copy of the GNU General Public License along
+ * with this program; if not, write to the Free Software Foundation, Inc.,
+ * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
+ */
+
+#include <pios_instrumentation.h>
+
+pios_perf_counter_t *pios_instrumentation_perf_counters = NULL;
+int8_t pios_instrumentation_max_counters = -1;
+int8_t pios_instrumentation_last_used_counter = -1;
+
+void PIOS_Instrumentation_Init(int8_t maxCounters)
+{
+    PIOS_Assert(maxCounters >= 0);
+    if (maxCounters > 0) {
+        pios_instrumentation_perf_counters = (pios_perf_counter_t *)pvPortMalloc(sizeof(pios_perf_counter_t) * maxCounters);
+        PIOS_Assert(pios_instrumentation_perf_counters);
+        memset(pios_instrumentation_perf_counters, 0, sizeof(pios_perf_counter_t) * maxCounters);
+        pios_instrumentation_max_counters  = maxCounters;
+    } else {
+        pios_instrumentation_perf_counters = NULL;
+        pios_instrumentation_max_counters  = -1;
+    }
+}
+
+int8_t PIOS_Instrumentation_CreateCounter(uint32_t id)
+{
+    PIOS_Assert(pios_instrumentation_perf_counters && (pios_instrumentation_max_counters > pios_instrumentation_last_used_counter));
+    int8_t idx = ++pios_instrumentation_last_used_counter;
+    pios_instrumentation_perf_counters[idx].id = id;
+    return idx;
+}
+
+int8_t PIOS_Instrumentation_SearchCounter(uint32_t id)
+{
+    PIOS_Assert(pios_instrumentation_perf_counters);
+    uint8_t i = 0;
+    while (i < pios_instrumentation_last_used_counter && pios_instrumentation_perf_counters[i].id != id) {}
+    if (pios_instrumentation_perf_counters[i].id != id) {
+        return -1;
+    }
+    return i;
+}
+
+void PIOS_Instrumentation_ForEachCounter(InstrumentationCounterCallback callback, void *context)
+{
+    PIOS_Assert(pios_instrumentation_perf_counters);
+    for (int8_t index = 0; index < pios_instrumentation_last_used_counter + 1; index++) {
+        const pios_perf_counter_t *counter = &pios_instrumentation_perf_counters[index];
+        callback(counter, index, context);
+    }
+}

flight/pios/inc/pios_instrumentation.h

@@ -0,0 +1,124 @@
+/**
+ ******************************************************************************
+ *
+ * @file       pios_instrumentation.h
+ * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2014.
+ * @brief      PiOS instrumentation infrastructure
+ *             Allow to collects performance indexes and execution times
+ * @see        The GNU Public License (GPL) Version 3
+ *
+ *****************************************************************************/
+/*
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
+ * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
+ * for more details.
+ *
+ * You should have received a copy of the GNU General Public License along
+ * with this program; if not, write to the Free Software Foundation, Inc.,
+ * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
+ */
+#ifndef PIOS_INSTRUMENTATION_H
+#define PIOS_INSTRUMENTATION_H
+#include <pios.h>
+#include <pios_debug.h>
+#include <pios_delay.h>
+#include <FreeRTOS.h>
+typedef struct {
+    uint32_t id;
+    int32_t  max;
+    int32_t  min;
+    int32_t  value;
+    uint32_t lastUpdateTS;
+} pios_perf_counter_t;
+extern pios_perf_counter_t *pios_instrumentation_perf_counters;
+extern int8_t pios_instrumentation_last_used_counter;
+
+/**
+ * Update a counter with a new value
+ * @param counterIdx index of the counter to update @see PIOS_Instrumentation_SearchCounter @see PIOS_Instrumentation_CreateCounter
+ * @param newValue the updated value.
+ */
+inline void PIOS_Instrumentation_updateCounter(int8_t counterIdx, int32_t newValue)
+{
+    PIOS_Assert(pios_instrumentation_perf_counters && (counterIdx <= pios_instrumentation_last_used_counter));
+    vPortEnterCritical();
+    pios_perf_counter_t *counter = &pios_instrumentation_perf_counters[counterIdx];
+    counter->value = newValue;
+    if (counter->value > counter->max) {
+        counter->max = counter->value;
+    }
+    if (counter->value < counter->min) {
+        counter->min = counter->value;
+    }
+    counter->lastUpdateTS = PIOS_DELAY_GetRaw();
+    vPortExitCritical();
+}
+
+/**
+ * Used to determine the time duration of a code block, mark the begin of the block. @see PIOS_Instrumentation_TimeEnd
+ * @param counterIdx counterIdx index of the counter @see PIOS_Instrumentation_SearchCounter @see PIOS_Instrumentation_CreateCounter
+ */
+inline void PIOS_Instrumentation_TimeStart(int8_t counterIdx)
+{
+    vPortEnterCritical();
+    PIOS_Assert(pios_instrumentation_perf_counters && (counterIdx <= pios_instrumentation_last_used_counter));
+    pios_perf_counter_t *counter = &pios_instrumentation_perf_counters[counterIdx];
+    counter->lastUpdateTS = PIOS_DELAY_GetRaw();
+    vPortExitCritical();
+}
+
+/**
+ * Used to determine the time duration of a code block, mark the end of the block. @see PIOS_Instrumentation_TimeStart
+ * @param counterIdx counterIdx index of the counter @see PIOS_Instrumentation_SearchCounter @see PIOS_Instrumentation_CreateCounter
+ */
+inline void PIOS_Instrumentation_TimeEnd(int8_t counterIdx)
+{
+    PIOS_Assert(pios_instrumentation_perf_counters && (counterIdx <= pios_instrumentation_last_used_counter));
+    vPortEnterCritical();
+    pios_perf_counter_t *counter = &pios_instrumentation_perf_counters[counterIdx];
+    counter->value = PIOS_DELAY_DiffuS(counter->lastUpdateTS);
+    if (counter->value > counter->max) {
+        counter->max = counter->value;
+    }
+    if (counter->value < counter->min) {
+        counter->min = counter->value;
+    }
+    counter->lastUpdateTS = PIOS_DELAY_GetRaw();
+    vPortExitCritical();
+}
+
+/**
+ * Initialize the Instrumentation infrastructure
+ * @param maxCounters maximum number of allowed counters
+ */
+void PIOS_Instrumentation_Init(int8_t maxCounters);
+
+/**
+ * Create a new counter.
+ * @param id the unique id to assig to the counter
+ * @return the counter index to be used to manage its content
+ */
+int8_t PIOS_Instrumentation_CreateCounter(uint32_t id);
+
+/**
+ * search a counter index by its unique Id
+ * @param id the unique id to assig to the counter
+ * @return the counter index to be used to manage its content
+ */
+int8_t PIOS_Instrumentation_SearchCounter(uint32_t id);
+
+typedef void (*InstrumentationCounterCallback)(const pios_perf_counter_t *counter, const int8_t index, void *context);
+/**
+ * Retrieve and execute the passed callback for each counter
+ * @param callback to be called for each counter
+ * @param context a context variable pointer that can be passed to the callback
+ */
+void PIOS_Instrumentation_ForEachCounter(InstrumentationCounterCallback callback, void *context);
+
+#endif /* PIOS_INSTRUMENTATION_H */

make/apps-defs.mk

@@ -100,6 +100,7 @@ SRC += $(PIOSCOMMON)/pios_usb_util.c
 SRC += $(PIOSCOMMON)/pios_task_monitor.c
 SRC += $(PIOSCOMMON)/pios_callbackscheduler.c
 SRC += $(PIOSCOMMON)/pios_notify.c
+SRC += $(PIOSCOMMON)/pios_instrumentation.c
 ## Misc library functions
 SRC += $(FLIGHTLIB)/fifo_buffer.c
 SRC += $(FLIGHTLIB)/sanitycheck.c