陀螺仪LSM6DSV16X与AI集成.14--上报匿名上位机
概述
本文介绍了如何将 LSM6DSV16X 传感器的姿态数据通过匿名通信协议上报到上位机。通过获取传感器的四元数数据，并将其转换为欧拉角（Roll、Pitch、Yaw），然后按照协议格式化数据帧并通过串口传输到上位机。上位机接收后可进行实时显示和分析。这种方式广泛应用于姿态检测和控制系统，特别适合无人机、机器人等需要姿态控制的场景。
最近在弄ST的课程，需要样片的可以加群申请：615061293 。
视频教学
https://www.bilibili.com/video/BV1ic1fYjEj2/
<iframe allowfullscreen="true" data-mediaembed="bilibili" frameborder="0" id="OBeS8iox-1728193008481" src="https://player.bilibili.com/player.html?aid=113256458881074"></iframe>陀螺仪LSM6DSV16X与AI集成(14)----上报匿名上位机
样品申请
https://www.wjx.top/vm/OhcKxJk.aspx#
源码下载
硬件准备
首先需要准备一个开发板，这里我准备的是自己绘制的开发板，需要的可以进行申请。
主控为STM32H503CB，陀螺仪为LSM6DSV16X，磁力计为LIS2MDL。
上位机通讯
这里使用的是匿名助手的上位机
https://gitee.com/anotc/AnoAssistant
有专门的通讯协议
串口通讯协议格式如下所示，需要注意传输为小端模式传输。
对应的源地址和目标地址分别为0xFD和0xFE。
我们只需要上报加速度和陀螺仪数据，所以功能码为0x01，数据长度为0x0D，需要主要为小端模式传输。
陀螺仪工作方式
加速度计测量线性加速度，而陀螺仪测量角旋转。为此，他们测量了科里奥利效应产生的力。
陀螺仪是一种运动传感器，能够感测物体在一轴或多轴上的旋转角速度。它能够精确地感测自由空间中复杂的移动动作，因此成为追踪物体移动方位和旋转动作的必要设备。与加速计和电子罗盘不同，陀螺仪不需要依赖外部力量（如重力或磁场），可以自主地发挥其功能。因此，从理论上讲，只使用陀螺仪就可以完成姿态导航的任务。
陀螺仪的每个通道检测一个轴的旋转。也就是说陀螺仪通过测量自身的旋转状态，判断出设备当前运动状态，是向前、向后、向上、向下、向左还是向右呢，是加速（角速度）还是减速（角速度）呢，都可以实现，但是要判断出设备的方位（东西南北），陀螺仪就没有办法。
MEMS陀螺仪主要利用科里奥利力（旋转物体在有径向运动时所受到的切向力）原理，公开的微机械陀螺仪均采用振动物体传感角速度的概念，利用振动来诱导和探测科里奥利力。
MEMS陀螺仪的核心是一个微加工机械单元，在设计上按照一个音叉机制共振运动，通过科里奥利力原理把角速率转换成一个特定感测结构的位移。
两个相同的质量块以方向相反的做水平震荡。当外部施加一个角速率，就会出现一个科氏力，力的方向垂直于质量运动方向，如垂直方向箭头所示。产生的科氏力使感测质量发生位移，位移大小与所施加的角速率大小成正比，科氏力引起的电容变化即可计算出角速率大小。
科里奥利效应指出，当质量 (m) 以速度 (v) 沿特定方向移动并施加外部角速率 (Ω)（红色箭头）时，科里奥利效应会产生一个力（黄色箭头），导致质量垂直移动。该位移的值与应用的角速率直接相关。
变量定义。
/* USER CODE BEGIN 2 */
float Yaw,Pitch,Roll; //偏航角，俯仰角，翻滚角
int16_t acc_int16[3] ={0,0,0};
int16_t gyr_int16[3] ={0,0,0};
float acc[3] = {0};
float gyr[3] = {0};
uint8_t sumcheck = 0;
uint8_t addcheck = 0;
int16_t angular_rate_raw[3]={0,0,0}; //pitch,roll,yaw
uint8_t data_angular_rate_raw[16]={0};
data_angular_rate_raw[0]=0xAB;//帧头
data_angular_rate_raw[1]=0xFD;//源地址
data_angular_rate_raw[2]=0xFE;//目标地址
data_angular_rate_raw[3]=0x03;//功能码ID
data_angular_rate_raw[4]=0x08;//数据长度LEN
data_angular_rate_raw[5]=0x00;//数据长度LEN 8
data_angular_rate_raw[6]=0x01;//mode = 1
data_angular_rate_raw[13]=0x00;//FUSION _STA：融合状态
/* USER CODE END 2 */
欧拉角数据的转换
将欧拉角 Roll、Pitch、Yaw 乘以 100，以保留两位小数的精度。并且为 Yaw 数据减去了 18000，这通常是为了将欧拉角的范围转换为 [-18000, 18000] 这样方便传输的范围。
Roll=euler[2];
Pitch=euler[1];
Yaw=euler[0];
int16_t Roll_int16;
int16_t Pitch_int16;
int16_t Yaw_int16;
Roll_int16 = (int16_t)(Roll);
Pitch_int16 = (int16_t)(Pitch);
Yaw_int16 = (int16_t)(Yaw);
// 将欧拉角数据转换为 int16_t 格式并填充到数据帧中
Roll_int16=Roll_int16*100;// 放大100倍以保留小数位
Pitch_int16=Pitch_int16*100;
Yaw_int16=Yaw_int16*100-18000;
数据帧填充
将转换后的 Roll_int16、Pitch_int16 和 Yaw_int16 数据依次填充到数据帧的相应位置。
data_angular_rate_raw[7] = Roll_int16 >> 8; // Roll 高字节
data_angular_rate_raw[8] = Roll_int16 & 0xFF; // Roll 低字节
data_angular_rate_raw[9] = Pitch_int16 >> 8; // Pitch 高字节
data_angular_rate_raw[10] = Pitch_int16 & 0xFF;// Pitch 低字节
data_angular_rate_raw[11] = Yaw_int16 >> 8; // Yaw 高字节
data_angular_rate_raw[12] = Yaw_int16 & 0xFF; // Yaw 低字节
校验和计算
使用了双层循环求和来计算校验和，这是一种累加和的方法，确保帧数据的完整性。
data_angular_rate_raw[13]=0;
sumcheck = 0;
addcheck = 0;
for(uint16_t i = 0; i < 14; i++) {
sumcheck += data_angular_rate_raw[i]; // 按字节累加计算 sumcheck
addcheck += sumcheck; // 累加 sumcheck 生成 addcheck
}
data_angular_rate_raw[14] = sumcheck; // 将校验和写入帧
data_angular_rate_raw[15] = addcheck; // 写入最终的累加值
数据发送
通过 UART 发送封装好的 16 字节数据帧。
HAL_UART_Transmit(&huart1 , (uint8_t *)&data_angular_rate_raw, 16, 0xFFFF);
演示
LSM6DSV16X 特性涉及到的是一种低功耗的传感器融合算法（Sensor Fusion Low Power, SFLP）.
低功耗传感器融合（SFLP）算法：
该算法旨在以节能的方式结合加速度计和陀螺仪的数据。传感器融合算法通过结合不同传感器的优势，提供更准确、可靠的数据。
6轴游戏旋转向量：
SFLP算法能够生成游戏旋转向量。这种向量是一种表示设备在空间中方向的数据，特别适用于游戏和增强现实应用，这些应用中理解设备的方向和运动非常关键。
四元数表示法：
旋转向量以四元数的形式表示。四元数是一种编码3D旋转的方法，它避免了欧拉角等其他表示法的一些限制（如万向节锁）。一个四元数有四个分量（X, Y, Z 和 W），其中 X, Y, Z 代表向量部分，W 代表标量部分。
FIFO存储：
四元数的 X, Y, Z 分量存储在 LSM6DSV16X 的 FIFO（先进先出）缓冲区中。FIFO 缓冲区是一种数据存储方式，允许临时存储传感器数据。这对于有效管理数据流非常有用，特别是在数据处理可能不如数据收集那么快的系统中。
图片包含了关于 LSM6DSV16X 传感器的低功耗传感器融合（Sensor Fusion Low Power, SFLP）功能的说明。这里是对图片内容的解释：
SFLP 功能：
- SFLP 单元用于生成基于加速度计和陀螺仪数据处理的以下数据：
- 游戏旋转向量：以四元数形式表示设备的姿态。
- 重力向量：提供一个三维向量，表示重力方向。
- 陀螺仪偏差：提供一个三维向量，表示陀螺仪的偏差。
激活与重置： - 通过在 EMB_FUNC_EN_A（04h）嵌入式功能寄存器中设置 SFLP_GAME_EN 位为 1 来激活 SFLP 单元。
- 通过在 EMB_FUNC_INIT_A（66h）嵌入式功能寄存器中设置 SFLP_GAME_INIT 位为 1 来重置 SFLP 单元。
性能参数表：
表格展示了 SFLP 功能在不同情况下的性能，包括静态精度、低动态精度和高动态精度，以及校准时间和方向稳定时间。这些参数反映了传感器在不同运动状态下的精确度和响应速度。
开启INT中断
陀螺仪LSM6DSV16X的中断管脚接到了PB0，需要将PB0设置为中端口。
开启中断。
中断读取传感器数据
INT1_CTRL (0Dh) 是 LSM6DSV16X 传感器的中断控制寄存器，用于配置和启用 INT1 引脚的各种中断信号。该寄存器的每一位对应于不同的中断源，通过设置这些位可以启用或禁用相应的中断信号。
INT1_FIFO_TH (bit 3)：
● 启用 FIFO 阈值中断，将其路由到 INT1 引脚。当 FIFO 达到设定的阈值时触发该中断。默认值为 0（禁用）。
mian.c中定义变量。
/* USER CODE BEGIN 0 */
uint8_t fifo_flag = 0;
/* USER CODE END 0 */
mian.c中开启中断。
lsm6dsv16x_pin_int_route_t pin_int;
pin_int.fifo_th = PROPERTY_ENABLE;
lsm6dsv16x_pin_int1_route_set(&dev_ctx, &pin_int);
在stm32h5xx_it.c中添加回调函数引用。
/* USER CODE BEGIN 0 */
extern void HAL_GPIO_EXTI_Callback(uint16_t GPIO_Pin);
/* USER CODE END 0 */
处理PB0外部中断线0（EXTI Line0）的中断。
/**
* @brief This function handles EXTI Line0 interrupt.
*/
void EXTI0_IRQHandler(void)
{
/* USER CODE BEGIN EXTI0_IRQn 0 */
HAL_GPIO_EXTI_Callback(INT1_Pin);
/* USER CODE END EXTI0_IRQn 0 */
HAL_GPIO_EXTI_IRQHandler(INT1_Pin);
/* USER CODE BEGIN EXTI0_IRQn 1 */
/* USER CODE END EXTI0_IRQn 1 */
}
在main.c中添加回调函数的定义，检查中断是否由 GPIO_PIN_0引脚触发。
/* USER CODE BEGIN 4 */
void HAL_GPIO_EXTI_Callback(uint16_t GPIO_Pin){
if(GPIO_Pin == GPIO_PIN_0)
{
mlc_flag=1;
}
}
/* USER CODE END 4 */
主程序
/* Infinite loop */
/* USER CODE BEGIN WHILE */
while (1)
{
if(mlc_flag==1)
{
mlc_flag=0;
uint16_t num = 0;
/* Read watermark flag */
lsm6dsv16x_fifo_status_get(&dev_ctx, &fifo_status);
if (fifo_status.fifo_th == 1) {
num = fifo_status.fifo_level;
printf( "-- FIFO num %d \r\n", num);
while (num--) {
lsm6dsv16x_fifo_out_raw_t f_data;
int16_t *axis;
float quat[4];
float gravity_mg[3];
float gbias_mdps[3];
/* Read FIFO sensor value */
lsm6dsv16x_fifo_out_raw_get(&dev_ctx, &f_data);
switch (f_data.tag) {
// case LSM6DSV16X_SFLP_GYROSCOPE_BIAS_TAG:
// axis = (int16_t *)&f_data.data[0];
// gbias_mdps[0] = lsm6dsv16x_from_fs125_to_mdps(axis[0]);
// gbias_mdps[1] = lsm6dsv16x_from_fs125_to_mdps(axis[1]);
// gbias_mdps[2] = lsm6dsv16x_from_fs125_to_mdps(axis[2]);
// printf("GBIAS [mdps]:%4.2f\t%4.2f\t%4.2f\r\n",
// (double_t)gbias_mdps[0], (double_t)gbias_mdps[1], (double_t)gbias_mdps[2]);
// break;
// case LSM6DSV16X_SFLP_GRAVITY_VECTOR_TAG:
// axis = (int16_t *)&f_data.data[0];
// gravity_mg[0] = lsm6dsv16x_from_sflp_to_mg(axis[0]);
// gravity_mg[1] = lsm6dsv16x_from_sflp_to_mg(axis[1]);
// gravity_mg[2] = lsm6dsv16x_from_sflp_to_mg(axis[2]);
// printf("Gravity [mg]:%4.2f\t%4.2f\t%4.2f\r\n",
// (double_t)gravity_mg[0], (double_t)gravity_mg[1], (double_t)gravity_mg[2]);
// break;
case LSM6DSV16X_SFLP_GAME_ROTATION_VECTOR_TAG:
sflp2q(quat, (uint16_t *)&f_data.data[0]);
// printf("Game Rotation \tX: %2.3f\tY: %2.3f\tZ: %2.3f\tW: %2.3f\r\n",
// (double_t)quat[0], (double_t)quat[1], (double_t)quat[2], (double_t)quat[3]);
float sx=quat[1];
float sy=quat[2];
float sz=quat[0];
float sw=quat[3];
if (sw< 0.0f)
{
sx*=-1.0f;
sy*=-1.0f;
sz*=-1.0f;
sw*=-1.0f;
}
float sqx = sx * sx;
float sqy = sy * sy;
float sqz = sz * sz;
float euler[3];
euler[0] = -atan2f(2.0f* (sy*sw+sx*sz), 1.0f-2.0f*(sqy+sqx));
euler[1] = -atan2f(2.0f * (sx*sy+sz*sw),1.0f-2.0f*(sqx+sqz));
euler[2] = -asinf(2.0f* (sx*sw-sy*sz));
if (euler[0] <0.0f)
euler[0] +=2.0f*3.1415926;
for(uint8_t i=0; i<3; i++){
euler[i] = 57.29578 * (euler[i]);
}
printf("euler[0]=%f,euler[1]=%f,euler[2]=%f\n",euler[0],euler[1],euler[2]);
break;
default:
break;
}
}
}
}
/* USER CODE END WHILE */
/* USER CODE BEGIN 3 */
}
/* USER CODE END 3 */
}
需要注意优化等级。
演示
