From a0bcb1b95fbbc68999ccea30e1e25761256f5765 Mon Sep 17 00:00:00 2001
From: borisbstyle <dreambb1982@gmail.com>
Date: Thu, 24 Dec 2015 14:11:26 +0100
Subject: [PATCH] Fix for watchdog delay not being triggered

---
 src/main/mw.c | 9 +++++++--
 1 file changed, 7 insertions(+), 2 deletions(-)

diff --git a/src/main/mw.c b/src/main/mw.c
index be9af372be..37b3031427 100644
--- a/src/main/mw.c
+++ b/src/main/mw.c
@@ -739,12 +739,17 @@ void taskMainPidLoop(void)
 
 // Function for loop trigger
 void taskMainPidLoopCheck(void) {
+    // getTaskDeltaTime() returns delta time freezed at the moment of entering the scheduler. currentTime is freezed at the very same point.
+    // To make busy-waiting timeout work we need to account for time spent within busy-waiting loop
+    uint32_t currentDeltaTime = getTaskDeltaTime(TASK_SELF);
+
     while (1) {
-        if (gyroSyncCheckUpdate() || (getTaskDeltaTime(TASK_SELF) >= (targetLooptime + GYRO_WATCHDOG_DELAY))) {
-            taskMainPidLoop();
+        if (gyroSyncCheckUpdate() || ((currentDeltaTime + (micros() - currentTime)) >= (targetLooptime + GYRO_WATCHDOG_DELAY))) {
             break;
         }
     }
+
+    taskMainPidLoop();
 }
 
 void taskUpdateAccelerometer(void)