陀螺仪LSM6DSV16X与AI集成.14--上报匿名上位机
- 概述
- 视频教学
- 样品申请
- 源码下载
- 硬件准备
- 上位机通讯
- 陀螺仪工作方式
- 欧拉角数据的转换
- 数据帧填充
- 校验和计算
- 数据发送
- 演示
- 开启INT中断
- 中断读取传感器数据
- 主程序
- 演示
概述
本文介绍了如何将 LSM6DSV16X 传感器的姿态数据通过匿名通信协议上报到上位机。通过获取传感器的四元数数据,并将其转换为欧拉角(Roll、Pitch、Yaw),然后按照协议格式化数据帧并通过串口传输到上位机。上位机接收后可进行实时显示和分析。这种方式广泛应用于姿态检测和控制系统,特别适合无人机、机器人等需要姿态控制的场景。
最近在弄ST的课程,需要样片的可以加群申请:615061293 。
视频教学
https://www.bilibili.com/video/BV1ic1fYjEj2/
陀螺仪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 */
}
需要注意优化等级。