多线程时BNO055读取不正确

问题描述 投票：0回答：1

我目前在 Linux 上使用多线程时从 BNO055 传感器读取不正确的数据时遇到问题。当我用一个线程运行一个单独的程序时，数据读取得很好，但是当集成到多线程程序中时，返回的数据不再准确。

  1. z 轴朝下时，加速度数据返回-9.8 m/s^2，朝上时，数据返回 false（小于 1）。这种情况与 x 和 y 轴相同，但仅当我尝试获得正重力加速度时。
  2. 当发生这种情况时，我尝试将传感器倾斜5到10度，返回的数据更加正确（在9 m/s^2以下）。我尝试了每个轴，但仍然发生同样的事情。
  3. 另外，我注意到即使加速度返回-9.8 m/s^2，当我沿一个轴旋转时，传感器融合模式下计算的方向有时会出错（偏航角返回负值）
  4. 运行单线程程序时不会出现上述现象。
  5. 我在 Raspberry Pi 4 上运行 RaspiOS。多线程平台是核心 Flight System。

我在访问设备地址时使用互斥锁处理了竞争条件

0x29
，所以我认为这不会成为问题。 不知道大家对于解决这个问题有什么建议吗？谢谢。

编辑1：

这是单线程。除了额外的互斥体之外，多线程几乎是相同的。

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

这是我从地球表面向外指向时获得的数据。

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

这是我尝试稍微倾斜时的数据。 Z 轴加速度看似正确，但航向为负。

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

这是多线程程序中返回负标题的数据。

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

简而言之，

  1. 当轴矢量方向指向垂直地球表面时，加速器返回错误值（正值）。当指向内时，数据看起来正常（负值）。
  2. 标题返回负值也是错误的，但我不知道它的行为（这个值从0到360度变化）。

编辑2 这是多线程程序的一部分。由于我有多个 I2C 设备，所以我认为获取和释放互斥体的部分是有必要的。

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;
}
c multithreading embedded-linux sensors imu
1个回答
0
投票

显然，我在访问寄存器时返回了错误类型的值。 所以这个值被错误地解释了。

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)

同时

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)
最新问题
© www.soinside.com 2019 - 2024. All rights reserved.