使用QMI8658六轴原始数据融合输出欧拉角笔记

关于四元素和三维旋转的知识，推荐看一下https://github.com/Krasjet/quaternion。

qmi8658六轴姿态传感器的原始数据读取函数如下。需要注意的是，陀螺仪数据的格式。

void Qmi8658_read_acc_xyz(float acc_xyz[3])
{unsigned char  buf_reg[6];short            raw_acc_xyz[3];Qmi8658_read_reg(Qmi8658Register_Ax_L, buf_reg, 6);  // 0x19, 25raw_acc_xyz[0] = (short)((unsigned short)(buf_reg[1]<<8) |( buf_reg[0]));raw_acc_xyz[1] = (short)((unsigned short)(buf_reg[3]<<8) |( buf_reg[2]));raw_acc_xyz[2] = (short)((unsigned short)(buf_reg[5]<<8) |( buf_reg[4]));acc_xyz[0] = (raw_acc_xyz[0]*ONE_G)/acc_lsb_div;acc_xyz[1] = (raw_acc_xyz[1]*ONE_G)/acc_lsb_div;acc_xyz[2] = (raw_acc_xyz[2]*ONE_G)/acc_lsb_div;
}void Qmi8658_read_gyro_xyz(float gyro_xyz[3])
{unsigned char  buf_reg[6];short            raw_gyro_xyz[3];Qmi8658_read_reg(Qmi8658Register_Gx_L, buf_reg, 6);     // 0x1f, 31raw_gyro_xyz[0] = (short)((unsigned short)(buf_reg[1]<<8) |( buf_reg[0]));raw_gyro_xyz[1] = (short)((unsigned short)(buf_reg[3]<<8) |( buf_reg[2]));raw_gyro_xyz[2] = (short)((unsigned short)(buf_reg[5]<<8) |( buf_reg[4]));gyro_xyz[0] = (raw_gyro_xyz[0]*1.0f)/gyro_lsb_div;gyro_xyz[1] = (raw_gyro_xyz[1]*1.0f)/gyro_lsb_div;gyro_xyz[2] = (raw_gyro_xyz[2]*1.0f)/gyro_lsb_div;
}

/* 魔法函数InvSqrt()相当于1.0/sqrt() */
static float invSqrt(float number)
{volatile long i;volatile float x, y;volatile const float f = 1.5F;x = number * 0.5F;y = number;i = * (( long * ) &y);i = 0x5f375a86 - ( i >> 1 );y = * (( float * ) &i);y = y * ( f - ( x * y * y ) );return y;
}

计算欧拉角

#define Kp 10.0f
#define Ki 0.008f
/* #define pi 3.14159265f */
#define halfT 0.002127f        /*half the sample period*/
/* 参与计算的加速度单位g 陀螺仪单位是弧度/s()【度*pi/180=弧度】*/
float roll, pitch,yaw ;
float exInt, eyInt, ezInt;
float q0 = 1.0f, q1 = 0.0f, q2 = 0.0f, q3 = 0.0f;   /** quaternion of sensor frame relative to auxiliary frame */
void get_euler_angles(float gx, float gy, float gz, float ax, float ay, float az)
{float recipNorm;float vx, vy, vz;float ex, ey, ez;float q0q0 = q0*q0;float q0q1 = q0*q1;float q0q2 = q0*q2;float q1q1 = q1*q1;float q1q3 = q1*q3;float q2q2 = q2*q2;float q2q3 = q2*q3;float q3q3 = q3*q3;if( ax*ay*az==0)return;/* 对加速度数据进行归一化处理 */recipNorm = invSqrt( ax* ax +ay*ay + az*az);ax = ax *recipNorm;ay = ay *recipNorm;az = az *recipNorm;/* DCM矩阵旋转 */vx = 2*(q1q3 - q0q2);vy = 2*(q0q1 + q2q3);vz = q0q0 - q1q1 - q2q2 + q3q3 ;/* 在机体坐标系下做向量叉积得到补偿数据 */ex = ay*vz - az*vy ;ey = az*vx - ax*vz ;ez = ax*vy - ay*vx ;/* 对误差进行PI计算，补偿角速度 */exInt = exInt + ex * Ki;eyInt = eyInt + ey * Ki;ezInt = ezInt + ez * Ki;gx = gx + Kp*ex + exInt;gy = gy + Kp*ey + eyInt;gz = gz + Kp*ez + ezInt;/* 按照四元素微分公式进行四元素更新 */q0 = q0 + (-q1*gx - q2*gy - q3*gz)*halfT;q1 = q1 + (q0*gx + q2*gz - q3*gy)*halfT;q2 = q2 + (q0*gy - q1*gz + q3*gx)*halfT;q3 = q3 + (q0*gz + q1*gy - q2*gx)*halfT;recipNorm = invSqrt(q0*q0 + q1*q1 + q2*q2 + q3*q3);q0 = q0*recipNorm;q1 = q1*recipNorm;q2 = q2*recipNorm;q3 = q3*recipNorm;roll =  atan2f(2*q2*q3 + 2*q0*q1, -2*q1*q1 - 2*q2*q2 + 1)*57.3f;pitch =  asinf(2*q1*q3 - 2*q0*q2)*57.3f;yaw  =  -atan2f(2*q1*q2 + 2*q0*q3, -2*q2*q2 -2*q3*q3 + 1)*57.3f;printf("pitch:%.2f roll:%.2f yaw:%.2f\r\n",pitch,roll,yaw);
}

使用QMI8658六轴原始数据融合输出欧拉角笔记相关推荐

如何利用小熊派获取MPU6050六轴原始数据
摘要:使用小熊派开发板,通过硬件IIC与MPU6050六轴传感器模块通信,完成相应寄存器配置,成功获取陀螺仪.加速度计数据. 本问主要讲述使用小熊派开发板+MPU6050六轴传感器,获取加速度计以及陀 ...
【51单片机快速入门指南】4.3.3： MPU6050使用Mahony AHRS算法实现六轴姿态融合获取四元数、欧拉角
目录源码 Mahony_6.c Mahony_6.h 使用方法测试程序 main.c 效果 STC89C516 32MHz Keil uVision V5.29.0.0 PK51 Prof.Dev ...
【51单片机快速入门指南】4.3.4： MPU6050使用Madgwick AHRS算法实现六轴姿态融合获取四元数、欧拉角
目录源码 Madgwick_6.c Madgwick_6.h 使用方法测试程序 main.c 效果 STC89C516 32MHz Keil uVision V5.29.0.0 PK51 Prof ...
STC51单片机15——MPU6050六轴数据融合，互补滤波，时间常数可调，可稳定运行，串口显示角度值
51单片机用模拟IIC的方式读取MPU6050的原始数据,之后经过换算转成三轴加速度和三轴角速度.设定定时器,以固定的频率采集以上得到的数据,并加入互补滤波,去除加速度的噪声以及陀螺仪的零飘.注意,本 ...
姿态解算知识（三）-陀螺仪加速度计6轴数据融合
这么久的惯导总算是没白看,加上一篇博客的指点,这两天把Mahony的九轴数据融合算法看懂了.可惜第二版硬件还没到,磁力计用不了,没法验证效果~今天先总结下陀螺仪和加速度计的六轴数据融合. 版权声明原 ...
STM32学习值传感器篇——MPU6050六轴加速度传感器
这个传感器整体来说不难只要将模拟iic的时序写对基本问题不大剩下的驱动文件按照我介绍的加进去就可以实现了,还是有官方算法的好啊,直接用就完事了,很方便,节省了不少代码开发时间, 注意mpu6050 ...
keil4怎么移植其他人的程序_【调试笔记】韦东山：在100ask_imx6ull上移植使用六轴传感器ICM20608...
之前发了LCD调试笔记,大家很感兴趣,所以这次再来一篇:六轴传感器ICM20608驱动移植笔记,大家还需要什么移植笔记?可以留言.我们尽量满足. 1.1 移植思路先找到驱动:也许内核里已经有,也许需 ...
【调试笔记】韦东山：在100ask_imx6ull上移植使用六轴传感器ICM20608
在线课堂:https://www.100ask.net/index(课程观看) 论坛:http://bbs.100ask.net/(学术答疑) 开发板:https://100ask.taoba ...
【STM32Cube】学习笔记（三）：六轴传感器
文章目录摘要一.简介 1.I2C原理 2.MPU6050介绍 3.MPU6050寄存器介绍 4.DMP使用二.硬件电路设计三.软件设计 1.CubeMX配置 2.CubeIDE代码 3.结果显 ...

使用QMI8658六轴原始数据融合输出欧拉角笔记

使用QMI8658六轴原始数据融合输出欧拉角笔记相关推荐

最新文章

热门文章