ESP32双核读取BNO055触发xQueueGenericSend断言失败问题排查

问题场景 #

使用ESP32双核架构读取BNO055传感器数据：

• Core0以40Hz采样率读取磁力计和欧拉角，计算航向
• Core1以100Hz采样率读取加速度计和陀螺仪，计算距离
  通过xTaskCreatePinnedToCore分配任务后，设备频繁重启，错误信息如下：

assert failed: xQueueGenericSend queue.c:832 (pxQueue->pcHead != ((void *)0) || pxQueue->u.xSemaphore.xMutexHolder == ((void *)0) || pxQueue->u.xSemaphore.xMutexHolder == xTaskGetCurrentTaskHandle())

相关代码 #

主程序代码：

#include "ins.h"

TaskHandle_t TaskCore0;
TaskHandle_t TaskCore1;

IMU bno ;

void setup() {
  Serial.begin(115200);
  bno.setUpImu();

  xTaskCreatePinnedToCore(
    core0,
    "setupCore0",
    10000,
    NULL,
    1,
    &TaskCore0,
    0
  );

  xTaskCreatePinnedToCore(
    core1,
    "setupCore1",
    10000,
    NULL,
    1,
    &TaskCore1,
    1
  );
}

void core0(void *pvParameters) {
  loopCore0();
}
void core1(void *pvParameters) {
  loopCore1();
}

void loop() {
}

void loopCore0() {
  while (true) {
   bno.getHeading();    
   delay(25);
  }
}

void loopCore1() {
  while (true) {
    bno.getDistance();
    delay(10);
  }
}

IMU类成员函数实现：

float IMU::getHeading() {
  imu::Vector<3> euler = myIMU.getVector(Adafruit_BNO055::VECTOR_EULER);
  imu::Vector<3> mag = myIMU.getVector(Adafruit_BNO055::VECTOR_MAGNETOMETER);

  float Xm = mag.x();
  float Ym = mag.y();
  float Zm = mag.z();

  float cosRoll = cos(euler.z() * M_PI / 180);
  float sinRoll = sin(euler.z() * M_PI / 180);
  float cosPitch = cos(euler.y() * M_PI / 180);
  float sinPitch = sin(euler.y() * M_PI / 180);

  float Xh = Xm * cosPitch + sinRoll * sinPitch * Ym - Zm * sinPitch * cosRoll;
  float Yh = Ym * cosRoll + sinRoll * Zm;

  float heading = atan2(Yh, Xh);

  if (heading < 0)
    heading += 2 * M_PI;
  headingAngle = heading * 180 / M_PI;
  return heading * 180 / M_PI;
}

float IMU::getDistance() {
  imu::Vector<3> acc = myIMU.getVector(Adafruit_BNO055::VECTOR_LINEARACCEL);
  imu::Vector<3> gyro = myIMU.getVector(Adafruit_BNO055::VECTOR_GYROSCOPE);

  // true when vehicle is accelerating only.
  if ((filter(acc.x()) < thresNeg) || (filter(acc.x()) > thresPosi)) {
    if ( ( gyro.z() > 50 ) || ( gyro.z() < -50 )  ) {
    } else {
      Fb = filter(acc.x());
      velocity = velocity + ( ( ( Fa + Fb ) * stepSize ) / 2 );
      Fa = Fb;
      flag = false;
    }
  }

  float Reading = acc.y() - 0.05;
  if ((Reading < 0 && previousReading >= 0) || (Reading > 0 && previousReading <= 0)) {
    crossings = crossings + 1;
  }

  if ( (( -0.2 <= Reading && Reading <= 0.2) && ( -0.2 <= previousReading && previousReading <= 0.2)) ) {
    flag = true;
  }

  previousReading = Reading;

  if (zeroRate == 0 && flag) {
    velocity = 0;
    flag = false ;
  }
  if ( ( gyro.z() > 30 ) || ( gyro.z() < -30 )  ) {
    velocity = 0;
  }

  if (count >= 10) {
    count = 0;
    zeroRate = crossings / 0.1;  // % rate per second
    crossings = 0;
  }

  dist += velocity * stepSize;

  return dist;
}

用户疑问 #

• 是否可以通过ESP32双核同时读取同一BNO055模块的不同数据？
• 怀疑I2C总线冲突（使用Adafruit BNO055库，无法控制传输的开始与结束）
• 不确定问题是否与任务栈大小有关
• 寻求具体解决办法

解决方案 #

核心原因分析 #

断言失败的本质是FreeRTOS队列操作的线程安全被破坏，结合场景来看，根源是双核任务并发访问BNO055的I2C总线：
Adafruit BNO055库依赖的Wire I2C驱动本身不是线程安全的，双核任务同时调用getVector()会触发总线竞争，进而破坏库内部FreeRTOS队列的状态，触发断言。

具体解决步骤 #

1. 添加互斥锁保护I2C访问
   在IMU类中引入FreeRTOS互斥锁，确保同一时间只有一个任务能访问I2C总线：

   // 在IMU类中添加成员
   SemaphoreHandle_t i2cMutex;
   
   // 在IMU::setUpImu()中初始化互斥锁
   void IMU::setUpImu() {
       // 原有的传感器初始化代码...
       i2cMutex = xSemaphoreCreateMutex();
   }
   
   // 修改getHeading函数，包裹I2C操作
   float IMU::getHeading() {
       float result = 0.0f;
       if(xSemaphoreTake(i2cMutex, portMAX_DELAY) == pdTRUE) {
           // 原有的I2C读取和计算代码...
           imu::Vector<3> euler = myIMU.getVector(Adafruit_BNO055::VECTOR_EULER);
           imu::Vector<3> mag = myIMU.getVector(Adafruit_BNO055::VECTOR_MAGNETOMETER);
           // ...后续计算逻辑
           xSemaphoreGive(i2cMutex);
       }
       return result;
   }
   
   // 修改getDistance函数，包裹I2C操作
   float IMU::getDistance() {
       float result = 0.0f;
       if(xSemaphoreTake(i2cMutex, portMAX_DELAY) == pdTRUE) {
           // 原有的I2C读取和计算代码...
           imu::Vector<3> acc = myIMU.getVector(Adafruit_BNO055::VECTOR_LINEARACCEL);
           imu::Vector<3> gyro = myIMU.getVector(Adafruit_BNO055::VECTOR_GYROSCOPE);
           // ...后续计算逻辑
           xSemaphoreGive(i2cMutex);
       }
       return result;
   }
   

2. 调整任务栈大小（适配后续扩展）
   当前任务栈大小为10000字节，虽然能满足现有需求，但后续添加Web服务器等功能时，建议将Core1的栈大小调整至16KB以上：

   xTaskCreatePinnedToCore(
     core1,
     "setupCore1",
     16384,  // 增大栈大小至16KB
     NULL,
     1,
     &TaskCore1,
     1
   );
   

3. 替换delay为FreeRTOS延迟函数（可选优化）
   用vTaskDelay()替代delay()，让任务主动释放CPU，提升系统整体响应性：

   void loopCore0() {
     while (true) {
      bno.getHeading();    
      vTaskDelay(pdMS_TO_TICKS(25));
     }
   }
   
   void loopCore1() {
     while (true) {
       bno.getDistance();
       vTaskDelay(pdMS_TO_TICKS(10));
     }
   }
   

额外说明 #

• 同一BNO055模块可以被双核任务读取，但必须通过互斥锁保证I2C访问的排他性——I2C是半双工总线，不支持并发传输。
• Adafruit BNO055库内部的队列用于处理I2C数据接收，双核并发调用时会破坏队列的线程安全状态，触发断言。

内容的提问来源于stack exchange，提问作者Mazen Elashry