MPU6500的使用之代码展示

一、获取初始数据

void mpu_get_data()
{mpu_read_bytes(MPU6500_ACCEL_XOUT_H, mpu_buff, 14);mpu_data.ax   = mpu_buff[0] << 8 | mpu_buff[1];mpu_data.ay   = mpu_buff[2] << 8 | mpu_buff[3];mpu_data.az   = mpu_buff[4] << 8 | mpu_buff[5];mpu_data.temp = mpu_buff[6] << 8 | mpu_buff[7];mpu_data.gx = ((mpu_buff[8]  << 8 | mpu_buff[9]) - mpu_data.gx_offset);mpu_data.gy = ((mpu_buff[10] << 8 | mpu_buff[11]) - mpu_data.gy_offset);mpu_data.gz = ((mpu_buff[12] << 8 | mpu_buff[13]) - mpu_data.gz_offset);memcpy(&imu.ax, &mpu_data.ax, 6 * sizeof(int16_t));imu.temp = 21 + mpu_data.temp / 333.87f;imu.wx   = mpu_data.gx / 16.384f / 57.3f; imu.wy   = mpu_data.gy / 16.384f / 57.3f; imu.wz   = mpu_data.gz / 16.384f / 57.3f;
}

二、更新数据

void imu_ahrs_update(void)
{float norm;float hx, hy, hz, bx, bz;float vx, vy, vz, wx, wy, wz;float ex, ey, ez, halfT;float tempq0,tempq1,tempq2,tempq3;float q0q0 = q0*q0;float q0q1 = q0*q1;float q0q2 = q0*q2;float q0q3 = q0*q3;float q1q1 = q1*q1;float q1q2 = q1*q2;float q1q3 = q1*q3;float q2q2 = q2*q2;   float q2q3 = q2*q3;float q3q3 = q3*q3;   gx = imu.wx;gy = imu.wy;gz = imu.wz;ax = (imu.ax/104.48979592f + A_offsetx)/AX_scale;ay = (imu.ay/104.48979592f + A_offsety)/AY_scale;az = (imu.az/104.48979592f + A_offsetz)/AZ_scale;mx = ((float)imu.mx - M_offsetx)*X_scale;my = ((float)imu.my - M_offsety)*Y_scale;mz = ((float)imu.mz - M_offsetz)*Z_scale;时间周期now_update  = HAL_GetTick(); //mshalfT       = ((float)(now_update - last_update) / 2000.0f);last_update = now_update;norm = inv_sqrt(ax*ax + ay*ay + az*az);       ax = ax * norm;ay = ay * norm;az = az * norm;**计算参考方向**hx = 2.0f*mx*(0.5f - q2q2 - q3q3) + 2.0f*my*(q1q2 - q0q3) + 2.0f*mz*(q1q3 + q0q2);hy = 2.0f*mx*(q1q2 + q0q3) + 2.0f*my*(0.5f - q1q1 - q3q3) + 2.0f*mz*(q2q3 - q0q1);hz = 2.0f*mx*(q1q3 - q0q2) + 2.0f*my*(q2q3 + q0q1) + 2.0f*mz*(0.5f - q1q1 - q2q2);         bx = sqrt((hx*hx) + (hy*hy));bz = hz; **vx、vy、vz是陀螺积分后的姿态来推算出的重力向量它们都是机体坐标参照系上的重力向量**vx = 2.0f*(q1q3 - q0q2);vy = 2.0f*(q0q1 + q2q3);vz = q0q0 - q1q1 - q2q2 + q3q3;wx = 2.0f*bx*(0.5f - q2q2 - q3q3) + 2.0f*bz*(q1q3 - q0q2);wy = 2.0f*bx*(q1q2 - q0q3) + 2.0f*bz*(q0q1 + q2q3);wz = 2.0f*bx*(q0q2 + q1q3) + 2.0f*bz*(0.5f - q1q1 - q2q2);  **向量间的误差，可以用向量叉积（也叫向量外积、叉乘）来表示，ex、ey、ez就是两个重力向量的叉积**ex = (ay*vz - az*vy) ey = (az*vx - ax*vz)ez = (ax*vy - ay*vx)**用叉积误差来做PI修正陀螺零偏**if(ex != 0.0f && ey != 0.0f && ez != 0.0f){exInt = exInt + ex * Ki * halfT;eyInt = eyInt + ey * Ki * halfT;   ezInt = ezInt + ez * Ki * halfT;gx = gx + Kp*ex + exInt;gy = gy + Kp*ey + eyInt;gz = gz + Kp*ez + ezInt;}**四元数微分方程，其中T为测量周期，gx、gy、gz为陀螺仪角速度，以下都是已知量，这里使用了一阶龙哥库塔求解四元数微分方程**tempq0 = q0 + (-q1*gx - q2*gy - q3*gz) * halfT;tempq1 = q1 + (q0*gx + q2*gz - q3*gy) * halfT;tempq2 = q2 + (q0*gy - q1*gz + q3*gx) * halfT;tempq3 = q3 + (q0*gz + q1*gy - q2*gx) * halfT;  **正规化四元数（由于只有单位四元数才能旋转，所以之前的计算一直是单位四元数，这里是将单位四元数重新转换为正规四元数）**norm = inv_sqrt(tempq0*tempq0 + tempq1*tempq1 + tempq2*tempq2 + tempq3*tempq3);q0 = tempq0 * norm;q1 = tempq1 * norm;q2 = tempq2 * norm;q3 = tempq3 * norm;
}

姿态计算


**根据四元数方向余弦阵和欧拉角的转换关系，把四元数转换成欧拉角：**
void imu_attitude_update(void)
{imu.yaw = -atan2(2*q1*q2 + 2*q0*q3, -2*q2*q2 - 2*q3*q3 + 1)* 57.3; imu.pit = -asin(-2*q1*q3 + 2*q0*q2)* 57.3;   imu.rol =  atan2(2*q2*q3 + 2*q0*q1, -2*q1*q1 - 2*q2*q2 + 1)* 57.3;
}

MPU6500的使用之代码展示相关推荐

干货 | 华尔街留下的指标之王（附代码展示）
壹 . 写在前面的话有人认为价格围绕价值上下波动,研究投资标的内在价值,于是就出现了基本面派: 有人为价格反映了一切,所有的信息(包括基本面)都反映到了盘面价格中,于是就有了技术面派: 从技术面派的 ...
java el表达式导航规则_诺禾：在jsp里面如何不写java代码展示数据(EL表达式的使用)...
EL表达式可以简化jsp中写的java代码在jsp里面如何不写java代码展示数据(EL表达式的使用) 在jsp里面如何不写java代码展示数据(EL表达式的使用) 在jsp里面如何不写java代码 ...
数独项目--关键代码展示：
关键代码展示: //判断该数字在当前数独是否符合要求 int judge(int num, int ple){ int x = ple / 9; //x表示数字的纵坐标 int y = ple % 9 ...
java echarts 生成图片_java如何在后台生成echarts图表？实际代码展示
学习是一个永无止境的过程,关于java的学习更是如此.今天主要为大家介绍下,如何在java中后台生成echarts图表,并且为大家展示实际的操作流程. 首先说下它的适用情况:⑴.支持echart4.0 ...
淘宝代购系统，代购程序，代购系统源码，海外代购系统源码部分PHP代码展示
什么是淘宝代购淘宝代购是近年兴起的一种购物模式,是帮国外客户购买中国商品.主要是通过万邦科技的外贸代购模式,把淘宝. 天猫等电商平台的全站商品通过API接入到你的网站上,瞬间就可以架设一个有数亿 ...
数据库大作业代码展示2
因为上个界面实在是太卡了,尤其在c#代码里,打字都是延缓的,所以分开来展示. 这一个主要是管理员界面的介绍,窗口调用如下: ManageMain窗口:(管理员主界面)省略代码和前文一样, (学生管理) ...
[R语言] 10行代码展示我国35个主要城市近一年来居民消费价格指数的变化
前言近年来,随着我国GDP呈现逐年增长的趋势,居民消费价格指数CPI也开始受到更多人的讨论,国家经济的发展,究竟是提高了居民的生活水平呢,还是增加了居民的生活压力呢? 接下来就让我们通过10行代码, ...
多多进宝商品列表接口，商品销量，店铺昵称，sku价格，sku属性，商品运费等接口代码展示
多多进宝商品列表接口,商品销量,店铺昵称,sku价格,sku属性,商品运费等接口代码展示如下: 1.公共参数名称类型必须描述 key String 是调用key(必须以GET方式拼接在URL ...
【开发总结】自动期末评教工具：技术功能说明、部分代码展示、演示视频
文章目录 1 背景说明 2 技术功能介绍 3 部分代码展示 4 演示视频 1 背景说明下图是我们学校的评教页面,我现在还没有评教完成(第一门课程已评完,当时是进行测试时评完的,其他课程都未评教完成) ...

MPU6500的使用之代码展示

MPU6500的使用之代码展示相关推荐

最新文章

热门文章