More Jitter Enhancements for Naze target

remove debug

Anti Jitter Enhancements for Naze target

Jitter Optimalisation without emf avoidance

src/main/flight/imu.c

@@ -48,7 +48,10 @@
 #include "config/runtime_config.h"
 
 //#define DEBUG_IMU
-#define DEBUG_IMU_SPEED
+//#define DEBUG_IMU_SPEED
+
+#define MAX_ACC_PROCESSING   360  // Anti jitter equal acc processing each cycle
+#define MAX_GYRO_PROCESSING  100  // Anti jitter equal gyro processing each cycle
 
 int16_t accSmooth[XYZ_AXIS_COUNT];
 int32_t accSum[XYZ_AXIS_COUNT];
@@ -186,23 +189,40 @@ int16_t imuCalculateHeading(t_fp_vector *vec)
 #if 0
 void imuUpdate(rollAndPitchTrims_t *accelerometerTrims)
 {
-#ifdef DEBUG_IMU_SPEED
+#if defined(NAZE)
-    uint32_t time = micros();
+	uint32_t accProcessTime, gyroProcessTime;
 #endif
+#if defined(NAZE) || defined(DEBUG_IMU_SPEED)
+	uint32_t time = micros();
+#endif
+
     gyroUpdate();
+#if defined(NAZE)
+    while (gyroProcessTime < MAX_GYRO_PROCESSING ) {
+        gyroProcessTime = micros() - time;
+    }
+#endif
 #ifdef DEBUG_IMU_SPEED
-    debug[0] = micros() - time;
+    debug[0] = micros() - time; // gyro read time
-    time = micros();
 #endif
     if (sensors(SENSOR_ACC)) {
+#if defined(NAZE) || defined(DEBUG_IMU_SPEED)
+        time = micros();
+#endif
         qAccProcessingStateMachine(accelerometerTrims);
+#if defined(NAZE)
+        while (accProcessTime < MAX_ACC_PROCESSING) {
+            accProcessTime = micros() - time;
+        }
+#endif
     } else {
         accADC[X] = 0;
         accADC[Y] = 0;
         accADC[Z] = 0;
     }
 #ifdef DEBUG_IMU_SPEED
-    debug[2] = debug[0] + debug[1];
+    debug[1] = micros() - time; // acc read time
+    debug[2] = debug[0] + debug[1]; // gyro + acc read time
 #endif
 }
 
@@ -210,23 +230,40 @@ void imuUpdate(rollAndPitchTrims_t *accelerometerTrims)
 
 void imuUpdate(rollAndPitchTrims_t *accelerometerTrims)
 {
-#ifdef DEBUG_IMU_SPEED
+#if defined(NAZE)
-    uint32_t time = micros();
+	uint32_t accProcessTime, gyroProcessTime;
 #endif
+#if defined(NAZE) || defined(DEBUG_IMU_SPEED)
+	uint32_t time = micros();
+#endif
+
     gyroUpdate();
+#if defined(NAZE)
+    while (gyroProcessTime < MAX_GYRO_PROCESSING ) {
+        gyroProcessTime = micros() - time;
+    }
+#endif
 #ifdef DEBUG_IMU_SPEED
-    debug[0] = micros() - time;
+    debug[0] = micros() - time; // gyro read time
-    time = micros();
 #endif
     if (sensors(SENSOR_ACC)) {
+#if defined(NAZE) || defined(DEBUG_IMU_SPEED)
+        time = micros();
+#endif
         qAccProcessingStateMachine(accelerometerTrims);
+#if defined(NAZE)
+    while (accProcessTime < MAX_ACC_PROCESSING) {
+        accProcessTime = micros() - time;
+    }
+#endif
     } else {
         accADC[X] = 0;
         accADC[Y] = 0;
         accADC[Z] = 0;
     }
 #ifdef DEBUG_IMU_SPEED
-    debug[2] = debug[0] + debug[1];
+    debug[1] = micros() - time;     // acc read time
+    debug[2] = debug[0] + debug[1]; // gyro + acc read time
 #endif
 }
 

src/main/mw.c

@@ -20,6 +20,7 @@
 #include <stdint.h>
 #include <math.h>
 
+#include "debug.h"
 #include "platform.h"
 
 #include "common/maths.h"
@@ -88,6 +89,8 @@ enum {
     ALIGN_MAG = 2
 };
 
+//#define DEBUG_JITTER 3
+
 /* VBAT monitoring interval (in microseconds) - 1s*/
 #define VBATINTERVAL (6 * 3500)       
 /* IBat monitoring interval (in microseconds) - 6 default looptimes */
@@ -776,12 +779,23 @@ void loop(void)
         loopTime = currentTime + targetLooptime;
         imuUpdate(&currentProfile->accelerometerTrims);
 
-
         // Measure loop rate just after reading the sensors
         currentTime = micros();
         cycleTime = (int32_t)(currentTime - previousTime);
         previousTime = currentTime;
 
+#ifdef DEBUG_JITTER
+        static uint32_t previousCycleTime;
+
+		if (previousCycleTime > cycleTime) {
+		    debug[DEBUG_JITTER] = previousCycleTime - cycleTime;
+		} else {
+	        debug[DEBUG_JITTER] = cycleTime - previousCycleTime;
+		}
+		previousCycleTime = cycleTime;
+#endif
+
+
         dT = (float)targetLooptime * 0.000001f;
 
         filterApply7TapFIR(gyroADC);

src/main/sensors/acceleration.c

@@ -19,7 +19,6 @@
 #include <stdint.h>
 
 #include "platform.h"
-#include "debug.h"
 
 #include "common/axis.h"
 
@@ -36,8 +35,6 @@
 
 #include "sensors/acceleration.h"
 
-#define DEBUG_IMU_SPEED
-
 int16_t accADC[XYZ_AXIS_COUNT];
 
 acc_t acc;                       // acc access functions
@@ -175,15 +172,10 @@ 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()) {