成品网站1688入口网页版怎样,攀枝花城市建设网站,2网站建设总结,wordpress 控制台 慢文章目录 一、前言二、硬件1.引脚说明2.原理图 三、软件1.IIC读写函数1.1 读函数1.2 写函数 2.初始化2.1 检测设备是否存在2.2 读取LSM6DS3TRC器件ID2.3 LSM6DS3TRC重启#xff0c;重置寄存器2.5 LSM6DS3TRC设置块数据更新2.6 LSM6DS3TRC设置加速度计的数据采样率2.7 LSM6DS3T… 文章目录 一、前言二、硬件1.引脚说明2.原理图 三、软件1.IIC读写函数1.1 读函数1.2 写函数 2.初始化2.1 检测设备是否存在2.2 读取LSM6DS3TRC器件ID2.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的电压 SCLI2C串行时钟SCL/SPI串行端口时钟SPC SDAI2C串行数据SDA/SPI串行数据输入SDI/3线接口串行数据输出SDO SDOSPI 4线接口串行数据输出SDO/I2C设备地址的最低有效位SA0 CSI2C/SPI模式选择1SPI空闲模式/I2C通信启用0SPI通信模式/I2C禁用 INT1/2中断 SDxI2C串行时钟主机MSCL SCxI2C串行数据主机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_I2CADDR1) | 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_I2CADDR1) | 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_I2CADDR1) | 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个寄存器获取的XYZ三轴加速度计的值均为一个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–roll3个角度来描述一个旋转的。由于它等价于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拥有一个实部和三个虚部。本书把实部写在前面也有地方把实部写在后面像下面这样 其中ik为四元数的三个虚部。这三个虚部满足以下关系式 由于它的这种特殊表示形式有时人们也用一个标量和一个向量来表达四元数 这里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(q0q1q2q3)a[2] 1 - 2*(q.x * q.x q.y * q.y);//1-2(q21q22) }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;//wcos(alpha/2)float x_q attitude-x;//xax*sin(alpha/2)float y_q attitude-y;//yay*sin(alpha/2)float z_q attitude-z;//zaz*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*zw*x) az*ay(1-cos(alpha))ax*sin(alpha)float z_ac 1 - x_q*x_q_2 - y_q*y_q_2;// w^2x^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.xq.y*q.z);g3 q.w*q.w-q.x*q.x-q.y*q.yq.z*q.z;g4 2.0f*(q.x*q.yq.w*q.z);g5 q.w*q.wq.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 航向角 }这段代码的作用是将四元数转换为俯仰角、横滚角和航向角。上述计算公式的 57.3 是弧度转换为角度即 180/π这样得到的结果就是以度°为单位的。 四、测试结果 软件用的VOFA里面的小控件。 1. 静止状态 2. 倾斜状态 五、总结 今天主要讲了陀螺仪LSM6DS3TR-C的简单使用。因为是第一次使用陀螺仪很多资料也都是借鉴的不足之处还请各位大佬多多指教轻喷谢谢 再次感谢你的观看