diff --git a/src/hal/interface/sensors.h b/src/hal/interface/sensors.h index 39c1d90057..1670345361 100644 --- a/src/hal/interface/sensors.h +++ b/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 */ diff --git a/src/hal/src/sensors.c b/src/hal/src/sensors.c index 741f856c1d..0b7cae41e1 100644 --- a/src/hal/src/sensors.c +++ b/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) { diff --git a/src/modules/src/stabilizer.c b/src/modules/src/stabilizer.c index 5a1956c90d..981b95a3c8 100644 --- a/src/modules/src/stabilizer.c +++ b/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 + } } } }