本文将带你从零实现LSM6DSV320X IMU传感器在Linux平台的驱动移植，并开发具备四元数和欧拉角解算的完整姿态感知应用。

<iframe src="https://player.bilibili.com/player.html?bvid=BV15W1wByESe&page=1" scrolling="no" border="0" frameborder="no" framespacing="0" allowfullscreen="true"> </iframe>

项目背景

LSM6DSV320X是一款高端、低噪声、低功耗小型IMU，集成16 g三轴数字低g加速度计、320 g三轴数字高g加速度计及三轴数字陀螺仪，提供最佳IMU传感器性能，采用四个单独通道（用户界面、OIS、EIS和高g加速度计数据通道）独立处理加速度与角速率数据，具有专用配置、处理和滤波功能，并配备专用高g传感器实现高g冲击与汽车碰撞检测。

虽然在STM32生态中有完善的HAL驱动支持，但在Linux平台的资料相对较少。本项目成功将官方驱动移植到Linux环境，并实现了实时姿态感知功能。

第一次写Linux驱动的项目，可能有许多笔误 还希望各位大佬多多包涵。有错误帮忙指出

之所以选择树莓派是因为其底层IIC驱动已经相对完善 ，一致起来节省时间 目前也在学习MP2系列的MPU，后续有机会也会写一篇MP257移植LSM6D的帖子

硬件准备

树莓派的硬件pinout如上 我们使用的是IIC接口 所以需要使用GPIO2 GPIO3 两个引脚

EVM板硬件连接

主板侧面引出排针直接连接IIC接口就可以

需要注意的一点是板卡的供电 由于树莓派接口使用的是3V3通信的电平 所以我们将传感器板的供电冒取下 改为右侧两个针并联 将树莓派的3V3连接到VDD上，这样就完成了在无需软件修改主板供电的情况下给传感器供电，（主板给传感器的电压默认为1.1V 如果Typec给主板供电可能会出现通信问题和电流倒灌）。

连接完成后 我们使用i2c工具探测连接

如图 出现0X6A即为传感器已经被Linux板卡识别到

使用i2cget工具读取who am i寄存器

出现0x73即linux与传感器通信正常

软件准备

访问ST官网 查找ST的传感器驱动库

https://github.com/STMicroelectronics/lsm6dsv320x-pid.git

clone下来驱动库， 我们只需要重新实现在Linux平台下的IIC读写操作即可

项目结构说明

reg结尾是ST官方驱动库函数 其中包括了传感器的初始化 以及寄存器地址 配置枚举变量 我们直接引用即可

rpi是我们的应用文件 这三个文件要放在同一个目录之下

否侧GCC编译会报错。

项目流程

项目初始化流程如下所示 主要是初始化变量以及打开IIC总线并设置IIC从设备地址（0X6A）和异常处理 正常打开后计入后将句柄传入ST驱动库中提供的句柄进行初始化驱动上下文。

IIC总线打开正常后，会读取传感器的WHO_AM_I寄存器并验证，读取并验证无误后复位一次传感器， 开始配置传感器参数（ACC : 120HZ ±4g 陀螺仪: 120Hz ±2000dps BPU启用），并获取一次时间戳。参数设置原理可以参考我上一篇帖子中 使用MEMS_Studio 的Easy Config配置传感器

传感器参数配置完成后 就开始整个应用逻辑

1. 计算时间间隔(dt) ↓
2. 检查数据就绪状态 ↓
3. 如果数据就绪：
   • 读取加速度计数据 → 转换为mg和g
   • 读取陀螺仪数据 → 转换为mdps和dps
   • 计算四元数姿态
   • 计算欧拉角 ↓
4. 显示所有数据 ↓
5. 延迟50ms ↓
6. 返回步骤1

数据输出格式为四种： 原始加速度: [x, y, z] // 16位原始数据 物理加速度: [ax, ay, az] // 毫克单位 姿态四元数: [w, x, y, z] // 归一化四元数 欧拉角度: [横滚, 俯仰, 偏航] // 度单位

由于是先行评估验证 关于欧拉角的偏航角积分偏移问题还没有处理 所以在代码中并未进行转换，输出数据一直为0，后续可以移植AHRS的参考算法进行磁力计纠偏；

应用层代码解析

题主之前也不是linux开发者，目前只是新手 下文中如有错误还请多多海涵

这个操作框架设计如下

I2C驱动层实现

首先需要实现Linux平台的I2C读写接口：

static int32_t rpi_i2c_write(void *handle, uint8_t reg, 
                           const uint8_t *buf, uint16_t len)
{
    int fd = *(int *)handle;
    struct i2c_msg msg;
    struct i2c_rdwr_ioctl_data ioctl_data;

    // 构建发送缓冲区（寄存器地址+数据）
    uint8_t *tx_buf = malloc(len + 1);
    tx_buf[0] = reg;
    memcpy(&tx_buf[1], buf, len);

    // 配置I2C消息
    msg.addr = LSM6DSV320X_I2C_ADD;
    msg.flags = 0;  // 写操作
    msg.len = len + 1;
    msg.buf = tx_buf;

    // 执行IOCTL调用
    if (ioctl(fd, I2C_RDWR, &ioctl_data) < 0) {
        perror("I2C写错误");
        free(tx_buf);
        return -1;
    }

    free(tx_buf);
    return 0;
}

关于ioctl子系统的相关只是可以在bilibili搜索MP157 Ioctl子系统相关视频

传感器初始化配置

// 设备检测和初始化
int fd = open("/dev/i2c-1", O_RDWR);
ioctl(fd, I2C_SLAVE, 0x6A);  // 设置传感器地址

// 初始化驱动上下文
stmdev_ctx_t dev_ctx;
dev_ctx.write_reg = rpi_i2c_write;
dev_ctx.read_reg = rpi_i2c_read;
dev_ctx.handle = &fd;

// 验证设备ID
uint8_t who_am_i;
lsm6dsv320x_device_id_get(&dev_ctx, &who_am_i);
printf("设备ID: 0x%02X\n", who_am_i);

// 传感器配置
lsm6dsv320x_xl_data_rate_set(&dev_ctx, LSM6DSV320X_ODR_AT_120Hz);  // 加速度计120Hz
lsm6dsv320x_gy_data_rate_set(&dev_ctx, LSM6DSV320X_ODR_AT_120Hz);  // 陀螺仪120Hz
lsm6dsv320x_xl_full_scale_set(&dev_ctx, LSM6DSV320X_4g);           // ±4g量程
lsm6dsv320x_gy_full_scale_set(&dev_ctx, LSM6DSV320X_2000dps);      // ±2000dps量程

这里实现的关键一步是

// 设备检测和初始化
int fd = open("/dev/i2c-1", O_RDWR);
ioctl(fd, I2C_SLAVE, 0x6A);  // 设置传感器地址

// 初始化驱动上下文
stmdev_ctx_t dev_ctx;
dev_ctx.write_reg = rpi_i2c_write;
dev_ctx.read_reg = rpi_i2c_read;
dev_ctx.handle = &fd;

Linux中可以将文件视作操作的最小单位，这里iic1总线视作文件打开，并将上文中实现的操作接口函数以指针的形式赋值给ST驱动接口平台

打开文件对应Linux内核中通过VFS访问IIC驱动。。

ioctl工作原理 ：

// ioctl调用流程：
用户空间 → 系统调用 → 内核VFS → I2C核心层 → I2C适配器驱动

// 在内核中的实现大致如下：
static long i2c_dev_ioctl(struct file *file, unsigned int cmd, unsigned long arg)
{
    switch (cmd) {
    case I2C_SLAVE:
        // 设置从设备地址到i2c_client结构体
        client->addr = arg;
        break;
    }
}

其次是初始化一个接口体对象，这部分在St驱动代码中 我们直接引用

stmdev_ctx_t dev_ctx;

结构体的定义：

typedef struct {
    int32_t (*write_reg)(void *handle, uint8_t reg, const uint8_t *buf, uint16_t len);
    int32_t (*read_reg)(void *handle, uint8_t reg, uint8_t *buf, uint16_t len);
    void *handle;
    void (*mdelay)(uint32_t millisec);
} stmdev_ctx_t;

//write_reg：函数指针，指向寄存器写函数

//read_reg：函数指针，指向寄存器读函数

//handle：泛型指针，用于传递平台特定数据

//mdelay：函数指针，指向延迟函数（可选）

我们的设计模式是将IIC操作统一成抽象的接口，即上文中实现的rpi的iic驱动层实现

这样就可以在不同平台中复用相同业务逻辑的代码 只需要移植驱动层函数即可

所以在rpi驱动实现中，保持了和ST一样的参数

// ST驱动期望的函数签名
int32_t (*write_reg)(void *handle, uint8_t reg, const uint8_t *buf, uint16_t len);
int32_t (*read_reg)(void *handle, uint8_t reg, uint8_t *buf, uint16_t len);

// 我们的平台实现必须匹配这个签名
static int32_t rpi_i2c_write(void *handle, uint8_t reg, const uint8_t *buf, uint16_t len);
static int32_t rpi_i2c_read(void *handle, uint8_t reg, uint8_t *buf, uint16_t len);

通过以上实现思路 我们即可在Linux系统下完成通过IIC读写传感器寄存器的操作。

传感器的初始化操作

实现了寄存器的访问后，应用层上编写逻辑就和STM32这类MCU相差不大，就是封装函数在写入即可，具体封装实现逻辑可以参考文末附件

// 设备检测和初始化
int fd = open("/dev/i2c-1", O_RDWR);
ioctl(fd, I2C_SLAVE, 0x6A);  // 设置传感器地址

// 初始化驱动上下文
stmdev_ctx_t dev_ctx;
dev_ctx.write_reg = rpi_i2c_write;
dev_ctx.read_reg = rpi_i2c_read;
dev_ctx.handle = &fd;

// 验证设备ID
uint8_t who_am_i;
lsm6dsv320x_device_id_get(&dev_ctx, &who_am_i);
printf("设备ID: 0x%02X\n", who_am_i);

// 传感器配置
lsm6dsv320x_xl_data_rate_set(&dev_ctx, LSM6DSV320X_ODR_AT_120Hz);  // 加速度计120Hz
lsm6dsv320x_gy_data_rate_set(&dev_ctx, LSM6DSV320X_ODR_AT_120Hz);  // 陀螺仪120Hz
lsm6dsv320x_xl_full_scale_set(&dev_ctx, LSM6DSV320X_4g);           // ±4g量程
lsm6dsv320x_gy_full_scale_set(&dev_ctx, LSM6DSV320X_2000dps);      // ±2000dps量程

姿态解算

本项目实现了基于互补滤波的实时姿态解算：

// 四元数计算函数
static void calculate_quaternion(float accel[3], float gyro[3], 
                               float dt, float q[4])
{
    // 加速度计数据归一化
    float norm = sqrt(accel[0]*accel[0] + accel[1]*accel[1] + accel[2]*accel[2]);
    if (norm > 0.0f) {
        accel[0] /= norm; accel[1] /= norm; accel[2] /= norm;
    }

    // 陀螺仪数据转换（度/秒 → 弧度/秒）
    float gx = gyro[0] * M_PI / 180.0f;
    float gy = gyro[1] * M_PI / 180.0f;
    float gz = gyro[2] * M_PI / 180.0f;

    // 互补滤波误差校正
    float vx = 2.0f * (q_prev[1]*q_prev[3] - q_prev[0]*q_prev[2]);
    float vy = 2.0f * (q_prev[0]*q_prev[1] + q_prev[2]*q_prev[3]);
    float vz = q_prev[0]*q_prev[0] - q_prev[1]*q_prev[1] 
               - q_prev[2]*q_prev[2] + q_prev[3]*q_prev[3];

    // 误差积分和反馈校正
    float ex = (accel[1]*vz - accel[2]*vy);
    float ey = (accel[2]*vx - accel[0]*vz);
    float ez = (accel[0]*vy - accel[1]*vx);

    // 四元数微分方程数值积分
    float qdot[4];
    qdot[0] = 0.5f * (-q_prev[1]*gx - q_prev[2]*gy - q_prev[3]*gz);
    qdot[1] = 0.5f * (q_prev[0]*gx + q_prev[2]*gz - q_prev[3]*gy);
    // ... 其他分量计算

    // 积分和归一化
    for(int i = 0; i < 4; i++) 
        q[i] = q_prev[i] + qdot[i] * dt;

    norm = sqrt(q[0]*q[0] + q[1]*q[1] + q[2]*q[2] + q[3]*q[3]);
    for(int i = 0; i < 4; i++) 
        q[i] /= norm;
}

四元数转欧拉角

static void quaternion_to_euler(float q[4], float euler[3])
{
    float w = q[0], x = q[1], y = q[2], z = q[3];

    // 横滚角 (Roll)
    float sinr_cosp = 2.0f * (w * x + y * z);
    float cosr_cosp = 1.0f - 2.0f * (x * x + y * y);
    euler[0] = atan2(sinr_cosp, cosr_cosp) * 180.0f / M_PI;

    // 俯仰角 (Pitch)
    float sinp = 2.0f * (w * y - z * x);
    if (fabs(sinp) >= 1.0f)
        euler[1] = copysign(90.0f, sinp);
    else
        euler[1] = asin(sinp) * 180.0f / M_PI;

    // 偏航角 (Yaw)
    float siny_cosp = 2.0f * (w * z + x * y);
    float cosy_cosp = 1.0f - 2.0f * (y * y + z * z);
    euler[2] = atan2(siny_cosp, cosy_cosp) * 180.0f / M_PI;
}

该部分参考可以参照哈尔滨工程大学创梦之翼战队 韭菜的菜开源

四元数EKF姿态更新算法 - 知乎

RoboMaster机器人姿态解算方案开源 - 知乎

代码编译

先前已经说了 驱动库和APP要放在一个目录下

gcc -o lsm6dsv320x_test lsm6dsv320x_rpi.c lsm6dsv320x_reg.c -lm

./lsm6dsv320x_test

执行效果