意法半导体LSM6DSV320X IMU传感器在Linux平台的驱动移植与姿态感知应用实现

‍‍‍‍‍‍‍‍感谢各位电子爱好者与行业伙伴们对ST板卡评测系列活动的持续关注！该系列活动通过ST中文论坛板卡申请用户对各类板卡的真实体验和评测文章分享，从多个维度、全方位深入剖析ST相关产品的性能表现与实际应用场景，为用户了解和选用板卡提供极具实用价值的参考。

LSM6DSV320X+MEMS主板开发平台STEVAL-MKI109D的板卡评测活动推出后，广大网友踊跃参与，涌现出很多优秀的评测投稿。本期为大家分享论坛网友短笛君的精彩评测内容。

LSM6DSV320X+MEMS主板开发平台STEVAL-MKI109D及其核心功能、应用场景、技术亮点及评测方向已在前期活动招募文中详细说明，想快速了解细节的朋友，可回顾板卡评测｜免费申请微型AI传感器LSM6DSV320X，解锁高精度运动跟踪新体验一文，这里不再赘述。本期我们聚焦论坛网友短笛君的实测体验。他首先分享了MEMS上位机体验并进行MLC数据预采集，随后实现了LSM6DSV320X IMU传感器在Linux平台的驱动移植，并开发具备四元数和欧拉角解算的完整姿态感知应用，最后讲解了如何在MEMS Studio上进行MLC的过程。

短笛君的评测内容

MEMS上位机体验以及MLC数据采集

LSM6DSV320X是意法半导体推出的一款高性能、低功耗的6轴惯性测量单元，集成了3轴数字加速度计和3轴数字陀螺仪。它是ST传感器产品线中的旗舰型号之一，专为需要高精度运动感知和复杂数据处理的先进应用而设计。实际效果，请在ST中文论坛了解。

根据官网描述这颗陀螺仪传感器内置双量程加速度计，高g加速度计量程高达320g量程。

内置智能与机器学习核心：

●集成了可编程的有限状态机和机器学习核心。

●FSM：允许用户定义简单的“如果-那么”逻辑规则，用于识别特定的运动模式（如单击、双击、计步），无需唤醒主处理器，极大节省功耗。

●MLC：能够直接在传感器内部运行机器学习算法（如决策树），用于识别更复杂的活动和情境（如跑步、驾驶、设备跌落），将分类结果直接输出，极大减轻主处理器的计算负担。

后续测试中实际体验了MLC功能经过学习数据后，内置硬件逻辑可以在极低的功耗下运行基础的传感器融合算法，很方便的可以处理外部数据最大限度的节省功耗。

LSM6DSV320X支持自适应自配置（ASC）功能，能够基于检测到的特定运动模式，或基于MLC中配置的特定决策树输出，自动实时重新配置设备，无需主处理器的干预。

内置滤波器：

该器件嵌入了一个专用加速度计传感器，具有独立通道和滤波功能，可进行高g加速度检测，外部总线还支持拓展传感器可以外界磁罗盘等数据进行融合算法的处理。

LSM6DSV320X还支持了I3C接口，通讯速率大幅提高。

MEMS-Studio

MEMS Studio是ST为MEMS传感器平台推出的一款一体化上位机传感器通过载板和软件配置可以实现传感器配置实时数据可视化管理与调试。

本次测评也基于这个上位机平台开展。

将评估平台插入电脑后软件会自动读取串口连接板卡。

连接对应板卡。

成功连接后，进入传感器配置界面，右侧展示了三个传感器的配置参数，分别是低g值加速度计的量程，操作模式以及数据输出速度（ODR），高g加速度计的量程以及ODR和陀螺仪量程以及ODR，通过配置这三个参数来达到不同的性能组合，数据输出速率越高消耗电流越大，三个传感器的最高输出速率均可达到7.68KHz。

接下来就是配置寄存器，由于之前写过默认配置文件，所以寄存器这里已经是配置好的状态，用户依然可以在上位机中自由读写这几个寄存器，数量太多可以自行查看。

这个功能对于软件开发人员来说很方便！没有载板平台时，开发者只能通过编写代码来读写寄存器，十分的麻烦，现在上位机可以一键配置、读取，十分方便。

数据保存界面可以将传感器的原始数据一键打log保存到csv文件内对后面MLC也是十分方便。

几个数据查看面板做的效果是非常直观的用户可以从图表中直接观察到传感器的数据输出。

高级功能中主要是针对FSM（有限状态机）以及MLC这两个流程评估的后续将会更新MLC的评估。

在数据分析界面中主要是针对先前提到的数据保存功能保存下来的log数据进行分析。

如上图将原始数据载入后可以一键标注数据。

上位机根据不同的数据行为会区分不同标签，用户将打过标签的数据输出到MLC这一步即可完成机器学习的模型训练，无需MCU介入传感器自己就可以识别不同的行为模式。

最后一项是类似NoDe-RED的基于流的开发工具，用户可以根据不同的节点来创建自动化任务以及应用程序，无需编辑代码即可快速评估传感器。极大地缩短了产品开发流程；此外，产品功耗表明，九轴配置时实际测量电流约为800ua。

关闭所有传感器待机。

符合手册中的描述，另外IO可编程电压也实际测试了一下，这个功能是为了适配不同MCU间通信，例如部分SOC通信信号引脚电平耐受1.1V就可以改变电平信号电压。

LSM6DSV320X不仅仅是一个运动传感器，更是一个高度集成的“智能传感中心”。它将高性能的加速度计和陀螺仪与独特的嵌入式智能（FSM、MLC）技术相结合，为设备制造商提供了：

●系统级节能：通过边缘计算将决策任务下放到传感器，大幅降低主处理器负载和整体系统功耗。

●增强的功能性：能够实现更复杂、更直观的用户交互和情境感知。

●设计的简化：高集成度减少了对外部元件的需求，简化了硬件和软件设计。

Linux平台驱动移植与姿态感知应用实现

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

项目背景

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

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

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

之所以选择树莓派是因为其底层IIC驱动已经相对完善，一致起来节省时间。

硬件准备

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

EVM板硬件连接

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

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

连接完成后，我们使用I2C工具探测连接。

如图所示，出现0X6A，即为传感器已经被Linux板卡识别到，接下来可使用i2cget工具读取who am i寄存器。

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

软件准备

访问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，BDU启用），并获取一次时间戳。参数设置原理可以参考使用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读写接口：

staticint32_trpi_i2c_write(void*handle,uint8_treg,
             constuint8_t*buf,uint16_tlen)
{
 intfd = *(int*)handle;
 structi2c_msgmsg;
 structi2c_rdwr_ioctl_dataioctl_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子系统相关视频。

传感器初始化配置

// 设备检测和初始化
intfd =open("/dev/i2c-1", O_RDWR);
ioctl(fd, I2C_SLAVE,0x6A); // 设置传感器地址
// 初始化驱动上下文
stmdev_ctx_tdev_ctx;
dev_ctx.write_reg = rpi_i2c_write;
dev_ctx.read_reg = rpi_i2c_read;
dev_ctx.handle = &fd;
// 验证设备ID
uint8_twho_am_i;
lsm6dsv320x_device_id_get(&dev_ctx, &who_am_i);
printf("设备ID: 0x%02X
", 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量程

这里实现的关键一步是

// 设备检测和初始化
intfd =open("/dev/i2c-1", O_RDWR);
ioctl(fd, I2C_SLAVE,0x6A); // 设置传感器地址
// 初始化驱动上下文
stmdev_ctx_tdev_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适配器驱动
// 在内核中的实现大致如下：
staticlongi2c_dev_ioctl(structfile*file, unsignedintcmd, unsignedlongarg)
{
 switch(cmd) {
 caseI2C_SLAVE:
   // 设置从设备地址到i2c_client结构体
    client->addr = arg;
   break;
  }
}

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

stmdev_ctx_t dev_ctx;

结构体的定义：

typedefstruct{
 int32_t(*write_reg)(void*handle,uint8_treg,constuint8_t*buf,uint16_tlen);
 int32_t(*read_reg)(void*handle,uint8_treg,uint8_t*buf,uint16_tlen);
 void*handle;
 void(*mdelay)(uint32_tmillisec);
}stmdev_ctx_t;
//write_reg：函数指针，指向寄存器写函数
//read_reg：函数指针，指向寄存器读函数
//handle：泛型指针，用于传递平台特定数据
//mdelay：函数指针，指向延迟函数（可选）

我们的设计模式是将IIC操作统一成抽象的接口，即上文中实现的rpi的IIC驱动层实现。这样就可以在不同平台中复用相同业务逻辑的代码，只需要移植驱动层函数即可，所以在rpi驱动实现中，保持了和ST一样的参数。

// ST驱动期望的函数签名
int32_t(*write_reg)(void*handle,uint8_treg,constuint8_t*buf,uint16_tlen);
int32_t(*read_reg)(void*handle,uint8_treg,uint8_t*buf,uint16_tlen);
// 我们的平台实现必须匹配这个签名
staticint32_trpi_i2c_write(void*handle,uint8_treg,constuint8_t*buf,uint16_tlen);
staticint32_trpi_i2c_read(void*handle,uint8_treg,uint8_t*buf,uint16_tlen);

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

传感器的初始化操作

实现了寄存器的访问后，应用层上编写逻辑就和STM32这类MCU相差不大，就是封装函数在写入即可，具体封装实现逻辑可以在ST中文论坛获取。

// 设备检测和初始化
intfd =open("/dev/i2c-1", O_RDWR);
ioctl(fd, I2C_SLAVE,0x6A); // 设置传感器地址
// 初始化驱动上下文
stmdev_ctx_tdev_ctx;
dev_ctx.write_reg = rpi_i2c_write;
dev_ctx.read_reg = rpi_i2c_read;
dev_ctx.handle = &fd;
// 验证设备ID
uint8_twho_am_i;
lsm6dsv320x_device_id_get(&dev_ctx, &who_am_i);
printf("设备ID: 0x%02X
", 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, floatq[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(inti =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(floatq[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

执行效果如上。

使用MEMS Studio训练数据并使用MLC功能进行识别

MLC（Machine Learning Core）功能可以将一些跑在MCU上的算法（如TinyML）在传感器端实现，从而实现MCU休眠时传感器可以持续检测某一特定动作组，这个学习处理的过程依靠决策逻辑树构成，具体的信息可以参考ST官网资料。

链接：https://www.st.com/content/st_com/en/MEMS-Sensors-Ecosystem-for-Machine-Learning.html

本文主要讲解如何在MEMS Studio上进行MLC过程。首先选择我们使用的板卡，具体配置过程可以参考前边的内容。

配置完传感器之后，使用MEMS Studio的数据采集功能，分别采集两种不同动作的数据，每个动作保存3组。记得打上时间戳，后面有用。

保存好的数据如下图：

shake和standby分别对应抖动时候和静止板卡时候的数据，用户可以根据自己不同的情况多定制几种不同的功能。

在电机advance feature里面的MLC选项卡，分别导入我们的数据，注意是分别导入。

我们先点击browse，将我们的数据加载进选项框中，然后再右面输入栏中输入我们的数据标签，然后再点击load。这样我们对于某一个动作的数据集就会被加载进去了，不要一股脑的选中所有数据全部加载进去。

加载完数据之后，我们开始配置窗口长度以及量程，配置的选项可以参考我们在初始化时侯的选项，每一项对应着选择就好。

接着，我们点击下面的AREF generation，配置我们的滤波器（filters），这个滤波器是重要的，可以对我们的原始信号就行处理。

滤波器一般会配置每个轴，可以选择平均值滤波，高通低通滤波器等，一般选择平均值滤波可以平滑扰动信号。

滤波器配置完成之后，我们就可以选择我们的特征数据（features）了，MLC的特征数据选择可以选择某几个统计学特征，如：

MEAN：均值，表示在给定的时间窗口内计算的传感器数据的平均值。

VARIANCE：方差，表示数据的分散程度，反映数据在给定时间窗口内的变化。

ENERGY（能量）：计算信号在给定时间窗口内的总能量。它反映了信号强度，通常用于识别较强或较弱的活动。

PEAK_TO_PEAK（峰峰值）：峰峰值特征计算的是信号在给定时间窗口内的最大值和最小值之间的差异。这个特征可以帮助检测信号的最大振幅。

上述选项配置完成后，我们确定生成设备树arrf文件。

然后点击“Generate Decision Tree”按钮以生成决策树。这个按钮基于之前通过ARFF文件加载的数据和特征生成了一个分类模型（决策树），用于识别不同的动作或状态。

这个界面有一个关键的参数即Kappa统计量，它是一种用于衡量分类器在考虑偶然因素时的准确性度量。这里显示为0.307479，虽然分类准确度高，但Kappa值可能表明结果受到数据分布或样本量的影响。

● 0.81 - 1.00：几乎完美的分类一致性。

● 0.61 - 0.80：很好的分类一致性。

● 0.41 - 0.60：中等分类一致性。

● 0.21 - 0.40：较低的分类一致性。

● 0.00 - 0.20：几乎没有一致性（接近随机猜测）。

我们可以查看决策树模型：

这个模型清晰的展示了不同行为执行的动作，还可以查看模型输出信息。

还记得之前我们说的录log时候要打上时间戳吗？我们在debug界面中导入我们录制的log数据，导入传感器配置文件。

耐心等待一会加载完成，就可以在我们刚刚训练好的模型上面运行记录的log数据，并查看分类效果，类似我们在炼丹时候的训练集和验证集，来验证我们模型的识别效果。

由于刚才我们的MLC模型只训练了两种状态，所以MLC_OUT的输出只有0X00和0XFF两种状态。

总的来说，MEMS Studio给我们提供了一套非常完成且简单易用的传感器解决方案，方便用户来评估功能以及分析数据，无代码开箱即用的设计大大缩减了开发进程，并且支持非常多的传感器。

另外，ST提供的跨平台开发驱动包也很好用，类似Linux驱动的设计框架可以很方便的让我们把驱动移植到各个平台中使用，而不仅仅是限制于MCU平台。

ST工程师总结：

短笛君这篇对LSM6DSV320X的测评精准聚焦于ST工具链的完整性与易用性，评测视角非常地全面，过程也很细致！从LSM6DSV32X延伸到开发工具，再落到实际开发体验，形成了从硬件到产品的完整分析链条。确实，ST广受赞誉的生态系统提供完善易用的开发工具套件和驱动代码，可以很方便的实现跨平台的移植和开发调试。对于MLC，ST的MEMS STUDIO可以大幅度减少开发人员的工作量，进一步优化产品开发周期

欢迎关注ST中文论坛了解更多不同产品系列板块的测评活动。

文章所有图片来源于：ST中文论坛