From bf23791fdf6940a0649c98cb85156023e0cf3283 Mon Sep 17 00:00:00 2001
From: Raphael Coeffic <raphaelcoeffic@users.noreply.github.com>
Date: Wed, 15 Feb 2023 07:05:39 +0100
Subject: [PATCH] fix: Various issues related to IMU sensor (#3171)

* fix(imu): reset error counter on successful IMU query

Please note that the HAL already handles busy I2C bus, so that we should not run into the case where the bus is used and another query is started. However, if that happens, HAL_BUSY will be returned, which leads to -1 being returned in our functions, thus increasing the error count until the sensor stops working.

* fix(imu): move imu init into mixer task

Fixes a race condition between init and use.
---
 radio/src/gyro.cpp             | 5 +++++
 radio/src/tasks.cpp            | 4 ----
 radio/src/tasks/mixer_task.cpp | 4 ++++
 3 files changed, 9 insertions(+), 4 deletions(-)

diff --git a/radio/src/gyro.cpp b/radio/src/gyro.cpp
index 56f49fd1f..ffd0e8d3f 100644
--- a/radio/src/gyro.cpp
+++ b/radio/src/gyro.cpp
@@ -54,6 +54,11 @@ void Gyro::wakeup()
     return;
   }
 
+  // reset error count on each
+  // successful query to avoid
+  // stopping the sensor forever
+  errors = 0;
+
   int16_t gx = values[0];
   int16_t gy = values[1];
   // int16_t gz = values[2];
diff --git a/radio/src/tasks.cpp b/radio/src/tasks.cpp
index 08ac269f2..8086c6385 100644
--- a/radio/src/tasks.cpp
+++ b/radio/src/tasks.cpp
@@ -56,10 +56,6 @@ TASK_FUNCTION(menusTask)
   touchPanelInit();
 #endif
 
-#if defined(IMU)
-  gyroInit();
-#endif
-  
   opentxInit();
 
 #if defined(PWR_BUTTON_PRESS)
diff --git a/radio/src/tasks/mixer_task.cpp b/radio/src/tasks/mixer_task.cpp
index 2ac5415d6..c683cebcc 100644
--- a/radio/src/tasks/mixer_task.cpp
+++ b/radio/src/tasks/mixer_task.cpp
@@ -133,6 +133,10 @@ void execMixerFrequentActions()
 
 TASK_FUNCTION(mixerTask)
 {
+#if defined(IMU)
+  gyroInit();
+#endif
+
   mixerSchedulerInit();
   mixerSchedulerStart();