Merge pull request #1432 from bitcraze/tobba/stabilizer_passthrough_fix

Fix stabilizer locked assert when passthrough interface is active

src/hal/interface/sensors.h

@@ -59,6 +59,11 @@ bool sensorsReadBaro(baro_t *baro);
 void sensorsSuspend();
 void sensorsResume();
 
+/**
+ * Check if sensors are suspended (sensor interrupts disabled)
+ * @return true if they are, else false
+ */
+bool isSensorsSuspended();
 /**
  * Set acc mode, one of accModes enum
  */

src/hal/src/sensors.c

@@ -145,6 +145,7 @@ static const sensorsImplementation_t sensorImplementations[SensorImplementation_
 static const sensorsImplementation_t* activeImplementation;
 static bool isInit = false;
 static const sensorsImplementation_t* findImplementation(SensorImplementation_t implementation);
+static bool sensorsSuspended = false;
 
 void sensorsInit(void) {
   if (isInit) {
@@ -208,12 +209,18 @@ void sensorsSetAccMode(accModes accMode) {
 void sensorsSuspend()
 {
   NVIC_DisableIRQ(EXTI15_10_IRQn);
+  sensorsSuspended = true;
 }
 
 void sensorsResume()
 {
   NVIC_EnableIRQ(EXTI15_10_IRQn);
+  sensorsSuspended = false;
+}
 
+bool isSensorsSuspended(void)
+{
+  return sensorsSuspended;
 }
 
 void __attribute__((used)) EXTI14_Callback(void) {

src/modules/src/stabilizer.c

@@ -269,9 +269,12 @@ void rateSupervisorTask(void *pvParameters) {
         }
       }
     } else {
-      // Handle the case where the semaphore was not given within the timeout
-      DEBUG_PRINT("ERROR: stabilizerTask is blocking\n");
-      ASSERT(false); // For safety, assert if the stabilizer task is blocking to ensure motor shutdown
+      // Don't assert if sensors are suspended
+      if (isSensorsSuspended() == false) {
+        // Handle the case where the semaphore was not given within the timeout
+        DEBUG_PRINT("ERROR: stabilizerTask is blocking\n");
+        ASSERT(false); // For safety, assert if the stabilizer task is blocking to ensure motor shutdown
+      }
     }
   }
 }