陀螺仪LSM6DS3TR-C的简单使用

文章目录

一、前言
二、硬件
- 1.引脚说明
- 2.原理图
三、软件
- 1.IIC读写函数
- - 1.1 读函数
  - 1.2 写函数
- 2.初始化
- - 2.1 检测设备是否存在
  - 2.2 读取LSM6DS3TRC器件ID
  - 2.3 LSM6DS3TRC重启，重置寄存器
  - 2.5 LSM6DS3TRC设置块数据更新
  - 2.6 LSM6DS3TRC设置加速度计的数据采样率
  - 2.7 LSM6DS3TRC设置陀螺仪的数据采样率
  - 2.8 LSM6DS3TRC加速度计满量程选择
  - 2.9 LSM6DS3TRC陀螺仪全量程选择
  - 2.10 LSM6DS3TRC设置加速度计模拟链带宽
- 3.读取数据
- - 3.1 读取加速度计数据
  - 3.2 读取陀螺仪数据
- 4.姿态解算
- - 4.1 欧拉角
  - 4.2 四元数
  - 4.3 四元数姿态解算
  - - 4.3.1 四元素初始化（第一次解算时计算）
    - 4.3.2 四元数归一化
    - 4.3.3 提取四元素等效余弦矩阵中的重力分量
    - 4.3.4 四元数姿态融合算法--互补滤波法
    - 4.3.5 四元数换算得到欧拉角
四、测试结果
- 1. 静止状态
- 2. 倾斜状态
五、总结

一、前言

最近做的东西需要检测倾斜和物体移动，需要用到陀螺仪传感器，不过我没有选择MPU6050，因为立创上卖太贵了，要四五十块一颗，我在立创上选了一颗四五块的TI的芯片LSM6DS3TR-C，它是一款集成了三轴加速度计和三轴陀螺仪的MEMS（微电子机械系统）传感器。可以通过数字形式（I2C 或 SPI 接口）输出三轴加速度计和三轴陀螺仪等数据，效果也还不错。

在这里插入图片描述

二、硬件

1.引脚说明

在这里插入图片描述

接口介绍：
GND：电源负极
VCC：电源正极，3.3V~5V的电压
SCL：I2C串行时钟（SCL）/SPI串行端口时钟（SPC）
SDA：I2C串行数据（SDA）/SPI串行数据输入（SDI）/3线接口串行数据输出（SDO）
SDO：SPI 4线接口串行数据输出（SDO）/I2C设备地址的最低有效位（SA0）
CS：I2C/SPI模式选择（1：SPI空闲模式/I2C通信启用；0：SPI通信模式/I2C禁用）
INT1/2：中断
SDx：I2C串行时钟主机（MSCL）
SCx：I2C串行数据主机（MSCL）

在这里插入图片描述

注意：对于IIC的地址，可以通过SDO/SA0引脚修改。SDO/SA0引脚可以用来修改设备地址的最低有效位。如果SDO/SA0引脚连接到电源电压，LSb（最低有效位）为’1’（地址1101011b）；否则，如果SDO/SA0引脚连接到地线，LSb的值为’0’（地址1101010b）。

2.原理图

由上面的引脚说明知道，要使用IIC接口，需要将CS引脚接VCC，在使用IIC通讯模式的时候，SA0是用来控制IIC的地址位的。对应的IIC接口如下所示。主要使用的管脚为CS、SCL、SDA、SA0。

在这里插入图片描述

实物大致如下所示。想着到时候直接把CS和3V3焊接在一块，把SDO和GND焊接在一块，另外就是IIC接口和单片机用杜邦线连接就可以了。

在这里插入图片描述

三、软件

1.IIC读写函数

如下采用的IIC的通信方式，软件模拟IIC。时序就不在这里一一列举了，简单说一下读写函数。

1.1 读函数

/******************************************************************************** 函数名：LSM6DS3TRC_ReadCommand* 描述  ：对LSM6DS3TRC读取数据* 输入  ：uint8_t reg_addr, uint8_t *rev_data, uint8_t length* 输出  ：void* 调用  ：内部调用* 备注  ：*******************************************************************************/
void LSM6DS3TRC_ReadCommand(uint8_t reg_addr, uint8_t *rev_data, uint8_t length)
{	while(length){*rev_data++ = LSM6DS3TRC_ReadOneByte(reg_addr++);length--; }		
}/******************************************************************************** 函数名：LSM6DS3TRC_ReadOneByte* 描述  ：从LSM6DS3TRC指定地址处开始读取一个字节数据* 输入  ：reg_addr地址* 输出  ：读取的数据dat* 调用  ：* 备注  ：*******************************************************************************/
uint8_t LSM6DS3TRC_ReadOneByte(uint8_t reg_addr)
{uint8_t dat = 0;	IIC_Start();//发送起始信号IIC_Send_Byte((LSM6DS3TRC_I2CADDR<<1) | 0x00);//从设备地址    delay_syms(1);	if(IIC_Wait_Ack())	/* 检测设备的ACK应答 */{IIC_Stop();//产生一个停止条件	  			printf("error1\r\n");	}else{	}IIC_Send_Byte(reg_addr);//寄存器地址delay_syms(1);			if(IIC_Wait_Ack())	/* 检测设备的ACK应答 */{IIC_Stop();//产生一个停止条件	  			printf("error2\r\n");	}else{	}IIC_Start();//发送重复起始信号，准备读取数据IIC_Send_Byte((LSM6DS3TRC_I2CADDR<<1) | 0x01);//从设备地址（读取模式）delay_syms(1);	if(IIC_Wait_Ack())	/* 检测设备的ACK应答 */{IIC_Stop();//产生一个停止条件	  			printf("error3\r\n");	}else{	}dat = IIC_Read_Byte(0);IIC_Stop();//发送停止信号	return dat;		
}

1.2 写函数

/******************************************************************************** 函数名：LSM6DS3TRC_WriteCommand* 描述  ：往LSM6DS3TRC写入命令* 输入  ：uint8_t reg_addr, uint8_t *send_data, uint16_t length* 输出  ：void* 调用  ：内部调用* 备注  ：*******************************************************************************/
void LSM6DS3TRC_WriteCommand(uint8_t reg_addr, uint8_t *send_data, uint16_t length)
{IIC_Start();	delay_syms(10);	IIC_Send_Byte((LSM6DS3TRC_I2CADDR<<1) | 0x00);//发送设备地址	if(IIC_Wait_Ack())	/* 检测设备的ACK应答 */{IIC_Stop();//产生一个停止条件	  			printf("error1\r\n");					}else{	}	delay_syms(10);			IIC_Send_Byte(reg_addr);//发送寄存器地址	delay_syms(10);	if(IIC_Wait_Ack())	/* 检测设备的ACK应答 */{IIC_Stop();//产生一个停止条件	  			printf("error2\r\n");					}else{	}	delay_syms(10);		IIC_Send_Byte(*send_data);//发送数据		delay_syms(10);	if(IIC_Wait_Ack())	/* 检测设备的ACK应答 */{IIC_Stop();//产生一个停止条件	  			printf("error3\r\n");				}else{	}	delay_syms(10);	IIC_Stop();//产生一个停止条件	
}

2.初始化

2.1 检测设备是否存在

如上面的硬件所示，我的SA0是接到GND，也就是SDA[0]为0，配合上SAD[6:1]的110101，得到1101010，而最后一位的读写位，此时需要的是读也就是1，将1101010左移一位加上最后一位，最后得到11010101也就是D5H。

在这里插入图片描述

#define LSM6DS3TRC_I2CADDR 0x6A//SA0接GND，如果接的是VCC，则地址是0x6B/******************************************************************************** 函数名：IIC_CheckDevice* 描述  ：检测I2C总线设备，CPU向发送设备地址，然后读取设备应答来判断该设备是否存在* 输入  ：_Address：设备的I2C总线地址* 输出  ：返回值 0 表示正确， 返回1表示未探测到* 调用  ：* 备注  ：*******************************************************************************/
uint8_t IIC_CheckDevice(uint8_t _Address)
{uint8_t ucAck;if(IIC_SDA_IN_Read() && IIC_SCL_IN_Read()){IIC_Start();		/* 发送启动信号 */IIC_Send_Byte(_Address );ucAck = IIC_Wait_Ack();	/* 检测设备的ACK应答 */IIC_Stop();			/* 发送停止信号 */return ucAck;}return 1;	/* I2C总线异常 */
}/******************************************************************************** 函数名：LSM6DS3TRC_CheckOk* 描述  ：判断LSM6DS3TRC是否正常* 输入  ：void* 输出  ： 1 表示正常， 0 表示不正常* 调用  ：* 备注  ：*******************************************************************************/
uint8_t LSM6DS3TRC_CheckOk(void)
{if(IIC_CheckDevice((LSM6DS3TRC_I2CADDR <<1)|0x00) == 0){print("Device exist\r\n");return 1;}else{/* 失败后，切记发送I2C总线停止信号 */print("Device not exist\r\n");		IIC_Stop();return 0;}
}

2.2 读取LSM6DS3TRC器件ID

查芯片手册可知ID寄存器为0x0F。

在这里插入图片描述

//Who I am ID
#define LSM6DS3TRC_WHO_AM_I		0x0F/******************************************************************************** 函数名：LSM6DS3TRC_GetChipID* 描述  ：读取LSM6DS3TRC器件ID* 输入  ：void* 输出  ：返回值true表示0x6a，返回false表示不是0x6a* 调用  ：内部调用* 备注  ：*******************************************************************************/
bool LSM6DS3TRC_GetChipID(void)
{uint8_t buf = 0;LSM6DS3TRC_ReadCommand(LSM6DS3TRC_WHO_AM_I, &buf, 1);//Who I am IDprintf("buf 0x%02X\r\n",buf);		if (buf == 0x6a){printf("ID ok\r\n");	return true;}else{printf("ID error\r\n");	return false;}
}

2.3 LSM6DS3TRC重启，重置寄存器

BOOT：重新启动内存内容。默认值:0。(0:正常模式;1:重启内存内容)
SW_RESET：软件复位。默认值:0。(0:正常模式;1:复位装置) 该位被自动清除

在这里插入图片描述

#define LSM6DS3TRC_CTRL3_C		0x12/******************************************************************************** 函数名：LSM6DS3TRC_Reset* 描述  ：LSM6DS3TRC重启和重置寄存器* 输入  ：void* 输出  ：void* 调用  ：内部调用* 备注  ：*******************************************************************************/
void LSM6DS3TRC_Reset(void)
{uint8_t buf[1] = {0};//reboot modulesbuf[0] = 0x80;LSM6DS3TRC_WriteCommand(LSM6DS3TRC_CTRL3_C, buf, 1);//BOOT->1delay_syms(15);//reset registerLSM6DS3TRC_ReadCommand(LSM6DS3TRC_CTRL3_C, buf, 1);//读取SW_RESET状态buf[0] |= 0x01;LSM6DS3TRC_WriteCommand(LSM6DS3TRC_CTRL3_C, buf, 1);//将CTRL3_C寄存器的SW_RESET位设为1while (buf[0] & 0x01)LSM6DS3TRC_ReadCommand(LSM6DS3TRC_CTRL3_C, buf, 1);//等到CTRL3_C寄存器的SW_RESET位返回0
}

2.5 LSM6DS3TRC设置块数据更新

BDU：块数据更新。默认值:0.(0:持续更新;1:在读取MSB和LSB之前不更新输出寄存器)

在这里插入图片描述

#define LSM6DS3TRC_CTRL3_C		0x12/******************************************************************************** 函数名：LSM6DS3TRC_Set_BDU* 描述  ：LSM6DS3TRC设置块数据更新* 输入  ：bool flag* 输出  ：void* 调用  ：内部调用* 备注  ：*******************************************************************************/
void LSM6DS3TRC_Set_BDU(bool flag)
{uint8_t buf[1] = {0};LSM6DS3TRC_ReadCommand(LSM6DS3TRC_CTRL3_C, buf, 1);if (flag == true){buf[0] |= 0x40;//启用BDULSM6DS3TRC_WriteCommand(LSM6DS3TRC_CTRL3_C, buf, 1);}else{buf[0] &= 0xbf;//禁用BDULSM6DS3TRC_WriteCommand(LSM6DS3TRC_CTRL3_C, buf, 1);}LSM6DS3TRC_ReadCommand(LSM6DS3TRC_CTRL3_C, buf, 1);
}

2.6 LSM6DS3TRC设置加速度计的数据采样率

加速度计的数据采样率主要是设置寄存器0x10的高4位，具体写入多少根据下面表格而定。

在这里插入图片描述

//加速度计控制寄存器
#define LSM6DS3TRC_CTRL1_XL		0x10//线性加速输出数据速率
#define LSM6DS3TRC_ACC_RATE_0	    0x00
#define LSM6DS3TRC_ACC_RATE_1HZ6	0xB0
#define LSM6DS3TRC_ACC_RATE_12HZ5	0x10
#define LSM6DS3TRC_ACC_RATE_26HZ	0x20
#define LSM6DS3TRC_ACC_RATE_52HZ	0x30
#define LSM6DS3TRC_ACC_RATE_104HZ	0x40
#define LSM6DS3TRC_ACC_RATE_208HZ	0x50
#define LSM6DS3TRC_ACC_RATE_416HZ	0x60
#define LSM6DS3TRC_ACC_RATE_833HZ	0x70
#define LSM6DS3TRC_ACC_RATE_1660HZ	0x80
#define LSM6DS3TRC_ACC_RATE_3330HZ	0x90
#define LSM6DS3TRC_ACC_RATE_6660HZ	0xA0/******************************************************************************** 函数名：LSM6DS3TRC_Set_Accelerometer_Rate* 描述  ：LSM6DS3TRC设置加速度计的数据采样率* 输入  ：uint8_t rate* 输出  ：void* 调用  ：内部调用* 备注  ：*******************************************************************************/
void LSM6DS3TRC_Set_Accelerometer_Rate(uint8_t rate)
{uint8_t buf[1] = {0};LSM6DS3TRC_ReadCommand(LSM6DS3TRC_CTRL1_XL, buf, 1);buf[0] |= rate;//设置加速度计的数据采样率LSM6DS3TRC_WriteCommand(LSM6DS3TRC_CTRL1_XL, buf, 1);
}

2.7 LSM6DS3TRC设置陀螺仪的数据采样率

陀螺仪的数据采样率主要是设置寄存器0x11的高4位，具体写入多少根据下面表格而定。

在这里插入图片描述

//陀螺仪控制寄存器
#define LSM6DS3TRC_CTRL2_G		0x11//线性陀螺仪输出数据速率
#define LSM6DS3TRC_GYR_RATE_0	    0x00
#define LSM6DS3TRC_GYR_RATE_1HZ6	0xB0
#define LSM6DS3TRC_GYR_RATE_12HZ5	0x10
#define LSM6DS3TRC_GYR_RATE_26HZ	0x20
#define LSM6DS3TRC_GYR_RATE_52HZ	0x30
#define LSM6DS3TRC_GYR_RATE_104HZ	0x40
#define LSM6DS3TRC_GYR_RATE_208HZ	0x50
#define LSM6DS3TRC_GYR_RATE_416HZ	0x60
#define LSM6DS3TRC_GYR_RATE_833HZ	0x70
#define LSM6DS3TRC_GYR_RATE_1660HZ	0x80
#define LSM6DS3TRC_GYR_RATE_3330HZ	0x90
#define LSM6DS3TRC_GYR_RATE_6660HZ	0xA0/******************************************************************************** 函数名：LSM6DS3TRC_Set_Gyroscope_Rate* 描述  ：LSM6DS3TRC设置陀螺仪数据速率* 输入  ：uint8_t rate* 输出  ：void* 调用  ：内部调用* 备注  ：*******************************************************************************/
void LSM6DS3TRC_Set_Gyroscope_Rate(uint8_t rate)
{uint8_t buf[1] = {0};LSM6DS3TRC_ReadCommand(LSM6DS3TRC_CTRL2_G, buf, 1);buf[0] |= rate;//设置陀螺仪数据速率LSM6DS3TRC_WriteCommand(LSM6DS3TRC_CTRL2_G, buf, 1);
}

2.8 LSM6DS3TRC加速度计满量程选择

加速度计满量程选择主要是设置寄存器0x10的第5位和第6位，具体写入多少根据下面表格而定。

在这里插入图片描述

//加速度计全量程
#define LSM6DS3TRC_ACC_FSXL_2G	0x00
#define LSM6DS3TRC_ACC_FSXL_16G	0x04
#define LSM6DS3TRC_ACC_FSXL_4G	0x08
#define LSM6DS3TRC_ACC_FSXL_8G	0x0C
/**
****************************************************************************** 函数名：LSM6DS3TRC_Set_Accelerometer_Fullscale* 描述  ：LSM6DS3TRC加速度计满量程选择* 输入  ：uint8_t value* 输出  ：void* 调用  ：内部调用* 备注  ：*******************************************************************************/
void LSM6DS3TRC_Set_Accelerometer_Fullscale(uint8_t value)
{uint8_t buf[1] = {0};LSM6DS3TRC_ReadCommand(LSM6DS3TRC_CTRL1_XL, buf, 1);buf[0] |= value;//设置加速度计的满量程LSM6DS3TRC_WriteCommand(LSM6DS3TRC_CTRL1_XL, buf, 1);
}

2.9 LSM6DS3TRC陀螺仪全量程选择

陀螺仪全量程选择主要是设置寄存器0x11的第5位和第6位，具体写入多少根据下面表格而定。

在这里插入图片描述

//陀螺仪全量程
#define LSM6DS3TRC_GYR_FSG_245	0x00
#define LSM6DS3TRC_GYR_FSG_500	0x04
#define LSM6DS3TRC_GYR_FSG_1000	0x08
#define LSM6DS3TRC_GYR_FSG_2000	0x0C/******************************************************************************** 函数名：LSM6DS3TRC_Set_Gyroscope_Fullscale* 描述  ：LSM6DS3TRC陀螺仪满量程选择* 输入  ：uint8_t value* 输出  ：void* 调用  ：内部调用* 备注  ：*******************************************************************************/
void LSM6DS3TRC_Set_Gyroscope_Fullscale(uint8_t value)
{uint8_t buf[1] = {0};LSM6DS3TRC_ReadCommand(LSM6DS3TRC_CTRL2_G, buf, 1);buf[0] |= value;//设置陀螺仪的满量程LSM6DS3TRC_WriteCommand(LSM6DS3TRC_CTRL2_G, buf, 1);
}

2.10 LSM6DS3TRC设置加速度计模拟链带宽

在这里插入图片描述

#define LSM6DS3TRC_CTRL1_XL		0x10//加速度计的模拟链带宽
#define LSM6DS3TRC_ACC_BW0XL_1500HZ	0x00
#define LSM6DS3TRC_ACC_BW0XL_400HZ	0x01#define LSM6DS3TRC_CTRL8_XL		0x17//加速度计带宽选择
//低通滤波器
#define LSM6DS3TRC_ACC_LOW_PASS_ODR_50	    0x88
#define LSM6DS3TRC_ACC_LOW_PASS_ODR_100  	0xA8
#define LSM6DS3TRC_ACC_LOW_PASS_ODR_9	    0xC8
#define LSM6DS3TRC_ACC_LOW_PASS_ODR_400	    0xE8
//高通滤波器
#define LSM6DS3TRC_ACC_HIGH_PASS_ODR_50  	0x04
#define LSM6DS3TRC_ACC_HIGH_PASS_ODR_100	0x24
#define LSM6DS3TRC_ACC_HIGH_PASS_ODR_9 	    0x44
#define LSM6DS3TRC_ACC_HIGH_PASS_ODR_400	0x64/******************************************************************************** 函数名：LSM6DS3TRC_Set_Accelerometer_Bandwidth* 描述  ：LSM6DS3TRC设置加速度计模拟链带宽* 输入  ：uint8_t BW0XL, uint8_t ODR* 输出  ：void* 调用  ：内部调用* 备注  ：BW0XL模拟链带宽, ODR输出数据率*******************************************************************************/
void LSM6DS3TRC_Set_Accelerometer_Bandwidth(uint8_t BW0XL, uint8_t ODR)
{uint8_t buf[1] = {0};LSM6DS3TRC_ReadCommand(LSM6DS3TRC_CTRL1_XL, buf, 1);buf[0] |= BW0XL;LSM6DS3TRC_WriteCommand(LSM6DS3TRC_CTRL1_XL, buf, 1);LSM6DS3TRC_ReadCommand(LSM6DS3TRC_CTRL8_XL, buf, 1);buf[0] |= ODR;LSM6DS3TRC_WriteCommand(LSM6DS3TRC_CTRL8_XL, buf, 1);
}

3.读取数据

可以根据寄存器0x1E的后三位的状态来读取当前是加速度计的数据还是陀螺仪的数据。

在这里插入图片描述

//用户界面的状态数据寄存器
#define LSM6DS3TRC_STATUS_REG	0x1E
#define LSM6DS3TRC_STATUS_GYROSCOPE		0x02
#define LSM6DS3TRC_STATUS_ACCELEROMETER	0x01/******************************************************************************** 函数名：LSM6DS3TRC_Get_Status* 描述  ：从LSM6DS3TRC状态寄存器获取数据状态* 输入  ：void* 输出  ：void* 调用  ：内部调用* 备注  ：*******************************************************************************/
uint8_t LSM6DS3TRC_Get_Status(void)
{uint8_t buf[1] = {0};LSM6DS3TRC_ReadCommand(LSM6DS3TRC_STATUS_REG, buf, 1);return buf[0];
}/******************************************************************************** 函数名：LSM6DS3TRC_SCAN* 描述  ：LSM6DS3TRC扫描* 输入  ：void* 输出  ：void* 调用  ：内部调用* 备注  ：*******************************************************************************/
void LSM6DS3TRC_SCAN(void)
{status = LSM6DS3TRC_Get_Status();if (status & LSM6DS3TRC_STATUS_ACCELEROMETER){LSM6DS3TRC_Get_Acceleration(LSM6DS3TRC_ACC_FSXL_2G, acc);					}if (status & LSM6DS3TRC_STATUS_GYROSCOPE){LSM6DS3TRC_Get_Gyroscope(LSM6DS3TRC_GYR_FSG_2000, gyr);		}
}

3.1 读取加速度计数据

加速度计（accelerometer）中的"G"（重力加速度）是指地球表面的重力加速度，约为9.8米/秒²。加速度计通常用于测量物体在三个轴（x、y、z）上的加速度，并且它们通常设计成可以测量超过地球重力加速度的范围，以应对不同应用场景下的加速度变化。

具体来说：

2G：表示加速度计可以测量的最大加速度为地球重力加速度的2倍，即2 * 9.8 =19.6米/秒²。这种设置适合对较小的加速度变化进行精确测量，比如一般的人体运动或者车辆在正常行驶过程中的振动。
4G：表示加速度计可以测量的最大加速度为地球重力加速度的4倍，即4 * 9.8 = 39.2米/秒²。这种设置适合对中等强度的加速度变化进行测量，比如运动员的快速动作或者车辆在急刹车时的加速度。
8G：表示加速度计可以测量的最大加速度为地球重力加速度的8倍，即8 * 9.8 = 78.4米/秒²。这种设置适合对较大的加速度变化进行测量，比如飞行器在起飞或者运动装置中的高速旋转。
16G：表示加速度计可以测量的最大加速度为地球重力加速度的16倍，即16 * 9.8 = 156.8米/秒²。这种设置适合对非常强烈的加速度变化进行测量，比如高速运动或者爆炸冲击下的加速度。

从寄存器0x28-0x2D等6个寄存器获取的X，Y，Z三轴加速度计的值均为一个16位的二进制补码。

在这里插入图片描述

根据不同的加速度计量程来选择不同的输出的数据的转换系数。

在这里插入图片描述

//加速度计输出接口XYZ
#define LSM6DS3TRC_OUTX_L_XL		0x28
#define LSM6DS3TRC_OUTX_H_XL		0x29
#define LSM6DS3TRC_OUTY_L_XL		0x2A
#define LSM6DS3TRC_OUTY_H_XL		0x2B
#define LSM6DS3TRC_OUTZ_L_XL		0x2C
#define LSM6DS3TRC_OUTZ_H_XL		0x2D/******************************************************************************** 函数名：LSM6DS3TRC_Get_Acceleration* 描述  ：从LSM6DS3TRC读取加速度计数据* 输入  ：uint8_t fsxl, float *acc_float* 输出  ：void* 调用  ：内部调用* 备注  ：转换为浮点数的加速度值*******************************************************************************/
void LSM6DS3TRC_Get_Acceleration(uint8_t fsxl, float *acc_float)
{uint8_t buf[6];int16_t acc[3];LSM6DS3TRC_ReadCommand(LSM6DS3TRC_OUTX_L_XL, buf, 6);//获取加速度计原始数据acc[0] = buf[1] << 8 | buf[0];acc[1] = buf[3] << 8 | buf[2];acc[2] = buf[5] << 8 | buf[4];
//printf("\r\nbuf1= %d buf2= %d\r", buf[1],buf[0]);	
//printf("\r\nacc:X:%d,\tY:%d,\tZ:%d\r", acc[0], acc[1], acc[2]);switch (fsxl)//根据不同量程来选择输出的数据的转换系数{case LSM6DS3TRC_ACC_FSXL_2G:acc_float[0] = ((float)acc[0] * 0.061f);acc_float[1] = ((float)acc[1] * 0.061f);acc_float[2] = ((float)acc[2] * 0.061f);		break;case LSM6DS3TRC_ACC_FSXL_16G:acc_float[0] = ((float)acc[0] * 0.488f);acc_float[1] = ((float)acc[1] * 0.488f);acc_float[2] = ((float)acc[2] * 0.488f);break;case LSM6DS3TRC_ACC_FSXL_4G:acc_float[0] = ((float)acc[0] * 0.122f);acc_float[1] = ((float)acc[1] * 0.122f);acc_float[2] = ((float)acc[2] * 0.122f);break;case LSM6DS3TRC_ACC_FSXL_8G:acc_float[0] = ((float)acc[0] * 0.244f);acc_float[1] = ((float)acc[1] * 0.244f);acc_float[2] = ((float)acc[2] * 0.244f);break;}
}

3.2 读取陀螺仪数据

陀螺仪（gyroscope）的单位"dps"表示每秒钟的角速度变化率，即度每秒。它用来测量物体在空间中绕其旋转轴的旋转速率。不同的"dps"值表示陀螺仪能够测量的角速度范围大小，通常与具体的应用需求和预期的旋转速率有关。

具体来说：

245dps：表示陀螺仪可以测量的最大角速度变化率为每秒245度。这种设置适合于需要较为精确测量的应用，比如一般的姿态控制或者低速运动状态下的导航。
500dps：表示陀螺仪可以测量的最大角速度变化率为每秒500度。这种设置适合需要稍高精度或者涉及到中等速度旋转的应用，比如车辆动态控制或者无人机的航向调整。
1000dps：表示陀螺仪可以测量的最大角速度变化率为每秒1000度。这种设置适合于需要更高精度或者快速旋转的应用，比如高速运动器件的导航系统或者工业机器人的动作控制。
2000dps：表示陀螺仪可以测量的最大角速度变化率为每秒2000度。这种设置适合于非常快速旋转的应用，比如飞行器中的旋转控制或者高速机械设备的动态平衡调整。

从寄存器0x22-0x27等6个寄存器获取的角速率传感器俯仰轴(X)，角速率传感器横滚轴(Y)，角速率传感器航向轴(Z)的值均为一个16位的二进制补码。

在这里插入图片描述

根据不同的陀螺仪量程来选择不同的输出的数据的转换系数。

//陀螺仪输出接口XYZ
#define LSM6DS3TRC_OUTX_L_G		0x22
#define LSM6DS3TRC_OUTX_H_G		0x23
#define LSM6DS3TRC_OUTY_L_G		0x24
#define LSM6DS3TRC_OUTY_H_G		0x25
#define LSM6DS3TRC_OUTZ_L_G		0x26
#define LSM6DS3TRC_OUTZ_H_G		0x27/******************************************************************************** 函数名：LSM6DS3TRC_Get_Gyroscope* 描述  ：从LSM6DS3TRC读取陀螺仪数据* 输入  ：uint8_t fsg, float *gry_float* 输出  ：void* 调用  ：内部调用* 备注  ：转换为浮点数的角速度值*******************************************************************************/
void LSM6DS3TRC_Get_Gyroscope(uint8_t fsg, float *gry_float)
{uint8_t buf[6];int16_t gry[3];LSM6DS3TRC_ReadCommand(LSM6DS3TRC_OUTX_L_G, buf, 6);//获取陀螺仪原始数据gry[0] = buf[1] << 8 | buf[0];gry[1] = buf[3] << 8 | buf[2];gry[2] = buf[5] << 8 | buf[4];		
//	printf("\rgyr:X:%d,\tY:%d,\tZ:%d\r", gry[0], gry[1], gry[2]);switch (fsg)//根据不同量程来选择输出的数据的转换系数{case LSM6DS3TRC_GYR_FSG_245:gry_float[0] = ((float)gry[0] * 8.750f);gry_float[1] = ((float)gry[1] * 8.750f);gry_float[2] = ((float)gry[2] * 8.750f);break;case LSM6DS3TRC_GYR_FSG_500:gry_float[0] = ((float)gry[0] * 17.50f);gry_float[1] = ((float)gry[1] * 17.50f);gry_float[2] = ((float)gry[2] * 17.50f);break;case LSM6DS3TRC_GYR_FSG_1000:gry_float[0] = ((float)gry[0] * 35.00f);gry_float[1] = ((float)gry[1] * 35.00f);gry_float[2] = ((float)gry[2] * 35.00f);break;case LSM6DS3TRC_GYR_FSG_2000:gry_float[0] = ((float)gry[0] * 70.00f);gry_float[1] = ((float)gry[1] * 70.00f);gry_float[2] = ((float)gry[2] * 70.00f);break;}
}

4.姿态解算

借助视觉SLAM十四讲里面的一些知识点进行介绍

4.1 欧拉角

无论是旋转矩阵，旋转变量，它们虽然能描述旋转，但是对我们人类来说非常的不直观。当我们看到一个旋转矩阵或选择变量时，很难想象出这个旋转究竟是什么样的。而欧拉角提供了一种非常直观的方式来描述旋转————它使用了3个分离的转角，把一个旋转分解为3次绕不同轴的旋转。

你或许在航空，航模中听说过“俯仰角”“航向角”这些词。欧拉角当中比较常用的一种，便是“偏航–俯仰–滚转”（yaw–pitch–roll）3个角度来描述一个旋转的。由于它等价于ZYX轴旋转。因此就以ZXY为例，假设一个刚体的前方（朝向我们的方向）为X轴，右侧为Y轴，上方为Z轴。如下图所示，ZYX转角相当于把任意旋转分解为以下3个轴上的转角。

绕物体的z轴旋转，得到偏航角yaw；
绕旋转之后的Y轴旋转，得到俯仰角pitch；
绕旋转之后的X轴旋转，得到滚转角roll。

在这里插入图片描述

欧拉角的一个重大缺点是会碰到著名的万向锁问题（Gimbal Lock）：在俯仰角为±90°时，第一次旋转与第三次旋转将使用同一个轴，使得系统丢失一个自由度（由3次旋转变成了2次旋转）。这被称为奇异性问题，在其他形式的欧拉角也同样存在。理论上可以证明，只要想用3个实数来表达三维旋转时，都会不可避免地碰到奇异性问题。由于这种原理，欧拉角不适于插值和迭代，往往只用于人机交互中。我们也很少在SLAM程序中直接使用欧拉角来表达姿态，同样不会在滤波或优化中使用欧拉角表达旋转（因为它具有奇异性）。不过。若你想验证自己的算法是否有错。转换为欧拉角能够快速分辨结果是否正确。

4.2 四元数

旋转矩阵用9个量描述3自由度的旋转，具有元余性；欧拉角和旋转向量是紧凑的，但具有奇异性。事实上，我们找不到不带奇异性的三维向量描述方式。这有点类似于用两个坐标表示地球表面（如经度和纬度），将必定存在奇异性（纬度为士90°时经度无意义）。三维旋传是一个三维流形，想要无奇异性地表达它，用3个量是不够的。

回忆以前学习过的复数。我们用复数集C表示复平面上的向量，而复数的乘法则表示复平面上的旋转：例如，乘上复数i相当于逆时针把一个复向量旋转90°。类似地，在表达三维空间旋转时，也有一种类似于复数的代数：四元数（Quaternion）。四元数是Hamilton找到的一种扩展的复数。它既是紧凑的，也没有奇异性。如果说缺点，四元数不够直观，其运算稍复杂些。

一个四元数q拥有一个实部和三个虚部。本书把实部写在前面（也有地方把实部写在后面），像下面这样：

在这里插入图片描述

其中i，k为四元数的三个虚部。这三个虚部满足以下关系式：

在这里插入图片描述

由于它的这种特殊表示形式，有时人们也用一个标量和一个向量来表达四元数：

在这里插入图片描述

这里，s 称为四元数的实部，而 v称为它的虚部。如果一个四元数的虚部为0，称之为实四元数。反之，若它的实部为0，则称之为虚四元数。

这和复数非常相似。考虑到三维空间需要3个轴，四元数也有3个虚部，那么，一个虚四元数能不能对应到一个空间点呢？事实上我们就是这样做的。同理，我们知道一个模长为1的复数可以表示复平面上的纯旋转（没有长度的缩放），那么，三维空间中的旋转是否能用单位四元数表达呢？答案也是肯定的。

我们能用单位四元数表示三维空间中任意一个旋转，不过这种表达方式和复数有着微妙的不同。在复数中，乘以i意味着旋转90°。这是否意味着四元数中，乘i就是绕i轴旋转90°？那么，ij=-k是否意味着，先绕i转90°，在绕j转90°，就等于绕k转-90°？其实不是这样的，正确的情形应该是，乘以i对应着旋转180°，这样才能保证ij=−k的性质。而i² =−1，意味着绕i轴旋转360°后得到一个相反的东西。这个东西要旋转两周才会和它原先的样子相等。

这似乎有些玄妙了，完整的解释需要引入太多额外的东西，我们还是冷静一下回到眼前。至少，我们知道单位四元数能够表达三维空间的旋转。这种表达方式和旋转矩阵、旋转向量有什么关系呢？我们不妨先来看旋转向量。假设某个旋转是绕单位向量n=[nx,ny,nz]T进行了角度为θ的旋转，那么这个旋转的四元数形式为：

在这里插入图片描述
反之，亦可从单位四元数中计算出对应旋转轴与夹角：

在这里插入图片描述
这个式子给了我们一种微妙的“转了一半”的感觉。同样，对旋转的四元数形式的加上2π，我们得到一个相同的旋转，但此时对应的四元数变成了−q。因此，在四元数中，任意的旋转都可以由两个互为相反数的四元数表示。同理，取θ为0，则得到一个没有任何旋转的实四元数：
在这里插入图片描述
四元数的运算

四元数和通常复数一样，可以进行一系列的运算。常见的四则运算、数乘、求逆、共轭等。

在这里插入图片描述

4.3 四元数姿态解算

用四元素法进行姿态解算，其步骤如下：

4.3.1 四元素初始化（第一次解算时计算）

①静止状态下由加速度，磁力计值求取初始rollγ、pitchθ、hdgψ：

在这里插入图片描述
②初始化四元素

在这里插入图片描述

/******************************************************************************** 函数名：MargAHRSinit* 描述  ：初始四元数q值计算* 输入  ：float ax, float ay, float az,quaternion_yuandian *attitude* 输出  ：void* 调用  ：内部调用* 备注  ：*******************************************************************************/
void MargAHRSinit(float ax, float ay, float az,quaternion_yuandian *attitude)
{float initialRoll, initialPitch;float cosRoll, sinRoll, cosPitch, sinPitch;float magX, magY;float initialHdg, cosHeading, sinHeading;float q0,q1,q2,q3;float q_rsqrt;//使用加速度数据计算欧拉角 ，滚转角和俯仰角initialRoll  = atan2(-ay, -az);initialPitch = atan2(ax, -az);magX = 1.0f;magY = 0.0f;initialHdg = atan2f(-magY, magX);//解算航向角cosRoll = cosf(initialRoll * 0.5f);sinRoll = sinf(initialRoll * 0.5f);cosPitch = cosf(initialPitch * 0.5f);sinPitch = sinf(initialPitch * 0.5f);cosHeading = cosf(initialHdg * 0.5f);sinHeading = sinf(initialHdg * 0.5f);q0 = cosRoll * cosPitch * cosHeading + sinRoll * sinPitch * sinHeading;q1 = sinRoll * cosPitch * cosHeading - cosRoll * sinPitch * sinHeading;q2 = cosRoll * sinPitch * cosHeading + sinRoll * cosPitch * sinHeading;q3 = cosRoll * cosPitch * sinHeading - sinRoll * sinPitch * cosHeading;q_rsqrt = invSqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3);attitude->w = q0 * q_rsqrt;attitude->x = q1 * q_rsqrt;attitude->y = q2 * q_rsqrt;attitude->z = q3 * q_rsqrt;
}

4.3.2 四元数归一化

四元数归一化，归一化之后的四元数的逆即是其共轭。

归一化的意义：

单位化向量：归一化通常指将向量调整为单位长度，即将其长度（或范数）调整为1。这样的向量称为单位向量。在几何和物理学中，单位向量表示方向而不受其大小的影响。
避免数值问题：在数值计算中，归一化可以减少数值计算误差的影响，特别是在迭代算法中，如迭代求解方程组、优化算法等。大多数数值方法对输入数据有某种形式的归一化要求，以确保算法的稳定性和效率。
算法要求：某些算法要求输入数据归一化，以确保它们的表现符合预期。例如，机器学习中的很多算法（如支持向量机、神经网络等）通常要求输入数据被归一化，以便它们能够更有效地学习权重和模式。
合法旋转表示：只有单位四元数才能完全表示合法的旋转。非单位四元数虽然也可以表示旋转，但在应用旋转时可能导致缩放或其他非预期的效果。因此，规范化确保了四元数代表的旋转操作是数学上正确和一致的。

/******************************************************************************** 函数名：invSqrt()* 描述  ：归一化* 输入  ：float x* 输出  ：float * 调用  ：内部调用* 备注  ：*******************************************************************************/
static float invSqrt(float x)
{float halfx = 0.5f * x;//计算x的一半,用于后续的牛顿迭代法float y = x;//将输入值x初始化为y，开始时假设y是x的平方根的倒数long i = *(long*)&y;//通过类型转换，将y的浮点数表示当作长整型数字来读取其位模式i = 0x5f3759df - (i >> 1);//0x5f3759df,与i的半逆转相减产生y的初估计值//这里使用位运算来加速运算，并通过位移操作实现除以2的效果y = *(float*)&i;//将长整型i的内容当作浮点数来读取，得到倒数平方根的初估计值y = y * (1.5f - (halfx * y * y));//进行一次牛顿迭代，以改进y的值，提高计算的准确度return y;//返回y值，即x的平方根的倒数
}

4.3.3 提取四元素等效余弦矩阵中的重力分量

在这里插入图片描述

/******************************************************************************** 函数名：get_acc_no_G()* 描述  ：获取重力分量* 输入  ：quaternion_yuandian q,float a[3]* 输出  ：void  * 调用  ：内部调用* 备注  ：*******************************************************************************/
void get_acc_no_G(quaternion_yuandian q,float a[3])
{a[0] = 2 * (q.x * q.z - q.w * q.y);//2(q1q3-q0q2)a[1] = 2 * (q.w * q.x + q.y * q.z);//2(q0q1+q2q3)a[2] = 1 - 2*(q.x * q.x + q.y * q.y);//1-2(q21+q22)
}

4.3.4 四元数姿态融合算法–互补滤波法

四元数姿态融合算法中的互补滤波法是一种常用的姿态估计算法，它结合了多个传感器的数据，通过互补特性来提高姿态估计的精度。

该算法的主要思路：利用不同传感器在频域上的互补特性，通过加权平均的方式将多个传感器的数据进行融合，以得到更准确的姿态估计。在四元数姿态解算中，常用的传感器包括陀螺仪、加速度计和磁力计。

陀螺仪：测量角速度，数据频率高，动态响应好，但存在积分累积误差。
加速度计：测量线性加速度，通过重力加速度分量计算倾斜角度，低频分量准确，但动态响应差。
磁力计：测量地球磁场方向，用于确定偏航角，低频分量准确，但易受干扰。

/******************************************************************************** 函数名：mix_gyrAcc_crossMethod()* 描述  ：四元数姿态融合算法--互补滤波法* 输入  ：quaternion_yuandian *attitude,float g[3],float a[3],float interval* 输出  ：void  * 调用  ：内部调用* 备注  ：*******************************************************************************/
void mix_gyrAcc_crossMethod(quaternion_yuandian *attitude,float g[3],float a[3],float interval)
{const static float FACTOR = 0.8;//两个重力矢量叉积后所乘的系数 p，用于和陀螺仪积分角度相叠加来修正陀螺仪（这里只用了比例 p，没用积分 i，）//FACTOR 为 1，则完全信任加速度计，为 0，则完全信任陀螺仪float delta_x,delta_y,delta_z,q_rsqrt;float w_q = attitude->w;//w=cos(alpha/2)float x_q = attitude->x;//x=ax*sin(alpha/2)float y_q = attitude->y;//y=ay*sin(alpha/2)float z_q = attitude->z;//z=az*sin(alpha/2)float x_q_2 = x_q * 2;float y_q_2 = y_q * 2;float z_q_2 = z_q * 2;//// 加速度计的读数，单位化。float a_rsqrt = invSqrt(a[0]*a[0]+a[1]*a[1]+a[2]*a[2]);float x_aa = a[0] * a_rsqrt;float y_aa = a[1] * a_rsqrt;float z_aa = a[2] * a_rsqrt;//float x_ac = x_q*z_q_2 - w_q*y_q_2;// 2*(x*z-w*y) =ax*az(1-cos(alpha))-ay*sin(alpha)float y_ac = y_q*z_q_2 + w_q*x_q_2;// 2*(y*z+w*x) =az*ay(1-cos(alpha))+ax*sin(alpha)float z_ac = 1 - x_q*x_q_2 - y_q*y_q_2;// w^2+x^2-y^2-z^2 =1-2*x^2-2*y^2 = cos(alpha)+(1-cos(alpha)*z^2)//// 测量值与常量的叉积。//测量值叉乘常量值,并以此向量表示误差角度大小与转轴方向，用于修正陀螺仪积分角度float x_ca = y_aa * z_ac - z_aa * y_ac;float y_ca = z_aa * x_ac - x_aa * z_ac;float z_ca = x_aa * y_ac - y_aa * x_ac;//// 构造增量旋转。//可看成分别绕 xyz 轴的三次旋转的叠加。sin(delta/2)近似为 delta/2,cos(delta/2)近似为 0delta_x = (g[0] + x_ca * FACTOR + x_ca * 0.01 *interval)*interval*0.5f;delta_y = (g[1] + y_ca * FACTOR + y_ca * 0.01 *interval)*interval*0.5f;delta_z = (g[2] + z_ca * FACTOR + z_ca * 0.01 *interval)*interval*0.5f;//根据增量旋转 delta_x, delta_y, delta_z 计算新的四元数值 w_q, x_q, y_q, z_q。w_q = w_q - x_q*delta_x - y_q*delta_y - z_q*delta_z;x_q = w_q*delta_x + x_q + y_q*delta_z - z_q*delta_y;y_q = w_q*delta_y - x_q*delta_z + y_q + z_q*delta_x;z_q = w_q*delta_z + x_q*delta_y - y_q*delta_x + z_q;//使用 invSqrt 函数计算四元数的倒数平方根，并将四元数归一化，确保其单位长度。q_rsqrt = invSqrt(w_q * w_q + x_q * x_q + y_q * y_q + z_q * z_q);attitude->w = w_q * q_rsqrt;attitude->x = x_q * q_rsqrt;attitude->y = y_q * q_rsqrt;attitude->z = z_q * q_rsqrt;
}

以上代码核心的思想是通过陀螺仪测量的角速度和加速度计测量的加速度数据来更新四元数表示的姿态。通过将陀螺仪测量的角速度与加速度计测量的加速度数据进行叉积运算，得到一个误差向量，然后根据这个误差向量计算增量旋转，最终更新当前的四元数姿态。这种方法可以有效地结合陀螺仪和加速度计的测量结果，从而得到更稳定和准确的姿态估计。

4.3.5 四元数换算得到欧拉角

在这里插入图片描述
*pitch：俯仰角，使用反正弦函数 asinf() 计算，参数为 g1。
*roll：横滚角，使用反正切函数 atanf() 计算，参数为 g2 / g3。
*yaw：航向角，同样使用反正切函数 atanf() 计算，参数为 g4 / g5。

/******************************************************************************** 函数名：get_angle()* 描述  ：四元数换算得到欧拉角：俯仰角、横滚角和航向角* 输入  ：quaternion_yuandian q,float *pitch,float *roll,float *yaw* 输出  ：void  * 调用  ：内部调用* 备注  ：*******************************************************************************/
void get_angle(quaternion_yuandian q,float *pitch,float *roll,float *yaw)
{float g1,g2,g3,g4,g5;g1 = 2.0f*(q.x*q.z-q.w*q.y);g2 = 2.0f*(q.w*q.x+q.y*q.z);g3 = q.w*q.w-q.x*q.x-q.y*q.y+q.z*q.z;g4 = 2.0f*(q.x*q.y+q.w*q.z);g5 = q.w*q.w+q.x*q.x-q.y*q.y-q.z*q.z;//计算得到俯仰角/横滚角/航向角*pitch = -asinf(g1) * 57.3f; // pitch 俯仰角*roll  = atanf(g2/g3) * 57.3f; // roll 横滚角*yaw   = atanf(g4/g5) * 57.3f; //yaw 航向角
}