Error assert failed: xqueuegenericsend queue.c:832 using dual core of esp32 and BNO055 on Arduino IDE

Question

I am trying to read measurements from the BNO005. I am using both cores of the Esp32, Core0 is reading the magnetometer and Euler angles to calculate the heading at sampling rate 40Hz. Core1 is reading accelerometer and Gyroscope to calculate distance at sampling rate 100Hz.

I am using xTaskCreatePinnedToCore to assign loops to cores.I have been getting this error that causes the Esp32 to reboot. " assert failed: xQueueGenericSend queue.c:832 (pxQueue->pcHead != ((void *)0) || pxQueue->u.xSemaphore.xMutexHolder == ((void *)0) || pxQueue->u.xSemaphore.xMutexHolder == xTaskGetCurrentTaskHandle()) "

Here is my code, I have created my own library to make my code cleaner with every other library I am using.

#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);
 
  }

}

There will be much more to add like handling a web server.

Functions mention do the following

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;

}

I wonder if I can actually read from the same module on both cores at the same time. I am not reading the same thing. I suspect there might be clashing on the I2C bus (I have no idea control over beginning and ending transmission, using Adafruit BNO055 library). I honestly have no idea, what can I do with this problem , does it have anything to do with the stack size.

Initially the problem started with calling another function I initialized in the .ino file not in library, at the beginning it worked but still problem occurred again.

score 0 · Answer 1 · answered Jul 19 '23 at 16:55

I'm getting the same issue. It's because the I2C transaction needs to begin and end in the same Thread. If two threads are trying to access the same I2C buss/i2c device roughly simultaneously, then you'll get this error.

How to fix it? Your tasks could write requests to a queue at timed intervals. A new task could then read this queue, check the type of request, and handle it serially. This ensures all I2C data to the sensor is managed by a single task.

There are of course other ways, like delaying for 5ms, and checking to see if it's time to read the sensor for the purposes you've set-out.

Error assert failed: xqueuegenericsend queue.c:832 using dual core of esp32 and BNO055 on Arduino IDE

1 Answers1