From 57fc0f0a5784ea9c8b5bb158b48372f29b7ae425 Mon Sep 17 00:00:00 2001
From: ProDrone <me@prodrone.com>
Date: Tue, 22 Sep 2015 16:32:12 +0200
Subject: [PATCH] Added debug IMU speed output

---
 src/main/flight/imu.c           | 39 ++++++++++++++++++++++++++++++++-
 src/main/sensors/acceleration.c | 10 +++++++++
 2 files changed, 48 insertions(+), 1 deletion(-)

diff --git a/src/main/flight/imu.c b/src/main/flight/imu.c
index f2bd18fc6..e433999b9 100644
--- a/src/main/flight/imu.c
+++ b/src/main/flight/imu.c
@@ -48,6 +48,7 @@
 #include "config/runtime_config.h"
 
 //#define DEBUG_IMU
+#define DEBUG_IMU_SPEED
 
 int16_t accSmooth[XYZ_AXIS_COUNT];
 int32_t accSum[XYZ_AXIS_COUNT];
@@ -182,10 +183,17 @@ int16_t imuCalculateHeading(t_fp_vector *vec)
     return head;
 }
 
+#if 0
 void imuUpdate(rollAndPitchTrims_t *accelerometerTrims)
 {
+#ifdef DEBUG_IMU_SPEED
+    uint32_t time = micros();
+#endif
     gyroUpdate();
-
+#ifdef DEBUG_IMU_SPEED
+    debug[0] = micros() - time;
+    time = micros();
+#endif
     if (sensors(SENSOR_ACC)) {
         qAccProcessingStateMachine(accelerometerTrims);
     } else {
@@ -193,8 +201,37 @@ void imuUpdate(rollAndPitchTrims_t *accelerometerTrims)
         accADC[Y] = 0;
         accADC[Z] = 0;
     }
+#ifdef DEBUG_IMU_SPEED
+    debug[2] = debug[0] + debug[1];
+#endif
 }
 
+#else
+
+void imuUpdate(rollAndPitchTrims_t *accelerometerTrims)
+{
+#ifdef DEBUG_IMU_SPEED
+    uint32_t time = micros();
+#endif
+    gyroUpdate();
+#ifdef DEBUG_IMU_SPEED
+    debug[0] = micros() - time;
+    time = micros();
+#endif
+    if (sensors(SENSOR_ACC)) {
+        qAccProcessingStateMachine(accelerometerTrims);
+    } else {
+        accADC[X] = 0;
+        accADC[Y] = 0;
+        accADC[Z] = 0;
+    }
+#ifdef DEBUG_IMU_SPEED
+    debug[2] = debug[0] + debug[1];
+#endif
+}
+
+#endif
+
 int16_t calculateThrottleAngleCorrection(uint8_t throttle_correction_value)
 {
     float cosZ = EstG.V.Z / sqrtf(EstG.V.X * EstG.V.X + EstG.V.Y * EstG.V.Y + EstG.V.Z * EstG.V.Z);
diff --git a/src/main/sensors/acceleration.c b/src/main/sensors/acceleration.c
index 17d58e866..0c3b70c74 100644
--- a/src/main/sensors/acceleration.c
+++ b/src/main/sensors/acceleration.c
@@ -19,11 +19,13 @@
 #include <stdint.h>
 
 #include "platform.h"
+#include "debug.h"
 
 #include "common/axis.h"
 
 #include "drivers/sensor.h"
 #include "drivers/accgyro.h"
+#include "drivers/system.h"
 
 #include "sensors/battery.h"
 #include "sensors/sensors.h"
@@ -34,6 +36,8 @@
 
 #include "sensors/acceleration.h"
 
+#define DEBUG_IMU_SPEED
+
 int16_t accADC[XYZ_AXIS_COUNT];
 
 acc_t acc;                       // acc access functions
@@ -171,9 +175,15 @@ void applyAccelerationTrims(flightDynamicsTrims_t *accelerationTrims)
 
 void updateAccelerationReadings(rollAndPitchTrims_t *rollAndPitchTrims)
 {
+#ifdef DEBUG_IMU_SPEED
+    uint32_t time = micros();
+#endif
     if (!acc.read(accADC)) {
         return;
     }
+#ifdef DEBUG_IMU_SPEED
+    debug[1] = micros() - time;
+#endif
     alignSensors(accADC, accADC, accAlign);
 
     if (!isAccelerationCalibrationComplete()) {