1. 为什么选LSM6D3TR-C和STM32C5这套组合
做惯性测量类的嵌入式项目,选传感器和主控从来不是拍脑袋的事。我手头这块STM32C5开发板是最近才拿到的样片,ST的C系列主打的是低功耗和性价比,Cortex-M33内核,主频能跑到250MHz,资源不算夸张但很均衡。既然要熟悉这颗MCU,顺手把IMU也换成ST自家新一代的LSM6D3TR-C,一套流程走下来,既能摸清MCU的HAL库和时钟树,又能把加速度计和陀螺仪的外设驱动练一遍。
先说LSM6D3TR-C。这颗芯片是LSM6DS3的升级版,六轴IMU,三轴加速度计加三轴陀螺仪,封装是2.5mm x 3mm的LGA,体积很小。它的陀螺仪满量程可选125/250/500/1000/2000dps,加速度计满量程可选2/4/8/16g。通信接口支持I2C和SPI,I2C地址可以通过SA0引脚切换,默认是0x6A。这颗芯片还有一个比较实用的特性是内置了智能FIFO和不少运动识别功能,比如计步器、倾角检测、敲击检测,功耗控制做得不错,正常测量模式下陀螺仪加加速度计全开,也就零点几毫安的电流。
再回头看STM32C5这颗MCU。它属于STM32C5系列,ARM Cortex-M33内核带FPU和DSP指令,主频250MHz,片上Flash最高512KB,SRAM 96KB。这个配置在ST的产品线里属于中端偏入门的位置,但它有个很吸引人的点是价格。对于成本敏感的量产项目,比如智能穿戴、工业手持设备、简单的运动监测模块,用这个组合能把BOM成本压下来不少。Cortex-M33内核跑HAL库完全没压力,甚至上RTOS也绰绰有余。
之所以第一期先做轮询方式读陀螺仪,而不是直接上中断或者DMA,原因是先要把最基础的外设链路打通。轮询方式逻辑最简单,不容易出错,适合验证硬件连接、I2C通信和寄存器读写是否正确。等这套基础跑稳了,后面再上FIFO、中断、DMP之类的功能,出问题时也容易定位是硬件问题还是软件逻辑问题。
在实际项目里,轮询读传感器确实不是最优方案,因为CPU会被占用,无法在读取期间做别的事,而且数据读取频率受限于主循环周期。但作为入门阶段的第一步,或者作为快速验证硬件是否正常的临时方案,轮询是最可靠的。这篇就把整个流程拆开讲一遍,从硬件连接、工程配置、寄存器初始化,到具体的轮询读取代码和数据处理,一步不落。
2. 硬件连接与工程搭建的几个容易踩的细节
2.1 引脚分配与接线方式
我使用的是STM32C5系列的标准开发板,具体型号是NUCLEO-C5系列,板载ST-LINK,调试和供电都很方便。LSM6D3TR-C模块我这里用的是常见的六轴IMU小模块,淘宝上很多,板子已经把I2C上拉电阻和去耦电容布好了,直接飞线接就行。
接线方式如下:
| LSM6D3TR-C模块引脚 | STM32C5引脚 | 说明 |
|---|---|---|
| VCC | 3.3V | 模块供电,注意不要接5V,芯片绝对最大额定电压只有3.6V |
| GND | GND | 共地 |
| SCL | PB8(I2C1_SCL) | 时钟线 |
| SDA | PB9(I2C1_SDA) | 数据线 |
| SA0 | GND | 拉低,I2C地址为0x6A |
这里有两个细节需要提醒。
第一,SA0引脚决定I2C地址。SA0接GND时地址是0x6A(7位地址),接VCC时地址是0x6B。如果读出来的设备ID不对,第一个要检查的就是这个引脚的接法。
第二,模块上的上拉电阻。市面上部分IMU模块没有板载上拉电阻,需要自己在SCL和SDA上各加一个4.7kΩ的上拉电阻到3.3V。我用的这个模块自带10kΩ上拉,实测I2C波形OK,但如果你的模块上没有,记得补上,否则通信时好时坏非常诡异。
2.2 STM32CubeMX工程配置要点
STM32C5系列的开发工具链和老的F1/G0系列基本一致,用STM32CubeMX生成初始化代码,IDE用Keil MDK或者IAR都行,我这里用的是STM32CubeIDE,集成了编译调试和代码生成,不用来回切换工具。
CubeMX里需要配置的部分如下。
RCC配置:选择HSE外部高速晶振作为时钟源,HCLK设为250MHz。STM32C5内部的HSI精度一般,做IMU这类对时序精度有要求的应用,建议优先使用外部晶振,保证I2C通信时钟准确。
I2C1配置:
- I2C1模式选择I2C
- 基本参数保持默认,I2C速度模式选Fast Mode,时钟设为400kHz
- 关闭I2C的Analog Noise Filter?保持默认就行,如果通信不稳定再调整
GPIO配置:
- PB8和PB9会自动被配置为I2C1的复用功能,一般不需要手动改
- 如果CubeMX没有自动识别出复用功能,手动把PB8、PB9设置为AF4模式
USART2配置:用于调试信息输出,异步模式,115200-8-N-1。通过ST-LINK的虚拟串口打印传感器数据,方便在PC端用串口助手查看。
配置完成后直接生成代码。注意CubeMX生成的是初始化代码,I2C的读写函数在stm32c5xx_hal_i2c.c里,驱动层代码需要自己写。
2.3 工程结构规划
为了后面加功能方便,我建议把传感器驱动单独拆成一个模块。这次的工程结构大致是这样的:
Core/ Inc/ main.h lsm6d3tr.h // 传感器寄存器定义和函数声明 Src/ main.c lsm6d3tr.c // 传感器驱动实现 Drivers/ STM32C5xx_HAL_Driver/ // HAL库驱动模块和业务逻辑分开,后面如果要切换到DMA或者中断模式,只需要改驱动内部实现,主函数的调用逻辑不用大改。这个习惯从第一版代码就要养成,否则项目一大,文件间的依赖关系会直接变成意大利面条。
3. LSM6D3TR-C的寄存器地图:从芯片手册到代码的翻译过程
3.1 芯片手册怎么看
拿到一颗新传感器,第一件事不是写代码,而是把datasheet里的寄存器地图过一遍。LSM6D3TR-C的寄存器不算多,总共两三百个字节的空间,但实际用到的就那么几个。我习惯把关键寄存器的地址和功能抄到自己的笔记里,写代码时随手能翻到,不用每次打开PDF。
这颗芯片的寄存器寻址方式是8位寄存器地址加8位数据。I2C的写入格式是先发送设备地址(7位地址加R/W位),然后发送寄存器地址,再发送要写入的数据。读取格式是先发送设备地址加写位,发送寄存器地址,然后重新发送设备地址加读位,再连续读取数据。HAL库的HAL_I2C_Mem_Write和HAL_I2C_Mem_Read函数封装的就是这个过程,寄存器地址作为Mem_Address参数传入。
3.2 设备ID确认:0x6A还是别的值
第一步永远是读WHO_AM_I寄存器,地址是0x0F。LSM6D3TR-C的WHO_AM_I默认值是0x69。如果读出来是这个值,说明I2C通信正常,芯片正常工作。如果不是,一个可能是接线错误或者I2C地址不对,另一个可能是芯片进入了低功耗模式或者硬件出了问题。
这里要注意,WHO_AM_I寄存器在芯片处于睡眠模式时也能读取。所以如果连这个寄存器的值都读不对,问题几乎可以确定出在硬件连接或者I2C配置上,而不是传感器配置上。
3.3 陀螺仪和加速度计的量程配置
加速度计的量程配置在CTRL1_XL寄存器(0x10),陀螺仪的量程配置在CTRL2_G寄存器(0x11)。这两个寄存器的低4位是ODR(输出数据速率)配置,高几位是量程配置。
CTRL1_XL寄存器位定义:
| 位 | 名称 | 功能 |
|---|---|---|
| 7 | ODR_XL[3] | 加速度计输出数据速率高四位 |
| 6-4 | FS_XL[2:0] | 加速度计量程选择:000=±2g,001=±16g,010=±4g,011=±8g |
| 3-0 | ODR_XL[3:0] | 加速度计输出数据速率低四位,详细映射见手册Table 22 |
CTRL2_G寄存器位定义:
| 位 | 名称 | 功能 |
|---|---|---|
| 7-4 | ODR_G[3:0] | 陀螺仪输出数据速率 |
| 3-2 | FS_G[1:0] | 陀螺仪满量程:00=±250dps,01=±500dps,10=±1000dps,11=±2000dps |
| 1 | 0 | 保留 |
| 0 | 0 | 保留 |
我这次常用的配置是加速度计±2g,陀螺仪±2000dps,ODR都是104Hz。这个组合适合一般的运动监测场景,量程够大不容易饱和。ODR选择104Hz而不是更高的208Hz或者416Hz,是因为样机验证阶段不需要太高的采样率,数据量小一点,串口打印和上位机分析都方便。
ODR配置有一个概念容易被忽略:陀螺仪和加速度计的ODR是独立配置的,可以不一样。比如加速度计用416Hz做震动检测,陀螺仪用52Hz做姿态变化监测,在低功耗场景里很常见。后面二期讲到中断和FIFO时这个特性会很关键。
CTRL1_XL和CTRL2_G的配置值分别是:
// 加速度计:ODR=104Hz,±2g // ODR_XL[3:0]=0100,FS_XL[2:0]=000 #define LSM6D3TR_CTRL1_XL_ODR_104HZ_FS_2G 0x40 // 陀螺仪:ODR=104Hz,±2000dps // ODR_G[3:0]=0100,FS_G[1:0]=11 #define LSM6D3TR_CTRL2_G_ODR_104HZ_FS_2000DPS 0x4C这两个值可以直接通过查手册里的表格得到,比如陀螺仪ODR 104Hz对应二进制的0100,满量程2000dps在FS_G[1:0]位里是11,组合起来就是0x4C。
3.4 关闭内置数字滤波的坑
LSM6D3TR-C内置了一套数字滤波链路,包括高通滤波、低通滤波,还有一组看似方便的嵌入式功能。开启后CPU负担小,但调试时容易让人抓狂——你不知道当前读到的数据是原始值还是经过滤波后的值。
我的建议是第一版代码里先关闭所有数字滤波,拿到原始的ADC值再说。这些滤波功能放在CTRL3_C寄存器(0x12)里,其中BDU位(Bit 6)要特别提一下。
BDU(Block Data Update)位设为1时,加速度计和陀螺仪的高字节和低字节数据会被锁定,直到两个字节都读完才更新。如果不设置这一位,在高字节和低字节的两次读取之间,数据可能被新值覆盖,导致算出来的结果完全错误。这个位对连续读取多字节数据的场景特别重要,建议从一开始就设为1。
CTRL3_C寄存器还有一个IF_INC位(Bit 2),默认是1,表示多字节读取时地址自动递增。做连续读取时这个位必须保持为1,否则每次读一个字节都要重新指定寄存器地址,效率会低很多。
初始化配置代码如下:
// CTRL3_C:BDU=1,IF_INC=1 // 0x04是IF_INC,0x40是BDU uint8_t ctrl3_c = 0x44; lsm6d3tr_write_reg(LSM6D3TR_CTRL3_C, ctrl3_c);这里还要提一下CTRL4_C寄存器里的I2C禁用位。如果将来要用SPI接口和这颗芯片通信,需要把CTRL4_C里的I2C_DISABLE位置1来关闭I2C接口。反过来,如果用I2C,这个位必须保持默认0。曾经有人不小心配置了这个位,结果I2C通信完全无响应,排查了半天才发现是寄存器配置的问题。
3.5 软复位与启动等待
CTRL3_C的Bit 0是SW_RESET,写入1会触发软件复位,所有寄存器恢复到默认值。这个功能很重要,尤其是代码经历了异常复位后,传感器可能处于一个未知的配置状态。初始化流程里应该先做一次软复位,等一段时间,然后再写入实际配置。
软复位后要等待多久?手册上没有明确写,但根据经验,等待50ms比较安全。这是我的初始化代码里加延时函数的由来。如果不加这个延时,复位还没完成就写入配置,配置可能会被复位覆盖掉,导致初始化不生效。
4. 轮询读取的完整实现:数据手册上的公式怎么落到代码里
4.1 初始化函数实现
初始化流程的代码实现如下:
uint8_t LSM6D3TR_Init(void) { uint8_t who_am_i = 0; // 1. 读取WHO_AM_I确认设备 if (LSM6D3TR_ReadReg(LSM6D3TR_WHO_AM_I, &who_am_i) != HAL_OK) { return 1; } if (who_am_i != LSM6D3TR_WHO_AM_I_VALUE) // 0x69 { return 2; } // 2. 软复位 uint8_t ctrl3_c = 0x01; LSM6D3TR_WriteReg(LSM6D3TR_CTRL3_C, ctrl3_c); HAL_Delay(50); // 3. 设置BDU和IF_INC ctrl3_c = 0x44; LSM6D3TR_WriteReg(LSM6D3TR_CTRL3_C, ctrl3_c); // 4. 配置加速度计和陀螺仪 LSM6D3TR_WriteReg(LSM6D3TR_CTRL1_XL, 0x40); // ODR=104Hz, ±2g LSM6D3TR_WriteReg(LSM6D3TR_CTRL2_G, 0x4C); // ODR=104Hz, ±2000dps HAL_Delay(20); // 等待传感器稳定 return 0; }注意WHO_AM_I的读取放在复位之前。这是因为WHO_AM_I寄存器在任何模式下都能读,先确认通信链路是否正常,再做后续配置,逻辑上是通的,排查问题也方便。
4.2 轮询读取的实现细节
LSM6D3TR-C的加速度计数据寄存器从0x28开始,陀螺仪数据寄存器从0x22开始。每个轴的数据占两个字节,低字节在前,高字节在后。所以连续读取陀螺仪X、Y、Z轴的原始数据,就是从0x22开始连续读6个字节。
具体的读取函数如下:
int16_t gyro_x_raw, gyro_y_raw, gyro_z_raw; uint8_t data[6]; HAL_I2C_Mem_Read(&hi2c1, LSM6D3TR_I2C_ADDR, LSM6D3TR_OUTX_L_G, I2C_MEMADD_SIZE_8BIT, data, 6, 100); gyro_x_raw = (int16_t)((data[1] << 8) | data[0]); gyro_y_raw = (int16_t)((data[3] << 8) | data[2]); gyro_z_raw = (int16_t)((data[5] << 8) | data[4]);实现这个读取逻辑时,有一个关键点是组合两个字节。因为寄存器里的数据是补码格式,而C语言里的int16_t恰好也是补码表示,所以直接左移+或运算,再强转成int16_t,就能得到有符号的原始ADC值。
有些人的代码里会先转成uint16_t再强制转换,这其实也可以,但要注意不要用在普通的int上。因为int在32位MCU上是32位的,左移8位后符号扩展行为不一样,容易算出错误结果。
下面这个宏定义是我用来做字节拼装的:
#define BYTE_TO_INT16(low, high) ((int16_t)(((uint16_t)(high) << 8) | (uint16_t)(low)))4.3 原始值到实际物理量的转换
读出来的原始ADC值怎么转成角速度?数据手册给了一个公式:
实际角速度 = 原始值 × 灵敏度
灵敏度取决于满量程配置:
| 满量程配置 | 灵敏度(mdps/LSB) | 换算系数(dps/LSB) |
|---|---|---|
| ±250dps | 8.75 | 0.00875 |
| ±500dps | 17.50 | 0.01750 |
| ±1000dps | 35.00 | 0.03500 |
| ±2000dps | 70.00 | 0.07000 |
按照我配置的±2000dps,转换公式是:
float gyro_x_dps = (float)gyro_x_raw * 0.070f; float gyro_y_dps = (float)gyro_y_raw * 0.070f; float gyro_z_dps = (float)gyro_z_raw * 0.070f;同理,加速度计的灵敏度:
| 满量程配置 | 灵敏度(mg/LSB) | 换算系数(g/LSB) |
|---|---|---|
| ±2g | 0.061 | 0.000061 |
| ±4g | 0.122 | 0.000122 |
| ±8g | 0.244 | 0.000244 |
| ±16g | 0.488 | 0.000488 |
如果用±2g,加速度转换公式就是原始值乘以0.000061得到g值。
浮点数运算在现代MCU上不是什么问题,STM32C5有FPU,单精度浮点运算是一个时钟周期的事。但如果用的是不带FPU的低端MCU,建议用定点数运算来代替浮点,或者用q15_t之类的定点格式,省下大量CPU时间。
4.4 主循环里的轮询逻辑
主函数里最核心的部分是主循环中的轮询逻辑:
int main(void) { HAL_Init(); SystemClock_Config(); MX_GPIO_Init(); MX_I2C1_Init(); MX_USART2_UART_Init(); if (LSM6D3TR_Init() != 0) { printf("LSM6D3TR init failed\r\n"); while(1); } printf("LSM6D3TR init OK\r\n"); while (1) { LSM6D3TR_ReadGyro(&gyro_x_raw, &gyro_y_raw, &gyro_z_raw); float gyro_x_dps = gyro_x_raw * 0.070f; float gyro_y_dps = gyro_y_raw * 0.070f; float gyro_z_dps = gyro_z_raw * 0.070f; printf("G_X:%.2f G_Y:%.2f G_Z:%.2f dps\r\n", gyro_x_dps, gyro_y_dps, gyro_z_dps); HAL_Delay(10); // 控制轮询频率 } }这里的HAL_Delay(10)其实就是把轮询频率限制在100Hz左右,和数据手册里配置的104Hz ODR基本对应。如果去掉这个延时不控制节奏,读取频率可能会超过ODR,导致连续两次读取到相同的数据,性能没有提升反而浪费CPU。
4.5 轮询还是中断:什么时候该切换
轮询方式的优点可以用一句话概括:简单直接,代码量小,逻辑清晰。但它最大的问题是阻塞——主循环在读取和打印数据的这段时间里,做不了任何其他任务。如果主程序还需要处理按键、通信、显示等事务,轮询就会成为整个系统的瓶颈。
一般我的选择经验是这样的:
- 如果传感器的采样率要求不超过100Hz,并且主循环的工作负载不大,轮询完全够用
- 如果采样率要求高(比如高频振动监测需要1kHz以上),或者系统里还要跑实时性要求高的通信任务,建议换中断方式(INT引脚触发)或者DMA方式(配合硬件FIFO)
- 如果做低功耗产品,CPU大部分时间要睡觉,那就必须用中断唤醒,轮询模式会直接把电池耗干
后面第二期我会写中断方式的实现,那才是真正可用于产品开发的姿势。
5. 调试中的意外情况与实际经验
5.1 串口输出的浮点数显示异常
用printf打印浮点数据时,如果连接上串口但显示的值全是0或者输出乱码,先检查一下编译器是否启用了浮点数打印支持。Keil MDK默认把printf的浮点格式精简掉了,需要在Options for Target的Linker页面里勾选Use MicroLIB,或者在C/C++页面把优化级别改成不优化浮点。STM32CubeIDE也有类似的设置,需要在工程属性里把--specs=float_printf加入链接器选项,否则printf无法正确处理%f格式。
5.2 I2C读回来全是0xFF
这是I2C通信最经典的故障表现。寄存器读回来全是0xFF,说明I2C总线上没有设备应答。排查顺序如下:
- 量一下SCL和SDA引脚电压,正常空闲状态应该是3.3V,如果有一个是低电平,说明上拉有问题或者总线被拉死
- 检查设备地址。我用的是
0x6A << 1还是0x6A?HAL库的I2C地址参数需要7位地址左移一位,变成8位地址。很多人在这里混淆。HAL库的HAL_I2C_Mem_Read函数第二个参数是DevAddress,这个参数需要左移一位。如果直接传0x6A,实际上发送的是0xD4作为设备地址,总线上的设备不会应答 - 确认SA0引脚电平状态
- 如果以上都排查了还是0xFF,用示波器抓一下I2C波形,看时序是否满足400kHz的要求
地址这个坑我印象太深了,不止一次看到群里有人卡在这里。HAL库的函数加不加左移,很多人各执一词,实际上ST的标准HAL库对I2C地址的处理方式一直都是:传入7位地址后,HAL内部会自动左移一位。但具体到某个库版本可能有差异,最稳妥的办法是直接看库源码里I2C_TransferConfig函数的实现。
5.3 陀螺仪数据噪声偏大
静态放置传感器时,读出来的角速度并不是0,而是在某个值附近跳动。这个现象完全正常,陀螺仪本身固有的零偏和噪声特性决定的。对于零偏,可以在初始化时采集100组静态数据做平均,得到一个零点偏移值,然后在运行时把每次读到的原始值减去这个偏移量。对于随机噪声,如果不是特别严重,可以用滑动平均或者低通滤波处理。
一个快速判断数据是否可信的方法是看Z轴的输出。把传感器平放在桌面上,Z轴朝上,此时Z轴的角速度应该接近0,X轴和Y轴也应该接近0。然后手动绕Z轴旋转传感器,Z轴的读数应该有明显变化,这样基本能确定传感器工作正常。
5.4 数据更新的时序问题
轮询读取时偶尔会遇到连续两次读取到相同值的情况。这不是芯片有问题,而是读取频率高于ODR导致的。比如你配置ODR为104Hz,但主循环里的读取频率达到了200Hz,在两次采样间隔之间读取,自然拿到的是上一次的采样结果。上一节提到的HAL_Delay,本质上就是在做频率匹配。
更优雅的解法是读STATUS寄存器(0x1E)的XLDA和GDA位,这两个位分别表示加速度计和陀螺仪是否有新数据。先读STATUS寄存器,如果GDA位为1再读陀螺仪数据,保证每次读到的都是新数据,不浪费I2C带宽。这个做法其实就是中断的轮询版本,只是省掉了中断引脚和外部中断配置。
uint8_t status; HAL_I2C_Mem_Read(&hi2c1, LSM6D3TR_I2C_ADDR, LSM6D3TR_STATUS_REG, I2C_MEMADD_SIZE_8BIT, &status, 1, 100); if (status & 0x02) // GDA位 { HAL_I2C_Mem_Read(&hi2c1, LSM6D3TR_I2C_ADDR, LSM6D3TR_OUTX_L_G, I2C_MEMADD_SIZE_8BIT, data, 6, 100); // 处理数据 }注意STATUS寄存器里GDA位是Bit 1,XLDA位是Bit 0。想同时检查两个轴数据是否都更新了,用(status & 0x03) == 0x03这种写法。
5.5 CubeMX生成的时钟树导致I2C通信异常
这个坑可能很少有人遇到,但我确实踩过:CubeMX默认生成时钟配置,如果开启HSE失败会回退到HSI,导致HCLK降低,I2C的时序计算也会变化。在调试初期,如果I2C通信不稳定,可以把I2C的时钟模式暂时降到Standard Mode 100kHz试试,排除高速模式下的信号质量问题。
还有一个和时钟相关的注意点,就是在不用的外设上保持禁用状态。CubeMX生成代码时会把没用到的外设时钟自动关闭,这是正常优化。但有时候GPIO的某个引脚一开始被配置成了其他功能,后来又改成I2C了,CubeMX可能没有完全清理掉之前的配置,导致引脚复用不正确。遇到奇怪问题的时候,重新生成一次工程,有时候就自己好了。
6. 原始数据可视化与半成品分析
6.1 用串口数据简单画波形
调试IMU光看串口数字是不够直观的,尤其是数据量大之后,眼睛根本看不过来。这里分享一个最简单的方法:用ST的免费工具Unicleo-GUI,它可以直接从虚拟串口读取数据并绘制实时波形。不过Unicleo-GUI对数据格式有要求,需要按照它的格式输出文本,比如[GYRO_X] 0.12 [GYRO_Y] -0.03 [GYRO_Z] 0.01。
不想折腾上位机的话,也可以用Python脚本读取串口数据,用matplotlib画图,效果一样好。Python的pyserial库几行代码就能搞定:
import serial import matplotlib.pyplot as plt ser = serial.Serial('COM3', 115200) xs, ys, zs = [], [], [] for i in range(500): line = ser.readline().decode().strip() # 解析数据 if 'G_X:' in line: parts = line.split() x = float(parts[1].replace('G_X:', '').replace('dps', '')) y = float(parts[3].replace('G_Y:', '').replace('dps', '')) z = float(parts[5].replace('G_Z:', '').replace('dps', '')) xs.append(x); ys.append(y); zs.append(z) plt.plot(xs, label='X') plt.plot(ys, label='Y') plt.plot(zs, label='Z') plt.legend() plt.show()调试IMU时,图形的价值远超数字。数据波形可以直观地反映出传感器是否正常,比如静止时输出是否平稳,拿起来绕某个轴旋转时对应轴的曲线是否有明显变化,这些用肉眼观察波形远比盯着串口数字容易判断。
6.2 从陀螺仪数据算角度
轮询打通之后,很多人第一件想做的事就是用陀螺仪积分算角度。原理不复杂:角度等于角速度对时间的积分,离散化之后就是不断累加角速度乘以采样周期。
float angle_z = 0.0f; float dt = 0.01f; // 假设轮询周期10ms // 每次读到新数据 gyro_z_dps = gyro_z_raw * 0.070f; angle_z += gyro_z_dps * dt;算法很简单,但实际用起来会发现一个问题:积分漂移。陀螺仪的零偏误差虽然很小,但积分是累积过程,几秒钟可能看不出来,一两分钟后角度已经明显偏离真实值。这就是为什么实际产品里陀螺仪数据不会单独用来算角度,而是要和加速度计数据融合,用互补滤波或者卡尔曼滤波。这个内容也会放到后面的文章里展开。
6.3 数据冻结时的排查手段
调试过程中如果发现数据完全不变,像是“冻住”了一样,先别急着怀疑传感器坏了。用调试器读一下传感器当前配置状态,看看ODR是否配置正确。如果只有加速度计数据更新而陀螺仪不更新,可能是CTRL2_G没配好,陀螺仪还在睡眠模式。如果两者都不更新,很可能传感器没退出睡眠模式——检查CTRL1_XL和CTRL2_G的ODR字段是否为0,这是传感器默认的睡眠状态。
另外,如果使用了寄存器地址自增,也要确认在连续读取时地址指针是否正确。IF_INC位如果意外被清零,连续读出来的数据实际上都是第一个寄存器的值,表现为XYZ三轴数据完全相同。
7. 实际操作中形成的一个不算技巧的技巧
整个调试下来,最想分享的反而不是某个具体寄存器的配置,而是一个习惯:在调试IMU这类器件时,把所有的寄存器读写都封装成带日志的版本,一旦通信异常可以快速定位是哪个寄存器出了问题。我的做法是在调试阶段给驱动层加一个全局开关,开启后每个寄存器读写都会打印寄存器地址和读写的数据,等系统跑稳定后再把这个开关关掉。这个习惯帮我省下了大量排查时间,尤其是做I2C这种底层通信时,打印信息几乎等于逻辑分析仪。
另外一个心得是:认真读一遍芯片手册,比在网上找十篇现成代码有用。LSM6D3TR-C的数据手册虽然比不过那些上千页的复杂SoC手册,但里面每个寄存器的每一位定义、每个模式的时序要求,都是工程师花了大量时间验证过的。照着手册写出来的代码,出错概率远低于照搬网上的代码。
第一期就先到这里。截止到现在,LSM6D3TR-C的轮询读取已经验证通过,硬件链路、I2C通信、寄存器配置和数据转换这套基础都没问题。接下来的二期计划做中断方式读取传感器数据,顺便打通数据就绪信号和外部中断的配合;三期考虑做FIFO的使用和DMA传输,把CPU负载进一步降下来。考虑到这颗芯片还自带计步、倾角检测等嵌入式功能,后续也可以单独开一篇讲讲这些特色功能的使用。