From 8edb9ef810e0e9edfbaab515491e5dfe58cd5b85 Mon Sep 17 00:00:00 2001
From: Martin Budden <mjbudden@gmail.com>
Date: Sun, 8 Jan 2017 21:36:16 +0000
Subject: [PATCH] Minor gyro optimisations

---
 src/main/fc/fc_tasks.c         |  2 +-
 src/main/scheduler/scheduler.h |  1 +
 src/main/sensors/gyro.c        | 24 ++++++++++++------------
 3 files changed, 14 insertions(+), 13 deletions(-)

diff --git a/src/main/fc/fc_tasks.c b/src/main/fc/fc_tasks.c
index 24448f58a7..1f0b283c71 100644
--- a/src/main/fc/fc_tasks.c
+++ b/src/main/fc/fc_tasks.c
@@ -307,7 +307,7 @@ cfTask_t cfTasks[TASK_COUNT] = {
         .taskName = "SYSTEM",
         .taskFunc = taskSystem,
         .desiredPeriod = TASK_PERIOD_HZ(10),        // 10Hz, every 100 ms
-        .staticPriority = TASK_PRIORITY_HIGH,
+        .staticPriority = TASK_PRIORITY_MEDIUM_HIGH,
     },
 
     [TASK_GYROPID] = {
diff --git a/src/main/scheduler/scheduler.h b/src/main/scheduler/scheduler.h
index 1df8e1866e..82bc9cf2fb 100644
--- a/src/main/scheduler/scheduler.h
+++ b/src/main/scheduler/scheduler.h
@@ -23,6 +23,7 @@ typedef enum {
     TASK_PRIORITY_IDLE = 0,     // Disables dynamic scheduling, task is executed only if no other task is active this cycle
     TASK_PRIORITY_LOW = 1,
     TASK_PRIORITY_MEDIUM = 3,
+    TASK_PRIORITY_MEDIUM_HIGH = 4,
     TASK_PRIORITY_HIGH = 5,
     TASK_PRIORITY_REALTIME = 6,
     TASK_PRIORITY_MAX = 255
diff --git a/src/main/sensors/gyro.c b/src/main/sensors/gyro.c
index 637fd98a2e..6570b862a8 100644
--- a/src/main/sensors/gyro.c
+++ b/src/main/sensors/gyro.c
@@ -409,6 +409,12 @@ void gyroUpdate(void)
         return;
     }
     gyro.dev.dataReady = false;
+    // move gyro data into 32-bit variables to avoid overflows in calculations
+    gyroADC[X] = gyro.dev.gyroADCRaw[X];
+    gyroADC[Y] = gyro.dev.gyroADCRaw[Y];
+    gyroADC[Z] = gyro.dev.gyroADCRaw[Z];
+
+    alignSensors(gyroADC, gyro.dev.gyroAlign);
 
     const bool calibrationComplete = isGyroCalibrationComplete();
     if (calibrationComplete) {
@@ -422,15 +428,7 @@ void gyroUpdate(void)
 #ifdef DEBUG_MPU_DATA_READY_INTERRUPT
         debug[3] = (uint16_t)(micros() & 0xffff);
 #endif
-    }
-    // move gyro data into 32-bit variables to avoid overflows in calculations
-    gyroADC[X] = gyro.dev.gyroADCRaw[X];
-    gyroADC[Y] = gyro.dev.gyroADCRaw[Y];
-    gyroADC[Z] = gyro.dev.gyroADCRaw[Z];
-
-    alignSensors(gyroADC, gyro.dev.gyroAlign);
-
-    if (!calibrationComplete) {
+    } else {
         performGyroCalibration(gyroConfig->gyroMovementCalibrationThreshold);
     }
 
@@ -448,9 +446,11 @@ void gyroUpdate(void)
         gyroADCf = notchFilter1ApplyFn(notchFilter1[axis], gyroADCf);
         gyroADCf = notchFilter2ApplyFn(notchFilter2[axis], gyroADCf);
         gyro.gyroADCf[axis] = gyroADCf;
+    }
 
-        if (!calibrationComplete) {
-            gyroADC[axis] = lrintf(gyro.gyroADCf[axis] / gyro.dev.scale);
-        }
+    if (!calibrationComplete) {
+        gyroADC[X] = lrintf(gyro.gyroADCf[X] / gyro.dev.scale);
+        gyroADC[Y] = lrintf(gyro.gyroADCf[Y] / gyro.dev.scale);
+        gyroADC[Z] = lrintf(gyro.gyroADCf[Z] / gyro.dev.scale);
     }
 }