From 16cb081f5c419cdafc15fe8ce32db86a74908575 Mon Sep 17 00:00:00 2001
From: Alessio Morale <alessiomorale@gmail.com>
Date: Tue, 10 Jun 2014 21:38:12 +0200
Subject: [PATCH] OP-1365 Flight code instrumentation API: Sample use of
 instrumentation in Atittude module. When instrumentation is enabled at board
 level (pios_board.h, enable PIOS_INCLUDE_INSTRUMENTATION)
 PIOS_INSTRUMENT_MODULE setups the module to be instrumented.

---
 flight/modules/Attitude/attitude.c | 31 ++++++++++++++++++++++++++----
 1 file changed, 27 insertions(+), 4 deletions(-)

diff --git a/flight/modules/Attitude/attitude.c b/flight/modules/Attitude/attitude.c
index 83103750e..817a8f31f 100644
--- a/flight/modules/Attitude/attitude.c
+++ b/flight/modules/Attitude/attitude.c
@@ -65,6 +65,19 @@
 #include "CoordinateConversions.h"
 #include <pios_notify.h>
 
+//#define PIOS_INSTRUMENT_MODULE
+#include <pios_instrumentation_helper.h>
+
+PERF_DEFINE_COUNTER(counterUpd);
+PERF_DEFINE_COUNTER(counterAccelSamples);
+PERF_DEFINE_COUNTER(counterPeriod);
+PERF_DEFINE_COUNTER(counterAtt);
+// 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).
+
 // Private constants
 #define STACK_SIZE_BYTES 540
 #define TASK_PRIORITY    (tskIDLE_PRIORITY + 3)
@@ -234,6 +247,12 @@ static void AttitudeTask(__attribute__((unused)) void *parameters)
         PIOS_ADC_Config((PIOS_ADC_RATE / 1000.0f) * UPDATE_RATE);
 #endif
     }
+
+    PERF_INIT_COUNTER(counterUpd, 0xA7710001);
+    PERF_INIT_COUNTER(counterAtt, 0xA7710002);
+    PERF_INIT_COUNTER(counterPeriod, 0xA7710003);
+    PERF_INIT_COUNTER(counterAccelSamples, 0xA7710004);
+
     // Force settings update to make sure rotation loaded
     settingsUpdatedCb(AttitudeSettingsHandle());
 
@@ -291,9 +310,11 @@ static void AttitudeTask(__attribute__((unused)) void *parameters)
         } else {
             // Do not update attitude data in simulation mode
             if (!AttitudeStateReadOnly()) {
+                PERF_TIMED_SECTION_START(counterAtt);
                 updateAttitude(&accelState, &gyros);
+                PERF_TIMED_SECTION_END(counterAtt);
             }
-
+            PERF_MEASURE_PERIOD(counterPeriod);
             AlarmsClear(SYSTEMALARMS_ALARM_ATTITUDE);
         }
     }
@@ -324,7 +345,7 @@ static int32_t updateSensors(AccelStateData *accelState, GyroStateData *gyros)
     if (PIOS_ADXL345_FifoElements() == 0) {
         return -1;
     }
-
+    PERF_TIMED_SECTION_START(counterUpd);
     // 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;
@@ -342,7 +363,7 @@ static int32_t updateSensors(AccelStateData *accelState, GyroStateData *gyros)
         y += -accel_data.y;
         z += -accel_data.z;
     } while ((i < 32) && (samples_remaining > 0));
-
+    PERF_TRACK_VALUE(counterAccelSamples, i);
     float accel[3] = { accel_scale.X * (float)x / i,
                        accel_scale.Y * (float)y / i,
                        accel_scale.Z * (float)z / i };
@@ -401,6 +422,7 @@ static int32_t updateSensors(AccelStateData *accelState, GyroStateData *gyros)
     // 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] += -gyros->z * yawBiasRate;
+    PERF_TIMED_SECTION_END(counterUpd);
 
     GyroStateSet(gyros);
     AccelStateSet(accelState);
@@ -429,6 +451,7 @@ static int32_t updateSensorsCC3D(AccelStateData *accelStateData, GyroStateData *
     if (GyroStateReadOnly() || AccelStateReadOnly()) {
         return 0;
     }
+    PERF_TIMED_SECTION_START(counterUpd);
     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 +515,7 @@ 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;
-
+    PERF_TIMED_SECTION_END(counterUpd);
     GyroStateSet(gyrosData);
     AccelStateSet(accelStateData);