0

I'm currently having problems reading incorrect data from the BNO055 sensor when multi-threading on linux. When I run a separate program with one thread, the data reads quite well, but when integrated into a multi-threading program, the data returned is no longer accurate.

  1. When the z axis face downward, the acceleration data returns -9.8 m/s^2, it faced upward, the data returns to be false (less than 1). This happens the same to x and y axis, but only when I tried to get positive gravity acceleration.
  2. And when this occurs, I tried tilting the sensor 5 to 10 degrees, and the data returned is more correct (under 9 m/s^2). I tried each axis and it still happens the same thing.
  3. In addition, I have noticed that even if the acceleration returns -9.8 m/s^2, when I rotate in one axis, the orientation calculated in sensor fusion mode will sometimes be wrong( the yaw angle returns negative)
  4. The above phenomena do not occur when I run a single thread program.
  5. I am running RaspiOS on a Raspberry Pi 4. And the multi-threading platform is core Flight System.

I have handled race condition using mutex lock when accessing the device address 0x29, so I don't think it can be a problem. I don't know if you guys have any suggestions to solve this problem? Thank you.

EDIT 1:

Here is the single thread. It is pretty much the same for multi threads, except for extra mutex.

#include <stdio.h>
#include <stdint.h>
#include <unistd.h>
#include <fcntl.h>
#include <sys/ioctl.h>
#include <linux/i2c-dev.h>
#include <time.h>

#define I2C_DEVICE "/dev/i2c-1"
#define BNO055_ADDR 0x29

double accel_x, accel_y, accel_z;
double gyro_x, gyro_y, gyro_z;
double mag_x, mag_y, mag_z;
double euler_heading, euler_roll, euler_pitch;

void writeRegister(int fd, uint8_t reg, uint8_t value) {
    uint8_t buf[2];
    buf[0] = reg;
    buf[1] = value;
    write(fd, buf, 2);
}

uint8_t readRegister(int fd, uint8_t reg) {
    write(fd, &reg, 1);
    uint8_t value;
    read(fd, &value, 1);
    return value;
}

int main() {
    int fd;

    if ((fd = open(I2C_DEVICE, O_RDWR)) < 0) {
        return 1;
    }

    if (ioctl(fd, I2C_SLAVE, BNO055_ADDR) < 0) {
        close(fd);
        return 1;
    }

    writeRegister(fd,0x07,0x00);                /*Set page 0*/
    writeRegister(fd, 0x3E, 0x00);              /*Set power to normal*/
    writeRegister(fd, 0x3D, 0x00);              /*Set to config mode*/
    usleep(10000);

    /* Other configurations */

    writeRegister(fd, 0x3B, 0x90);              /*Set unit*/
    writeRegister(fd, 0x3D, 0b1100);            /*Set to NDOF mode*/

    while (1) {
        int16_t raw_accel_x = (int16_t)((readRegister(fd, 0x09) << 8) | readRegister(fd, 0x08));
        int16_t raw_accel_y = (int16_t)((readRegister(fd, 0x0B) << 8) | readRegister(fd, 0x0A));
        int16_t raw_accel_z = (int16_t)((readRegister(fd, 0x0D) << 8) | readRegister(fd, 0x0C));
        accel_x = (double) raw_accel_x / 100.0;
        accel_y = (double) raw_accel_y / 100.0;
        accel_z = (double) raw_accel_z / 100.0;

        int16_t raw_gyro_x = (int16_t)((readRegister(fd, 0x15) << 8) | readRegister(fd, 0x14));
        int16_t raw_gyro_y = (int16_t)((readRegister(fd, 0x17) << 8) | readRegister(fd, 0x16));
        int16_t raw_gyro_z = (int16_t)((readRegister(fd, 0x19) << 8) | readRegister(fd, 0x18));
        gyro_x = (double) raw_gyro_x / 16.0;
        gyro_y = (double) raw_gyro_y / 16.0;
        gyro_z = (double) raw_gyro_z / 16.0;

        int16_t raw_mag_x = (int16_t)((readRegister(fd, 0x0F) << 8) | readRegister(fd, 0x0E));
        int16_t raw_mag_y = (int16_t)((readRegister(fd, 0x11) << 8) | readRegister(fd, 0x10));
        int16_t raw_mag_z = (int16_t)((readRegister(fd, 0x13) << 8) | readRegister(fd, 0x12));
        mag_x = raw_mag_x / 16.0;
        mag_y = raw_mag_y / 16.0;
        mag_z = raw_mag_z / 16.0;

        int16_t raw_euler_heading = (int16_t)((readRegister(fd, 0x1B) << 8) | readRegister(fd, 0x1A));
        int16_t raw_euler_roll = (int16_t)((readRegister(fd, 0x1D) << 8) | readRegister(fd, 0x1C));
        int16_t raw_euler_pitch = (int16_t)((readRegister(fd, 0x1F) << 8) | readRegister(fd, 0x1E));
        euler_heading = raw_euler_heading / 16.0;
        euler_roll = raw_euler_roll / 16.0;
        euler_pitch = raw_euler_pitch / 16.0;

        printf("Accelerometer (X,Y,Z): %.2f, %.2f, %.2f m/s²\n", accel_x, accel_y, accel_z);
        printf("Gyroscope (X,Y,Z): %.2f, %.2f, %.2f °/s\n", gyro_x, gyro_y, gyro_z);
        printf("Magnetometer (X,Y,Z): %.2f, %.2f, %.2f µT\n", mag_x, mag_y, mag_z);
        printf("Euler Angles (Heading, Roll, Pitch): %.2f , %.2f, %.2f degrees\n", euler_heading, euler_roll, euler_pitch);

        usleep(500000);
    }
    close(fd);
    return 0;
}

Here is the data I acquired when pointing outward from earth surface.

Single thread:
Accelerometer (X,Y,Z): 0.37, 0.14, 9.42 m/s²
Euler Angles (Heading, Roll, Pitch): 359.94 , 2.06, -0.69 degrees


Multi threads data:
Accelerometer (X,Y,Z): 0.35, 0.11, -0.85 m/s²
Euler Angles (Heading, Roll, Pitch): 359.25 , 2.06, 0.69 degrees

Here is the data when I tried tilting a little bit. The acceleration in Z axis seems right, but the heading is negative.

Single thread data:
Accelerometer (X,Y,Z): 5.75, -0.19, 7.80 m/s²
Euler Angles (Heading, Roll, Pitch): 8.75 , 36.12, 1.56 degrees

Multi threads data:
Accelerometer (X,Y,Z): 5.45, -0.31, 8.05 m/s²
Euler Angles (Heading, Roll, Pitch): -6.50 , 34.25, -2.38 degrees

Here is the data in multi threads program that returns negative heading.

Multi threads:
Accelerometer (X,Y,Z): -0.16, -0.48, -9.33 m/s²
Euler Angles (Heading, Roll, Pitch): -4.44 , -0.25, -1.19 degrees

In short,

  1. When the direction of the axis vector point out the perpendicular earth surface, the accelerator returns wrong value (positive value). When pointing inward, the data seems normal (negative value).
  2. The heading return negative value is also wrong, but I don't know its behaviors (this value varies from 0 to 360 degree).

EDIT 2 Here is part in multi-threading program. Since I have multiple I2C devices so I consider the part to take and release mutex is kinda necessary.

void closeDevice(int32 fd)
{
    close(fd);
    rw_i2c_release_mut();
}

int32 openDevice(const char *file)
{
    rw_i2c_get_mut();
    int32 fd = open(file, O_RDWR);
    if (fd < 0) {
        closeDevice(fd);
        return -1;
    }
    return fd;
}

int32 wakeUpDevice(int32 fd, uint8 addr)
{
    if (ioctl(fd, I2C_SLAVE, addr) < 0) {
        closeDevice(fd);
        return -1;
    }
    return CFE_SUCCESS;
}

int32   bno_read_test(double test[3],uint8 sys_pwr)
{
    double store[3] = {0.0,0.0,0.0};
    int32 Status = CFE_SUCCESS;
    int32 fd;
    fd = openDevice(I2C_DEVICE_1);
    if(fd < 0)  return -1;

    Status = wakeUpDevice(fd,BNO055_DEVICE_ADDR);
    if(Status != CFE_SUCCESS) return -1;

    writeRegister(fd,0x07,0x00);                /*Set page 0*/
    writeRegister(fd, 0x3E, 0x00);              /*Set power to normal*/
    writeRegister(fd, 0x3D, 0x00);              /*Set to config mode*/
    usleep(10000);

    /* Other configurations */

    writeRegister(fd, 0x3B, 0x90);              /*Set unit*/
    writeRegister(fd, 0x3D, 0b1100);            /*Set to NDOF mode*/

    int8 i;
    double scale = 16.0;
    uint16 test_raw[3];
    for(i = 0;i < SAMPLE_TAKE_NUM;i++){
        /* 
        ** Read from registers and store inside test_raw 
        */
    
        store[0] += (double) test_raw[0] * scale;
        store[1] += (double) test_raw[1] * scale;
        store[2] += (double) test_raw[2] * scale;
    }

    store[0] /= SAMPLE_TAKE_NUM;
    store[1] /= SAMPLE_TAKE_NUM;
    store[2] /= SAMPLE_TAKE_NUM;

    test[0] = store[0];
    test[1] = store[1];
    test[2] = store[2];

    closeDevice(fd);
    return Status;
}
datng31
  • 1
  • 2
  • 3
    Can you [edit] your question to show us a [mre] that shows the issue? – pmacfarlane Aug 19 '23 at 15:15
  • Hi, thank you for taking a look. I added some extra details for clarification. – datng31 Aug 19 '23 at 16:09
  • 1
    Unless I misunderstand, it seems you've shown us the code that works, rather than the code that doesn't? – pmacfarlane Aug 19 '23 at 17:23
  • Yes. I can assure that their pattern are the same. But I added the part that doesn't work anyway. However, it only doesn't work at some particular positions is what I'm wondering about. I will try to take a look at the data in binary/hex form next Monday as one comment suggest. – datng31 Aug 20 '23 at 03:58
  • If your `Read from registers` comment means just a single iteration of the `while` loop, then your `Single thread` version is very different from your `Multi threads` version. In your `Single thread` version, the sensor is initialized only *one* time. But in your `Multi threads` version, the sensor is initialized *every* time before a read sequence. Why is this repeated initialization necessary? Move the initialization into the `while` loop for a better replication, and then see what happens. BTW Your program should also allow the sensor about 7ms after `Set to NDOF mode` per the datasheet. – sawdust Aug 20 '23 at 05:04
  • @sawdust Yes, you are right, sorry for the lack of information. I will modify later for better illustration. Actually I do have an iteration to read the from registers and then I take the average value. About the repeated initialization, the configuration values can be modified during runtime by a separated program. So I think it is better to configure it every time the reading function is called. Thank you for pointing out the switching time. – datng31 Aug 20 '23 at 05:42
  • @sawdust I just arrived at the lab and take a look at data in hex from registers. I figured out that the mistake comes from my readRegister function (different from single thread) that return a int8_t value. It should return an unsigned instead. Thank you for the suggestion. But why is that ? I will add the answer after I got home. – datng31 Aug 20 '23 at 07:25

1 Answers1

0

Huge appreciate to sawdust for his suggestion. Apparently, I return the wrong type of value when accessing a register. So that the value got interpreted wrongly. The value should return as unsigned.

uint8_t readRegister(int fd, uint8_t reg) {
    write(fd, &reg, 1);
    uint8_t value;
    read(fd, &value, 1);
    return value;
}

0xFF will result in 255 (decimal)

While

int8_t readRegister(int fd, uint8_t reg) {
    write(fd, &reg, 1);
    uint8_t value;
    read(fd, &value, 1);
    return value;
}

0xFF will result in -1 (decimal)
datng31
  • 1
  • 2