From 10d3c2e76b8d8823fc0fda04664079a5607c35cc Mon Sep 17 00:00:00 2001
From: Martin Budden <mjbudden@gmail.com>
Date: Thu, 5 Jan 2017 21:52:40 +0000
Subject: [PATCH] Reset task statistics after gyro calibration complete

---
 src/main/scheduler/scheduler.c | 19 +++++++++++++++++++
 src/main/scheduler/scheduler.h |  1 +
 src/main/sensors/gyro.c        | 13 ++++++++-----
 3 files changed, 28 insertions(+), 5 deletions(-)

diff --git a/src/main/scheduler/scheduler.c b/src/main/scheduler/scheduler.c
index 12c6419521..9bee98ac6a 100755
--- a/src/main/scheduler/scheduler.c
+++ b/src/main/scheduler/scheduler.c
@@ -185,6 +185,25 @@ uint32_t getTaskDeltaTime(cfTaskId_e taskId)
     }
 }
 
+void schedulerResetTaskStatistics(cfTaskId_e taskId)
+{
+#ifdef SKIP_TASK_STATISTICS
+    UNUSED(taskId);
+#else
+    if (taskId == TASK_SELF) {
+        currentTask->movingSumExecutionTime = 0;
+        currentTask->totalExecutionTime = 0;
+        currentTask->maxExecutionTime = 0;
+    } else if (taskId < TASK_COUNT) {
+        cfTasks[taskId].movingSumExecutionTime = 0;
+        cfTasks[taskId].totalExecutionTime = 0;
+        cfTasks[taskId].totalExecutionTime = 0;
+    } else {
+        return 0;
+    }
+#endif
+}
+
 void schedulerInit(void)
 {
     queueClear();
diff --git a/src/main/scheduler/scheduler.h b/src/main/scheduler/scheduler.h
index 06014f01bb..8fbcd8c246 100644
--- a/src/main/scheduler/scheduler.h
+++ b/src/main/scheduler/scheduler.h
@@ -144,6 +144,7 @@ void getTaskInfo(cfTaskId_e taskId, cfTaskInfo_t *taskInfo);
 void rescheduleTask(cfTaskId_e taskId, uint32_t newPeriodMicros);
 void setTaskEnabled(cfTaskId_e taskId, bool newEnabledState);
 uint32_t getTaskDeltaTime(cfTaskId_e taskId);
+void schedulerResetTaskStatistics(cfTaskId_e taskId);
 
 void schedulerInit(void);
 void scheduler(void);
diff --git a/src/main/sensors/gyro.c b/src/main/sensors/gyro.c
index 8cd8fd8feb..53be29c818 100644
--- a/src/main/sensors/gyro.c
+++ b/src/main/sensors/gyro.c
@@ -54,6 +54,8 @@
 #include "io/beeper.h"
 #include "io/statusindicator.h"
 
+#include "scheduler/scheduler.h"
+
 #include "sensors/sensors.h"
 #include "sensors/boardalignment.h"
 #include "sensors/gyro.h"
@@ -359,6 +361,7 @@ static void performGyroCalibration(uint8_t gyroMovementCalibrationThreshold)
     }
 
     if (isOnFinalGyroCalibrationCycle()) {
+        schedulerResetTaskStatistics(TASK_SELF); // so calibration cycles do not pollute tasks statistics
         beeper(BEEPER_GYRO_CALIBRATED);
     }
     calibratingG--;
@@ -403,11 +406,11 @@ void gyroUpdate(void)
     const bool calibrationComplete = isGyroCalibrationComplete();
     if (calibrationComplete) {
 #if defined(GYRO_USES_SPI) && defined(USE_MPU_DATA_READY_SIGNAL)
-            // SPI-based gyro so can read and update in ISR
-            if (gyroConfig->gyro_isr_update) {
-                mpuGyroSetIsrUpdate(&gyro.dev, gyroUpdateISR);
-                return;
-            }
+        // SPI-based gyro so can read and update in ISR
+        if (gyroConfig->gyro_isr_update) {
+            mpuGyroSetIsrUpdate(&gyro.dev, gyroUpdateISR);
+            return;
+        }
 #endif
 #ifdef DEBUG_MPU_DATA_READY_INTERRUPT
         debug[3] = (uint16_t)(micros() & 0xffff);