Minimize latency in Acro modes with acc enabled

src/main/flight/imu.c

@@ -187,25 +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
 
+	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 (sensors(SENSOR_ACC) && (!imuUpdateSensors == ONLY_GYRO)) {
 #if defined(NAZE) || defined(DEBUG_IMU_SPEED)
         time = micros();
 #endif
@@ -222,31 +225,36 @@ void imuUpdate(rollAndPitchTrims_t *accelerometerTrims)
     }
 #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
 }
 
 #else
 
-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
 
+	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 (sensors(SENSOR_ACC) && (!imuUpdateSensors == ONLY_GYRO)) {
 #if defined(NAZE) || defined(DEBUG_IMU_SPEED)
         time = micros();
 #endif
@@ -263,7 +271,9 @@ void imuUpdate(rollAndPitchTrims_t *accelerometerTrims)
     }
 #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();