BNO055 reading incorrect when multi-threading

Question

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

Answer 1

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)