Merge branch 'amorale/OP-1227_cc_high_cpu_utilization' into next

flight/libraries/inc/instrumentation.h

@@ -0,0 +1,39 @@
+/**
+ ******************************************************************************
+ *
+ * @file       instrumentation.h
+ * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2014.
+ * @brief      Instrumentation infrastructure
+ *             UAVObject wrapper layer for PiOS instrumentation
+ * @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 INSTRUMENTATION_H
+#define INSTRUMENTATION_H
+#include <perfcounter.h>
+/**
+ * Initialize the instrumentationUAVObject wrapper
+ */
+void InstrumentationInit();
+
+/**
+ * publish all counters to UAVObjects
+ */
+void InstrumentationPublishAllCounters();
+
+#endif /* INSTRUMENTATION_H */

flight/libraries/instrumentation.c

@@ -0,0 +1,62 @@
+/**
+ ******************************************************************************
+ *
+ * @file       instrumentation.c
+ * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2014.
+ * @brief      Instrumentation infrastructure
+ *             UAVObject wrapper layer for PiOS instrumentation
+ * @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 <openpilot.h>
+#include <instrumentation.h>
+#include <pios_instrumentation.h>
+
+static uint8_t publishedCountersInstances = 0;
+static void counterCallback(const pios_perf_counter_t *counter, const int8_t index, void *context);
+static xSemaphoreHandle sem;
+void InstrumentationInit()
+{
+    PerfCounterInitialize();
+    publishedCountersInstances = 1;
+    vSemaphoreCreateBinary(sem);
+}
+
+void InstrumentationPublishAllCounters()
+{
+    if (xSemaphoreTake(sem, 0) != pdTRUE) {
+        return;
+    }
+    PIOS_Instrumentation_ForEachCounter(&counterCallback, NULL);
+    xSemaphoreGive(sem);
+}
+
+void counterCallback(const pios_perf_counter_t *counter, const int8_t index, __attribute__((unused)) void *context)
+{
+    if (publishedCountersInstances < index + 1) {
+        PerfCounterCreateInstance();
+        publishedCountersInstances++;
+    }
+    PerfCounterData data;
+    data.Id = counter->id;
+    data.Counter.Max   = counter->max;
+    data.Counter.Min   = counter->min;
+    data.Counter.Value = counter->value;
+    PerfCounterInstSet(index, &data);
+}

flight/modules/Actuator/actuator.c

@@ -45,6 +45,12 @@
 #include "cameradesired.h"
 #include "manualcontrolcommand.h"
 #include "taskinfo.h"
+#undef PIOS_INCLUDE_INSTRUMENTATION
+#ifdef PIOS_INCLUDE_INSTRUMENTATION
+#include <pios_instrumentation.h>
+static int8_t counter;
+// Counter 0xAC700001 total Actuator body execution time(excluding queue waits etc).
+#endif
 
 // Private constants
 #define MAX_QUEUE_SIZE       2
@@ -106,7 +112,6 @@ int32_t ActuatorStart()
 #ifdef PIOS_INCLUDE_WDG
     PIOS_WDG_RegisterFlag(PIOS_WDG_ACTUATOR);
 #endif
-
     return 0;
 }
 
@@ -173,6 +178,9 @@ static void actuatorTask(__attribute__((unused)) void *parameters)
     float throttleDesired;
     float collectiveDesired;
 
+#ifdef PIOS_INCLUDE_INSTRUMENTATION
+    counter = PIOS_Instrumentation_CreateCounter(0xAC700001);
+#endif
     /* Read initial values of ActuatorSettings */
     ActuatorSettingsData actuatorSettings;
 
@@ -199,7 +207,9 @@ static void actuatorTask(__attribute__((unused)) void *parameters)
 
         // Wait until the ActuatorDesired object is updated
         uint8_t rc = xQueueReceive(queue, &ev, FAILSAFE_TIMEOUT_MS / portTICK_RATE_MS);
-
+#ifdef PIOS_INCLUDE_INSTRUMENTATION
+        PIOS_Instrumentation_TimeStart(counter);
+#endif
         /* Process settings updated events even in timeout case so we always act on the latest settings */
         if (actuator_settings_updated) {
             actuator_settings_updated = false;
@@ -449,6 +459,9 @@ static void actuatorTask(__attribute__((unused)) void *parameters)
             ActuatorCommandSet(&command);
             AlarmsSet(SYSTEMALARMS_ALARM_ACTUATOR, SYSTEMALARMS_ALARM_CRITICAL);
         }
+#ifdef PIOS_INCLUDE_INSTRUMENTATION
+        PIOS_Instrumentation_TimeEnd(counter);
+#endif
     }
 }
 
@@ -482,17 +495,17 @@ float ProcessMixer(const int index, const float curve1, const float curve2,
         result += accumulator;
         if (period > 0.0f) {
             if (accumulator > 0.0f) {
-                float filter = mixerSettings->AccelTime / period;
+                float invFilter = period / mixerSettings->AccelTime;
-                if (filter < 1) {
+                if (invFilter > 1) {
-                    filter = 1;
+                    invFilter = 1;
                 }
-                accumulator -= accumulator / filter;
+                accumulator -= accumulator * invFilter;
             } else {
-                float filter = mixerSettings->DecelTime / period;
+                float invFilter = period / mixerSettings->DecelTime;
-                if (filter < 1) {
+                if (invFilter > 1) {
-                    filter = 1;
+                    invFilter = 1;
                 }
-                accumulator -= accumulator / filter;
+                accumulator -= accumulator * invFilter;
             }
         }
         filterAccumulator[index] = accumulator;

flight/modules/Attitude/attitude.c

@@ -65,6 +65,17 @@
 #include "CoordinateConversions.h"
 #include <pios_notify.h>
 
+#undef PIOS_INCLUDE_INSTRUMENTATION
+#ifdef PIOS_INCLUDE_INSTRUMENTATION
+#include <pios_instrumentation.h>
+int8_t counterUpd;
+int8_t counterAccelSamples = -1;
+// Counters:
+// - 0xA7710001 sensor fetch duration
+// - 0xA7710002 updateAttitude execution time
+// - 0xA7710003 Attitude loop rate(period)
+// - 0xA7710004 number of accel samples read for each loop (cc only).
+#endif
 // Private constants
 #define STACK_SIZE_BYTES 540
 #define TASK_PRIORITY    (tskIDLE_PRIORITY + 3)
@@ -72,7 +83,7 @@
 #define SENSOR_PERIOD    4
 #define UPDATE_RATE      25.0f
 
-#define UPDATE_EXPECTED  (1.0f / 666.0f)
+#define UPDATE_EXPECTED  (1.0f / 500.0f)
 #define UPDATE_MIN       1.0e-6f
 #define UPDATE_MAX       1.0f
 #define UPDATE_ALPHA     1.0e-2f
@@ -231,12 +242,19 @@ static void AttitudeTask(__attribute__((unused)) void *parameters)
         gyro_queue = xQueueCreate(1, sizeof(float) * 4);
         PIOS_Assert(gyro_queue != NULL);
         PIOS_ADC_SetQueue(gyro_queue);
-        PIOS_ADC_Config((PIOS_ADC_RATE / 1000.0f) * UPDATE_RATE);
+        PIOS_ADC_Config(46);
 #endif
     }
     // Force settings update to make sure rotation loaded
     settingsUpdatedCb(AttitudeSettingsHandle());
-
+#ifdef PIOS_INCLUDE_INSTRUMENTATION
+    counterUpd = PIOS_Instrumentation_CreateCounter(0xA7710001);
+    int8_t counterAtt    = PIOS_Instrumentation_CreateCounter(0xA7710002);
+    int8_t counterPeriod = PIOS_Instrumentation_CreateCounter(0xA7710003);
+    if (!cc3d) {
+        counterAccelSamples = PIOS_Instrumentation_CreateCounter(0xA7710004);
+    }
+#endif
     PIOS_DELTATIME_Init(&dtconfig, UPDATE_EXPECTED, UPDATE_MIN, UPDATE_MAX, UPDATE_ALPHA);
 
     // Main task loop
@@ -291,11 +309,19 @@ static void AttitudeTask(__attribute__((unused)) void *parameters)
         } else {
             // Do not update attitude data in simulation mode
             if (!AttitudeStateReadOnly()) {
+#ifdef PIOS_INCLUDE_INSTRUMENTATION
+                PIOS_Instrumentation_TimeStart(counterAtt);
+#endif
                 updateAttitude(&accelState, &gyros);
+#ifdef PIOS_INCLUDE_INSTRUMENTATION
+                PIOS_Instrumentation_TimeEnd(counterAtt);
+#endif
             }
-
             AlarmsClear(SYSTEMALARMS_ALARM_ATTITUDE);
         }
+#ifdef PIOS_INCLUDE_INSTRUMENTATION
+        PIOS_Instrumentation_TrackPeriod(counterPeriod);
+#endif
     }
 }
 
@@ -321,10 +347,13 @@ static int32_t updateSensors(AccelStateData *accelState, GyroStateData *gyros)
     }
 
     // No accel data available
-    if (PIOS_ADXL345_FifoElements() == 0) {
+    uint8_t fifoSamples = PIOS_ADXL345_FifoElements();
+    if (fifoSamples == 0) {
         return -1;
     }
-
+#ifdef PIOS_INCLUDE_INSTRUMENTATION
+    PIOS_Instrumentation_TimeStart(counterUpd);
+#endif
     // First sample is temperature
     gyros->x = -(gyro[1] - STD_CC_ANALOG_GYRO_NEUTRAL) * gyro_scale.X;
     gyros->y = (gyro[2] - STD_CC_ANALOG_GYRO_NEUTRAL) * gyro_scale.Y;
@@ -333,8 +362,14 @@ static int32_t updateSensors(AccelStateData *accelState, GyroStateData *gyros)
     int32_t x = 0;
     int32_t y = 0;
     int32_t z = 0;
-    uint8_t i = 0;
+
+    uint8_t i = fifoSamples;
     uint8_t samples_remaining;
+    samples_remaining = PIOS_ADXL345_ReadAndAccumulateSamples(&accel_data, fifoSamples);
+    x = accel_data.x;
+    y = -accel_data.y;
+    z = -accel_data.z;
+    if (samples_remaining > 0) {
         do {
             i++;
             samples_remaining = PIOS_ADXL345_Read(&accel_data);
@@ -342,7 +377,10 @@ static int32_t updateSensors(AccelStateData *accelState, GyroStateData *gyros)
             y += -accel_data.y;
             z += -accel_data.z;
         } while ((i < 32) && (samples_remaining > 0));
-
+    }
+#ifdef PIOS_INCLUDE_INSTRUMENTATION
+    PIOS_Instrumentation_updateCounter(counterAccelSamples, i);
+#endif
     float accel[3] = { accel_scale.X * (float)x / i,
                        accel_scale.Y * (float)y / i,
                        accel_scale.Z * (float)z / i };
@@ -363,7 +401,6 @@ static int32_t updateSensors(AccelStateData *accelState, GyroStateData *gyros)
         accelState->y = accel[1];
         accelState->z = accel[2];
     }
-
     if (trim_requested) {
         if (trim_samples >= MAX_TRIM_FLIGHT_SAMPLES) {
             trim_requested = false;
@@ -402,6 +439,10 @@ static int32_t updateSensors(AccelStateData *accelState, GyroStateData *gyros)
     // and make it average zero (weakly)
     gyro_correct_int[2] += -gyros->z * yawBiasRate;
 
+#ifdef PIOS_INCLUDE_INSTRUMENTATION
+    PIOS_Instrumentation_TimeEnd(counterUpd);
+#endif
+
     GyroStateSet(gyros);
     AccelStateSet(accelState);
 
@@ -429,6 +470,10 @@ static int32_t updateSensorsCC3D(AccelStateData *accelStateData, GyroStateData *
     if (GyroStateReadOnly() || AccelStateReadOnly()) {
         return 0;
     }
+#ifdef PIOS_INCLUDE_INSTRUMENTATION
+    PIOS_Instrumentation_TimeStart(counterUpd);
+    vPortEnterCritical();
+#endif
     gyros[0]  = mpu6000_data.gyro_x * gyro_scale.X;
     gyros[1]  = mpu6000_data.gyro_y * gyro_scale.Y;
     gyros[2]  = mpu6000_data.gyro_z * gyro_scale.Z;
@@ -492,7 +537,10 @@ static int32_t updateSensorsCC3D(AccelStateData *accelStateData, GyroStateData *
     // Because most crafts wont get enough information from gravity to zero yaw gyro, we try
     // and make it average zero (weakly)
     gyro_correct_int[2] += -gyrosData->z * yawBiasRate;
-
+#ifdef PIOS_INCLUDE_INSTRUMENTATION
+    vPortExitCritical();
+    PIOS_Instrumentation_TimeEnd(counterUpd);
+#endif
     GyroStateSet(gyrosData);
     AccelStateSet(accelStateData);
 
@@ -512,7 +560,7 @@ static inline void apply_accel_filter(const float *raw, float *filtered)
     }
 }
 
-static void updateAttitude(AccelStateData *accelStateData, GyroStateData *gyrosData)
+__attribute__((optimize("O3"))) static void updateAttitude(AccelStateData *accelStateData, GyroStateData *gyrosData)
 {
     float dT      = PIOS_DELTATIME_GetAverageSeconds(&dtconfig);
 
@@ -553,10 +601,10 @@ static void updateAttitude(AccelStateData *accelStateData, GyroStateData *gyrosD
     if (grot_mag < 1.0e-3f) {
         return;
     }
-
+    const float invMag = 1.0f / (accel_mag * grot_mag);
-    accel_err[0] /= (accel_mag * grot_mag);
+    accel_err[0] *= invMag;
-    accel_err[1] /= (accel_mag * grot_mag);
+    accel_err[1] *= invMag;
-    accel_err[2] /= (accel_mag * grot_mag);
+    accel_err[2] *= invMag;
 
     // Accumulate integral of error.  Scale here so that units are (deg/s) but Ki has units of s
     gyro_correct_int[0] += accel_err[0] * accelKi;
@@ -565,9 +613,10 @@ static void updateAttitude(AccelStateData *accelStateData, GyroStateData *gyrosD
     // gyro_correct_int[2] += accel_err[2] * accelKi;
 
     // Correct rates based on error, integral component dealt with in updateSensors
-    gyros[0] += accel_err[0] * accelKp / dT;
+    const float kpInvdT = accelKp / dT;
-    gyros[1] += accel_err[1] * accelKp / dT;
+    gyros[0] += accel_err[0] * kpInvdT;
-    gyros[2] += accel_err[2] * accelKp / dT;
+    gyros[1] += accel_err[1] * kpInvdT;
+    gyros[2] += accel_err[2] * kpInvdT;
 
     { // scoping variables to save memory
       // Work out time derivative from INSAlgo writeup
@@ -603,10 +652,11 @@ static void updateAttitude(AccelStateData *accelStateData, GyroStateData *gyrosD
         q[2] = 0;
         q[3] = 0;
     } else {
-        q[0] = q[0] / qmag;
+        const float invQmag = 1.0f / qmag;
-        q[1] = q[1] / qmag;
+        q[0] = q[0] * invQmag;
-        q[2] = q[2] / qmag;
+        q[1] = q[1] * invQmag;
-        q[3] = q[3] / qmag;
+        q[2] = q[2] * invQmag;
+        q[3] = q[3] * invQmag;
     }
 
     AttitudeStateData attitudeState;

flight/modules/System/systemmod.c

@@ -55,6 +55,12 @@
 #include <callbackinfo.h>
 #include <hwsettings.h>
 #include <pios_flashfs.h>
+
+#ifdef PIOS_INCLUDE_INSTRUMENTATION
+#include <instrumentation.h>
+#include <pios_instrumentation.h>
+#endif
+
 #if defined(PIOS_INCLUDE_RFM22B)
 #include <oplinkstatus.h>
 #endif
@@ -124,7 +130,6 @@ int32_t SystemModStart(void)
     // Register task
     PIOS_TASK_MONITOR_RegisterTask(TASKINFO_RUNNING_SYSTEM, systemTaskHandle);
 
-
     return 0;
 }
 
@@ -148,6 +153,10 @@ int32_t SystemModInitialize(void)
     WatchdogStatusInitialize();
 #endif
 
+#ifdef PIOS_INCLUDE_INSTRUMENTATION
+    InstrumentationInit();
+#endif
+
     objectPersistenceQueue = xQueueCreate(1, sizeof(UAVObjEvent));
     if (objectPersistenceQueue == NULL) {
         return -1;
@@ -205,6 +214,10 @@ static void systemTask(__attribute__((unused)) void *parameters)
         updateWDGstats();
 #endif
 
+#ifdef PIOS_INCLUDE_INSTRUMENTATION
+        InstrumentationPublishAllCounters();
+#endif
+
 #ifdef DIAG_TASKS
         // Update the task status object
         PIOS_TASK_MONITOR_ForEachTask(taskMonitorForEachCallback, &taskInfoData);

flight/pios/common/pios_adxl345.c

@@ -41,6 +41,12 @@ struct adxl345_dev {
     uint32_t slave_num;
     enum pios_adxl345_dev_magic magic;
 };
+#undef PIOS_INCLUDE_INSTRUMENTATION
+#ifdef PIOS_INCLUDE_INSTRUMENTATION
+#include <pios_instrumentation.h>
+static int8_t counterUpd;
+// Counter 0xA3450001 time it takes to transfer the requested amount of samples using PIOS_ADXL345_ReadAndAccumulateSamples
+#endif
 
 // ! Global structure for this device device
 static struct adxl345_dev *dev;
@@ -59,6 +65,9 @@ static struct adxl345_dev *PIOS_ADXL345_alloc(void)
 {
     struct adxl345_dev *adxl345_dev;
 
+#ifdef PIOS_INCLUDE_INSTRUMENTATION
+    counterUpd  = PIOS_Instrumentation_CreateCounter(0xA3450001);
+#endif
     adxl345_dev = (struct adxl345_dev *)pvPortMalloc(sizeof(*adxl345_dev));
     if (!adxl345_dev) {
         return NULL;
@@ -99,7 +108,7 @@ static int32_t PIOS_ADXL345_ClaimBus()
     if (PIOS_SPI_ClaimBus(dev->spi_id) != 0) {
         return -2;
     }
-
+    PIOS_SPI_SetClockSpeed(dev->spi_id, PIOS_SPI_PRESCALER_8);
     PIOS_SPI_RC_PinSet(dev->spi_id, dev->slave_num, 0);
 
     return 0;
@@ -237,7 +246,7 @@ int32_t PIOS_ADXL345_Init(uint32_t spi_id, uint32_t slave_num)
     dev->slave_num = slave_num;
 
     PIOS_ADXL345_ReleaseBus();
-    PIOS_ADXL345_SelectRate(ADXL_RATE_3200);
+    PIOS_ADXL345_SelectRate(ADXL_RATE_1600);
     PIOS_ADXL345_SetRange(ADXL_RANGE_8G);
     PIOS_ADXL345_FifoDepth(16);
     PIOS_ADXL345_SetMeasure(1);
@@ -299,6 +308,7 @@ int32_t PIOS_ADXL345_FifoElements()
     return rec[1] & 0x3f;
 }
 
+
 /**
  * @brief Read a single set of values from the x y z channels
  * @returns The number of samples remaining in the fifo
@@ -312,13 +322,11 @@ uint8_t PIOS_ADXL345_Read(struct pios_adxl345_data *data)
     if (PIOS_ADXL345_ClaimBus() != 0) {
         return -2;
     }
-
     // To save memory use same buffer for in and out but offset by
     // a byte
-    uint8_t buf[9] = { 0, 0, 0, 0, 0, 0, 0, 0 };
+    uint8_t buf[10] = { 0 };
-    uint8_t rec[9] = { 0, 0, 0, 0, 0, 0, 0, 0 };
+    uint8_t *rec    = buf + 1;
     buf[0] = ADXL_X0_ADDR | ADXL_MULTI_BIT | ADXL_READ_BIT; // Multibyte read starting at X0
-
     if (PIOS_SPI_TransferBlock(dev->spi_id, &buf[0], &rec[0], 9, NULL) < 0) {
         PIOS_ADXL345_ReleaseBus();
         return -3;
@@ -333,4 +341,47 @@ uint8_t PIOS_ADXL345_Read(struct pios_adxl345_data *data)
     return rec[8] & 0x7F; // return number of remaining entries
 }
 
+/**
+ * @brief Read a single set of values from the x y z channels
+ * @returns 0 if successful
+ */
+
+int8_t PIOS_ADXL345_ReadAndAccumulateSamples(struct pios_adxl345_data *data, uint8_t samples)
+{
+    if (PIOS_ADXL345_Validate(dev) != 0) {
+        return -1;
+    }
+
+    if (PIOS_ADXL345_ClaimBus() != 0) {
+        return -2;
+    }
+    data->x = 0;
+    data->y = 0;
+    data->z = 0;
+    // To save memory use same buffer for in and out but offset by
+    // a byte
+#ifdef PIOS_INCLUDE_INSTRUMENTATION
+    PIOS_Instrumentation_TimeStart(counterUpd);
+#endif
+    for (uint8_t i = 0; i < samples; i++) {
+        uint8_t buf[7] = { 0 };
+        uint8_t *rec   = &buf[1];
+        PIOS_SPI_RC_PinSet(dev->spi_id, dev->slave_num, 0);
+        buf[0] = ADXL_X0_ADDR | ADXL_MULTI_BIT | ADXL_READ_BIT; // Multibyte read starting at X0
+        if (PIOS_SPI_TransferBlock(dev->spi_id, &buf[0], &rec[0], 7, NULL) < 0) {
+            PIOS_ADXL345_ReleaseBus();
+            return -3;
+        }
+        PIOS_SPI_RC_PinSet(dev->spi_id, dev->slave_num, 1);
+        data->x += rec[1] + (rec[2] << 8);
+        data->y += rec[3] + (rec[4] << 8);
+        data->z += rec[5] + (rec[6] << 8);
+    }
+    uint8_t r = PIOS_SPI_TransferByte(dev->spi_id, ADXL_FIFOSTATUS_ADDR | ADXL_READ_BIT);
+#ifdef PIOS_INCLUDE_INSTRUMENTATION
+    PIOS_Instrumentation_TimeEnd(counterUpd);
+#endif
+    PIOS_ADXL345_ReleaseBus();
+    return r & 0x7F; // return number of remaining entries
+}
 #endif /* PIOS_INCLUDE_ADXL345 */

flight/pios/common/pios_instrumentation.c

@@ -0,0 +1,75 @@
+/**
+ ******************************************************************************
+ *
+ * @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;
+    pios_instrumentation_perf_counters[idx].max = INT32_MIN;
+    pios_instrumentation_perf_counters[idx].min = INT32_MAX;
+    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_adc.h

@@ -32,7 +32,7 @@
 #define PIOS_ADC_H
 
 // Maximum of 50 oversampled points
-#define PIOS_ADC_MAX_SAMPLES ((((PIOS_ADC_NUM_CHANNELS + PIOS_ADC_USE_ADC2) >> PIOS_ADC_USE_ADC2) << PIOS_ADC_USE_ADC2) * PIOS_ADC_MAX_OVERSAMPLING * 2)
+#define PIOS_ADC_MAX_SAMPLES ((((PIOS_ADC_NUM_CHANNELS + PIOS_ADC_USE_ADC2) >> PIOS_ADC_USE_ADC2) << PIOS_ADC_USE_ADC2) * PIOS_ADC_MAX_OVERSAMPLING * 2 / 2)
 
 typedef void (*ADCCallback)(float *data);
 

flight/pios/inc/pios_adxl345.h

@@ -72,6 +72,7 @@ int32_t PIOS_ADXL345_SelectRate(uint8_t rate);
 int32_t PIOS_ADXL345_SetRange(uint8_t range);
 int32_t PIOS_ADXL345_Init(uint32_t spi_id, uint32_t slave_num);
 uint8_t PIOS_ADXL345_Read(struct pios_adxl345_data *data);
+int8_t PIOS_ADXL345_ReadAndAccumulateSamples(struct pios_adxl345_data *data, uint8_t samples);
 int32_t PIOS_ADXL345_FifoElements();
 int32_t PIOS_ADXL345_Test();
 

flight/pios/inc/pios_instrumentation.h

@@ -0,0 +1,145 @@
+/**
+ ******************************************************************************
+ *
+ * @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();
+}
+
+/**
+ * Used to determine the mean period between each call to the function
+ * @param counterIdx counterIdx index of the counter @see PIOS_Instrumentation_SearchCounter @see PIOS_Instrumentation_CreateCounter
+ */
+inline void PIOS_Instrumentation_TrackPeriod(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];
+    uint32_t period = PIOS_DELAY_DiffuS(counter->lastUpdateTS);
+    counter->lastUpdateTS = PIOS_DELAY_GetRaw();
+    counter->value = (counter->value * 14 + period * 2) / 16;
+    if (counter->value > counter->max) {
+        counter->max = counter->value;
+    }
+    if (counter->value < counter->min) {
+        counter->min = counter->value;
+    }
+    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 */

flight/pios/stm32f10x/inc/pios_architecture.h

@@ -33,7 +33,7 @@
 // defines for Temp measurements
 #define PIOS_ADC_STM32_TEMP_V25       1.43f /* V */
 #define PIOS_ADC_STM32_TEMP_AVG_SLOPE 4.3f /* mV/C */
-#define PIOS_CONVERT_VOLT_TO_CPU_TEMP(x) ((PIOS_ADC_STM32_TEMP_V25 - x) * 1000.0f / PIOS_ADC_STM32_TEMP_AVG_SLOPE + 25.0f)
+#define PIOS_CONVERT_VOLT_TO_CPU_TEMP(x) ((PIOS_ADC_STM32_TEMP_V25 - x) * (1000.0f / PIOS_ADC_STM32_TEMP_AVG_SLOPE) + 25.0f)
 
 
 #endif /* PIOS_ARCHITECTURE_H */

flight/pios/stm32f10x/pios_adc.c

@@ -32,7 +32,6 @@
 #ifdef PIOS_INCLUDE_ADC
 
 #include <pios_adc_priv.h>
-
 // Private types
 
 enum pios_adc_dev_magic {
@@ -47,12 +46,14 @@ struct pios_adc_dev {
 #endif
     volatile int16_t *valid_data_buffer;
     volatile uint8_t adc_oversample;
+    volatile uint8_t adc_oversample_block_size;
     uint8_t  dma_block_size;
     uint16_t dma_half_buffer_size;
 #if defined(PIOS_INCLUDE_ADC)
     int16_t  fir_coeffs[PIOS_ADC_MAX_SAMPLES + 1]  __attribute__((aligned(4)));
     volatile int16_t raw_data_buffer[PIOS_ADC_MAX_SAMPLES]  __attribute__((aligned(4))); // Double buffer that DMA just used
     float    downsampled_buffer[PIOS_ADC_NUM_CHANNELS]  __attribute__((aligned(4)));
+    uint8_t  downsampleStep; // allows to reduce the downsampling buffer by operating in two steps
 #endif
     enum pios_adc_dev_magic magic;
 };
@@ -135,7 +136,6 @@ int32_t PIOS_ADC_Init(const struct pios_adc_cfg *cfg)
     }
 
     PIOS_ADC_Config(PIOS_ADC_MAX_OVERSAMPLING);
-
     return 0;
 }
 
@@ -145,8 +145,10 @@ int32_t PIOS_ADC_Init(const struct pios_adc_cfg *cfg)
  */
 void PIOS_ADC_Config(uint32_t oversampling)
 {
+    // Ensure oversample is an even number
+    PIOS_Assert(!(oversampling & 0x1));
     pios_adc_dev->adc_oversample = (oversampling > PIOS_ADC_MAX_OVERSAMPLING) ? PIOS_ADC_MAX_OVERSAMPLING : oversampling;
-
+    pios_adc_dev->adc_oversample_block_size = pios_adc_dev->adc_oversample / 2;
     ADC_DeInit(ADC1);
     ADC_DeInit(ADC2);
 
@@ -219,7 +221,7 @@ void PIOS_ADC_Config(uint32_t oversampling)
 
     /* This makes sure we have an even number of transfers if using ADC2 */
     pios_adc_dev->dma_block_size = ((PIOS_ADC_NUM_CHANNELS + PIOS_ADC_USE_ADC2) >> PIOS_ADC_USE_ADC2) << PIOS_ADC_USE_ADC2;
-    pios_adc_dev->dma_half_buffer_size = pios_adc_dev->dma_block_size * pios_adc_dev->adc_oversample;
+    pios_adc_dev->dma_half_buffer_size = pios_adc_dev->dma_block_size * pios_adc_dev->adc_oversample_block_size;
 
     /* Configure DMA channel */
     DMA_InitTypeDef dma_init = pios_adc_dev->cfg->dma.rx.init;
@@ -324,23 +326,54 @@ void PIOS_ADC_SetFIRCoefficients(float *new_filter)
     }
 }
 
+
 /**
  * @brief Downsample the data for each of the channels then call
  * callback function if installed
  */
+__attribute__((optimize("O3")))
 void PIOS_ADC_downsample_data()
 {
     uint16_t chan;
     uint16_t sample;
     float *downsampled_buffer = &pios_adc_dev->downsampled_buffer[0];
+    static int32_t sum[PIOS_ADC_NUM_CHANNELS] = { 0 };
 
+    if (pios_adc_dev->downsampleStep == 0) {
+        for (uint8_t i = 0; i < PIOS_ADC_NUM_CHANNELS; i++) {
+            sum[i] = 0;
+        }
+    }
+
+    for (sample = 0; sample < pios_adc_dev->adc_oversample_block_size; sample++) {
+        const uint16_t *buffer  = (const uint16_t *)&pios_adc_dev->valid_data_buffer[sample * pios_adc_dev->dma_block_size];
+        const uint16_t firCoeff = pios_adc_dev->fir_coeffs[sample + PIOS_ADC_NUM_CHANNELS * pios_adc_dev->downsampleStep];
+#if (PIOS_ADC_USE_TEMP_SENSOR)
+        for (chan = 1; chan < PIOS_ADC_NUM_CHANNELS; chan++) {
+#else
         for (chan = 0; chan < PIOS_ADC_NUM_CHANNELS; chan++) {
-        int32_t sum = 0;
+#endif
-        for (sample = 0; sample < pios_adc_dev->adc_oversample; sample++) {
+            sum[chan] += buffer[chan] * firCoeff;
-            sum += pios_adc_dev->valid_data_buffer[chan + sample * pios_adc_dev->dma_block_size] * pios_adc_dev->fir_coeffs[sample];
         }
-        downsampled_buffer[chan] = (float)sum / pios_adc_dev->fir_coeffs[pios_adc_dev->adc_oversample];
     }
+    // Full downsampling + fir filter is a little bit overwhelming for temp sensor.
+    if (pios_adc_dev->downsampleStep == 0) {
+        // first part of downsampling done. wait until the second block of samples is acquired
+        pios_adc_dev->downsampleStep = 1;
+        return;
+    }
+    pios_adc_dev->downsampleStep = 0;
+#if (PIOS_ADC_USE_TEMP_SENSOR)
+    for (chan = 1; chan < PIOS_ADC_NUM_CHANNELS; chan++) {
+    #else
+    for (chan = 0; chan < PIOS_ADC_NUM_CHANNELS; chan++) {
+    #endif
+        downsampled_buffer[chan] = (float)sum[chan] / (pios_adc_dev->fir_coeffs[pios_adc_dev->adc_oversample]);
+    }
+
+    #if (PIOS_ADC_USE_TEMP_SENSOR)
+    downsampled_buffer[0] = pios_adc_dev->valid_data_buffer[0];
+    #endif
 
 #if defined(PIOS_INCLUDE_FREERTOS)
     if (pios_adc_dev->data_queue) {

flight/targets/boards/coptercontrol/board_hw_defs.c

@@ -455,7 +455,7 @@ static const struct pios_adc_cfg pios_adc_cfg = {
             .flags = (DMA1_FLAG_TC1 | DMA1_FLAG_TE1 | DMA1_FLAG_HT1 | DMA1_FLAG_GL1),
             .init  = {
                 .NVIC_IRQChannel    = DMA1_Channel1_IRQn,
-                .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGH,
+                .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID,
                 .NVIC_IRQChannelSubPriority        = 0,
                 .NVIC_IRQChannelCmd = ENABLE,
             },

flight/targets/boards/coptercontrol/firmware/Makefile

@@ -72,6 +72,7 @@ ifndef TESTAPP
     SRC += $(OPSYSTEM)/coptercontrol.c
     SRC += $(OPSYSTEM)/pios_board.c
     SRC += $(FLIGHTLIB)/alarms.c
+    SRC += $(FLIGHTLIB)/instrumentation.c
     SRC += $(OPUAVTALK)/uavtalk.c
     SRC += $(OPUAVOBJ)/uavobjectmanager.c
     SRC += $(OPUAVOBJ)/eventdispatcher.c
@@ -126,6 +127,7 @@ ifndef TESTAPP
     SRC += $(OPUAVSYNTHDIR)/txpidsettings.c
     SRC += $(OPUAVSYNTHDIR)/airspeedstate.c
     SRC += $(OPUAVSYNTHDIR)/mpu6000settings.c
+    SRC += $(OPUAVSYNTHDIR)/perfcounter.c
 else
     ## Test Code
     SRC += $(OPTESTS)/test_common.c

flight/targets/boards/coptercontrol/firmware/inc/FreeRTOSConfig.h

@@ -23,7 +23,7 @@
 /* Notes: We use 5 task priorities */
 
 #define configUSE_PREEMPTION                         1
-#define configUSE_IDLE_HOOK                          0
+#define configUSE_IDLE_HOOK                          1
 #define configUSE_TICK_HOOK                          0
 #define configUSE_MALLOC_FAILED_HOOK                 1
 #define configCPU_CLOCK_HZ                           ((unsigned long)72000000)

flight/targets/boards/coptercontrol/firmware/inc/pios_config.h

@@ -56,6 +56,8 @@
 #define PIOS_INCLUDE_INITCALL
 #define PIOS_INCLUDE_SYS
 #define PIOS_INCLUDE_TASK_MONITOR
+// #define PIOS_INCLUDE_INSTRUMENTATION
+#define PIOS_INSTRUMENTATION_MAX_COUNTERS 5
 
 /* PIOS hardware peripherals */
 #define PIOS_INCLUDE_IRQ

flight/targets/boards/coptercontrol/firmware/pios_board.c

@@ -33,6 +33,9 @@
 #include <gcsreceiver.h>
 #include <taskinfo.h>
 
+#ifdef PIOS_INCLUDE_INSTRUMENTATION
+#include <pios_instrumentation.h>
+#endif
 /*
  * Pull in the board-specific static HW definitions.
  * Including .c files is a bit ugly but this allows all of
@@ -150,6 +153,10 @@ void PIOS_Board_Init(void)
     PIOS_LED_Init(led_cfg);
 #endif /* PIOS_INCLUDE_LED */
 
+#ifdef PIOS_INCLUDE_INSTRUMENTATION
+    PIOS_Instrumentation_Init(PIOS_INSTRUMENTATION_MAX_COUNTERS);
+#endif
+
 #if defined(PIOS_INCLUDE_SPI)
     /* Set up the SPI interface to the serial flash */
 

flight/targets/boards/coptercontrol/pios_board.h

@@ -210,7 +210,7 @@ extern uint32_t pios_com_hkosd_id;
 // PCKL2 = HCLK / 16
 // ADCCLK = PCLK2 / 2
 #define PIOS_ADC_RATE                    (72.0e6f / 1.0f / 8.0f / 252.0f / (PIOS_ADC_NUM_CHANNELS >> PIOS_ADC_USE_ADC2))
-#define PIOS_ADC_MAX_OVERSAMPLING        36
+#define PIOS_ADC_MAX_OVERSAMPLING        48
 
 #define PIOS_ADC_TEMPERATURE_PIN         0
 

flight/targets/boards/discoveryf4bare/firmware/Makefile

@@ -75,12 +75,14 @@ ifndef TESTAPP
     SRC += $(OPUAVTALK)/uavtalk.c
     SRC += $(OPUAVOBJ)/uavobjectmanager.c
     SRC += $(OPUAVOBJ)/eventdispatcher.c
+    
     #ifeq ($(DEBUG), YES)
     SRC += $(OPSYSTEM)/dcc_stdio.c
     SRC += $(OPSYSTEM)/cm3_fault_handlers.c
     #endif
 
     ## Misc library functions
+    SRC += $(FLIGHTLIB)/instrumentation.c
     SRC += $(FLIGHTLIB)/paths.c
 	SRC += $(FLIGHTLIB)/plans.c
     SRC += $(FLIGHTLIB)/WorldMagModel.c

flight/targets/boards/discoveryf4bare/firmware/UAVObjects.inc

@@ -114,6 +114,7 @@ UAVOBJSRCFILENAMES += poilearnsettings
 UAVOBJSRCFILENAMES += mpu6000settings
 UAVOBJSRCFILENAMES += txpidsettings
 UAVOBJSRCFILENAMES += takeofflocation
+UAVOBJSRCFILENAMES += perfcounter
 
 UAVOBJSRC = $(foreach UAVOBJSRCFILE,$(UAVOBJSRCFILENAMES),$(OPUAVSYNTHDIR)/$(UAVOBJSRCFILE).c )
 UAVOBJDEFINE = $(foreach UAVOBJSRCFILE,$(UAVOBJSRCFILENAMES),-DUAVOBJ_INIT_$(UAVOBJSRCFILE) )

flight/targets/boards/discoveryf4bare/firmware/inc/pios_config.h

@@ -56,6 +56,9 @@
 #define PIOS_INCLUDE_SYS
 #define PIOS_INCLUDE_TASK_MONITOR
 
+#define PIOS_INSTRUMENTATION_MAX_COUNTERS 10
+#define PIOS_INCLUDE_INSTRUMENTATION
+
 /* PIOS hardware peripherals */
 #define PIOS_INCLUDE_IRQ
 #define PIOS_INCLUDE_RTC

flight/targets/boards/discoveryf4bare/firmware/pios_board.c

@@ -36,6 +36,11 @@
 #include <pios_oplinkrcvr_priv.h>
 #include <taskinfo.h>
 #include <pios_callbackscheduler.h>
+
+#ifdef PIOS_INCLUDE_INSTRUMENTATION
+#include <pios_instrumentation.h>
+#endif
+
 /*
  * Pull in the board-specific static HW definitions.
  * Including .c files is a bit ugly but this allows all of
@@ -340,6 +345,11 @@ void PIOS_Board_Init(void)
     PIOS_LED_Init(led_cfg);
 #endif /* PIOS_INCLUDE_LED */
 
+#ifdef PIOS_INCLUDE_INSTRUMENTATION
+    PIOS_Instrumentation_Init(PIOS_INSTRUMENTATION_MAX_COUNTERS);
+#endif
+
+
 #if false
     /* Set up the SPI interface to the gyro/acelerometer */
     if (PIOS_SPI_Init(&pios_spi_gyro_id, &pios_spi_gyro_cfg)) {

flight/targets/boards/revolution/firmware/Makefile

@@ -73,6 +73,7 @@ ifndef TESTAPP
     SRC += $(OPSYSTEM)/revolution.c
     SRC += $(OPSYSTEM)/pios_board.c
     SRC += $(FLIGHTLIB)/alarms.c
+    SRC += $(FLIGHTLIB)/instrumentation.c
     SRC += $(OPUAVTALK)/uavtalk.c
     SRC += $(OPUAVOBJ)/uavobjectmanager.c
     SRC += $(OPUAVOBJ)/eventdispatcher.c

flight/targets/boards/revolution/firmware/UAVObjects.inc

@@ -114,6 +114,7 @@ UAVOBJSRCFILENAMES += poilearnsettings
 UAVOBJSRCFILENAMES += mpu6000settings
 UAVOBJSRCFILENAMES += txpidsettings
 UAVOBJSRCFILENAMES += takeofflocation
+UAVOBJSRCFILENAMES += perfcounter
 
 UAVOBJSRC = $(foreach UAVOBJSRCFILE,$(UAVOBJSRCFILENAMES),$(OPUAVSYNTHDIR)/$(UAVOBJSRCFILE).c )
 UAVOBJDEFINE = $(foreach UAVOBJSRCFILE,$(UAVOBJSRCFILENAMES),-DUAVOBJ_INIT_$(UAVOBJSRCFILE) )

flight/targets/boards/revolution/firmware/inc/pios_config.h

@@ -56,6 +56,9 @@
 #define PIOS_INCLUDE_SYS
 #define PIOS_INCLUDE_TASK_MONITOR
 
+#define PIOS_INCLUDE_INSTRUMENTATION
+#define PIOS_INSTRUMENTATION_MAX_COUNTERS 10
+
 /* PIOS hardware peripherals */
 #define PIOS_INCLUDE_IRQ
 #define PIOS_INCLUDE_RTC

flight/targets/boards/revolution/firmware/pios_board.c

@@ -37,6 +37,11 @@
 #include <taskinfo.h>
 #include <pios_ws2811.h>
 
+
+#ifdef PIOS_INCLUDE_INSTRUMENTATION
+#include <pios_instrumentation.h>
+#endif
+
 /*
  * Pull in the board-specific static HW definitions.
  * Including .c files is a bit ugly but this allows all of
@@ -353,6 +358,11 @@ void PIOS_Board_Init(void)
     PIOS_LED_Init(led_cfg);
 #endif /* PIOS_INCLUDE_LED */
 
+
+#ifdef PIOS_INCLUDE_INSTRUMENTATION
+    PIOS_Instrumentation_Init(PIOS_INSTRUMENTATION_MAX_COUNTERS);
+#endif
+
     /* Set up the SPI interface to the gyro/acelerometer */
     if (PIOS_SPI_Init(&pios_spi_gyro_id, &pios_spi_gyro_cfg)) {
         PIOS_DEBUG_Assert(0);

ground/openpilotgcs/src/plugins/uavobjects/uavobjects.pro

@@ -126,7 +126,8 @@ HEADERS += \
     $$UAVOBJECT_SYNTHETICS/waypoint.h \
     $$UAVOBJECT_SYNTHETICS/waypointactive.h \
     $$UAVOBJECT_SYNTHETICS/mpu6000settings.h \
-    $$UAVOBJECT_SYNTHETICS/takeofflocation.h
+    $$UAVOBJECT_SYNTHETICS/takeofflocation.h \
+    $$UAVOBJECT_SYNTHETICS/perfcounter.h
 
 SOURCES += \
     $$UAVOBJECT_SYNTHETICS/accelgyrosettings.cpp \
@@ -229,4 +230,6 @@ SOURCES += \
     $$UAVOBJECT_SYNTHETICS/waypoint.cpp \
     $$UAVOBJECT_SYNTHETICS/waypointactive.cpp \
     $$UAVOBJECT_SYNTHETICS/mpu6000settings.cpp \
-    $$UAVOBJECT_SYNTHETICS/takeofflocation.cpp
+    $$UAVOBJECT_SYNTHETICS/takeofflocation.cpp \
+    $$UAVOBJECT_SYNTHETICS/perfcounter.cpp
+    

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

shared/uavobjectdefinition/perfcounter.xml

@@ -0,0 +1,11 @@
+<xml>
+    <object name="PerfCounter" singleinstance="false" settings="false" category="System">
+        <description>A single performance counter, used to instrument flight code</description>
+        <field name="Id" units="hex" type="uint32" elements="1" />
+        <field name="Counter" units="" type="int32" elementnames="Value, Min, Max"/>
+        <access gcs="readwrite" flight="readwrite"/>
+        <telemetrygcs acked="false" updatemode="manual" period="0"/>
+        <telemetryflight acked="false" updatemode="manual" period="0"/>
+        <logging updatemode="manual" period="0"/>
+    </object>
+</xml>