Minimize latency in Acro modes with acc enabled

src/main/flight/imu.c

@@ -187,66 +187,28 @@ int16_t imuCalculateHeading(t_fp_vector *vec)
 }
 
 #if 0
-void imuUpdate(rollAndPitchTrims_t *accelerometerTrims)
+void imuUpdate(rollAndPitchTrims_t *accelerometerTrims, uint8_t imuUpdateSensors)
 {
-#if defined(NAZE)
-	uint32_t accProcessTime, gyroProcessTime;
-#endif
 #if defined(NAZE) || defined(DEBUG_IMU_SPEED)
 	uint32_t time = micros();
 #endif
+#if defined(NAZE)
+	uint32_t accProcessTime, gyroProcessTime;
+#endif
 
-    gyroUpdate();
+	if (imuUpdateSensors == ONLY_GYRO || imuUpdateSensors == ACC_AND_GYRO) {
+        gyroUpdate();
 #if defined(NAZE)
-    while (gyroProcessTime < MAX_GYRO_PROCESSING ) {
-        gyroProcessTime = micros() - time;
-    }
-#endif
-#ifdef DEBUG_IMU_SPEED
-    debug[0] = micros() - time; // gyro read time
-#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;
+        while (gyroProcessTime < MAX_GYRO_PROCESSING ) {
+            gyroProcessTime = micros() - time;
         }
-#endif
-    } else {
-        accADC[X] = 0;
-        accADC[Y] = 0;
-        accADC[Z] = 0;
-    }
-#ifdef DEBUG_IMU_SPEED
-    debug[1] = micros() - time; // acc read time
-    debug[2] = debug[0] + debug[1]; // gyro + acc read time
-#endif
-}
 
-#else
-
-void imuUpdate(rollAndPitchTrims_t *accelerometerTrims)
-{
-#if defined(NAZE)
-	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; // gyro read time
 #endif
-    if (sensors(SENSOR_ACC)) {
+    }
+    if (sensors(SENSOR_ACC) && (!imuUpdateSensors == ONLY_GYRO)) {
 #if defined(NAZE) || defined(DEBUG_IMU_SPEED)
         time = micros();
 #endif
@@ -263,7 +225,55 @@ void imuUpdate(rollAndPitchTrims_t *accelerometerTrims)
     }
 #ifdef DEBUG_IMU_SPEED
     debug[1] = micros() - time;     // acc read time
-    debug[2] = debug[0] + debug[1]; // gyro + acc read time
+	if (imuUpdateSensors == ACC_AND_GYRO) {
+        debug[2] = debug[0] + debug[1]; // gyro + acc read time
+	}
+#endif
+}
+
+#else
+
+void imuUpdate(rollAndPitchTrims_t *accelerometerTrims, uint8_t imuUpdateSensors)
+{
+#if defined(NAZE) || defined(DEBUG_IMU_SPEED)
+	uint32_t time = micros();
+#endif
+#if defined(NAZE)
+	uint32_t accProcessTime, gyroProcessTime;
+#endif
+
+	if (imuUpdateSensors == ONLY_GYRO || imuUpdateSensors == ACC_AND_GYRO) {
+        gyroUpdate();
+#if defined(NAZE)
+        while (gyroProcessTime < MAX_GYRO_PROCESSING ) {
+            gyroProcessTime = micros() - time;
+        }
+
+#endif
+#ifdef DEBUG_IMU_SPEED
+    debug[0] = micros() - time; // gyro read time
+#endif
+    }
+    if (sensors(SENSOR_ACC) && (!imuUpdateSensors == ONLY_GYRO)) {
+#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[1] = micros() - time;     // acc read time
+	if (imuUpdateSensors == ACC_AND_GYRO) {
+        debug[2] = debug[0] + debug[1]; // gyro + acc read time
+	}
 #endif
 }
 

src/main/flight/imu.h

@@ -64,6 +64,12 @@ typedef enum {
     ACCPROC_COPY
 } accProcessorState_e;
 
+typedef enum {
+    ONLY_GYRO = 0,
+    ONLY_ACC,
+    ACC_AND_GYRO
+} imuUpdateMode_e;
+
 typedef struct accProcessor_s {
     accProcessorState_e state;
 } accProcessor_t;
@@ -84,4 +90,4 @@ float calculateAccZLowPassFilterRCTimeConstant(float accz_lpf_cutoff);
 int16_t imuCalculateHeading(t_fp_vector *vec);
 
 void imuResetAccelerationSum(void);
-void imuUpdate(rollAndPitchTrims_t *accelerometerTrims);
+void imuUpdate(rollAndPitchTrims_t *accelerometerTrims, uint8_t imuUpdateSensors);

src/main/mw.c

@@ -727,6 +727,14 @@ void filterRc(void){
     }
 }
 
+bool imuUpdateAccDelayed(void) {
+    if (flightModeFlags) {
+        return false;
+    } else {
+        return true;
+    }
+}
+
 void loop(void)
 {
     static uint32_t loopTime;
@@ -777,7 +785,13 @@ void loop(void)
     if (gyroSyncCheckUpdate() || (int32_t)(currentTime - (loopTime + GYRO_WATCHDOG_DELAY)) >= 0) {
 
         loopTime = currentTime + targetLooptime;
-        imuUpdate(&currentProfile->accelerometerTrims);
+
+        // Determine current flight mode. When no acc needed in pid calculations we should only read gyro to reduce latency
+        if (imuUpdateAccDelayed()) {
+            imuUpdate(&currentProfile->accelerometerTrims, ONLY_GYRO);  // When no level modes active read only gyro
+        } else {
+            imuUpdate(&currentProfile->accelerometerTrims, ACC_AND_GYRO);  // When level modes active read gyro and acc
+        }
 
         // Measure loop rate just after reading the sensors
         currentTime = micros();
@@ -872,6 +886,11 @@ void loop(void)
             writeMotors();
         }
 
+        // When no level modes active read acc after motor update
+        if (imuUpdateAccDelayed()) {
+            imuUpdate(&currentProfile->accelerometerTrims, ONLY_ACC);
+        }
+
 #ifdef BLACKBOX
         if (!cliMode && feature(FEATURE_BLACKBOX)) {
             handleBlackbox();