weixin_58208631 2021-08-06 14:47 采纳率: 100%
浏览 344
已结题

基于msp430f5529驱动mpu6050,通过原始数据求出欧拉角

求一份使用msp430f5529驱动mpu6050得到欧拉角的代码,使用官方dmp库的。已经通过i2c通信拿到mpu6050的6个原始数据了,将原始数据转换成欧拉角的代码也行

  • 写回答

1条回答 默认 最新

  • soar3033 2021-08-06 14:52
    关注
    Read_MPU6050(tp);    //**读取6060,6个数据在tp里,这个是个例子,到时候根据你6个数据进行替换**
    
    Angle_ax = ((float)(((int *)&tp)[0])) / 4096.0;    //加速度处理    结果单位是 +- g
    Angle_ay = ((float)(((int *)&tp)[1])) / 4096.0;    //转换关系    8192 LSB/g, 1g对应读数8192
    Angle_az = ((float)(((int *)&tp)[2])) / 4096.0;    //加速度量程 +-4g/S
    Angle_gx = ((float)(((int *)&tp)[4])) / 65.5;    //陀螺仪处理    结果单位是 +-度
    Angle_gy = ((float)(((int *)&tp)[5])) / 65.5;    //陀螺仪量程 +-500度/S, 1度/秒 对应读数 65.536
    Angle_gz = ((float)(((int *)&tp)[6])) / 65.5;    //转换关系65.5 LSB/度
    
    IMUupdate(Angle_gx*0.0174533f, Angle_gy*0.0174533f, Angle_gz*0.0174533f, Angle_ax,Angle_ay,Angle_az);//前面加速度乘以0.0174533f是为了转换为重力加速度
    
    void IMUupdate(float gx, float gy, float gz, float ax,float ay, float az)
    {
        const float Kp = 3.5, Ki = 0.05;
        static float exInt=0, eyInt=0, ezInt=0;
        float q0 = 1.0f, q1 = 0.0f, q2 = 0.0f, q3 = 0.0f; // roll,pitch,yaw 都为 0 时对应的四元数值。
        float haftT=0.004f ;
        float norm;
        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;
    
        // 第一步:对加速度数据进行归一化
        norm = sqrt(ax*ax + ay*ay + az*az); 
        ax = ax / norm; 
        ay = ay / norm; 
        az = az / norm; 
    
        // 第二步: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;
    
        norm = sqrt(q0*q0 + q1*q1 + q2*q2 + q3*q3);
        q0 = q0/norm;
        q1 = q1/norm;
        q2 = q2/norm;
        q3 = q3/norm;
    
        roll =  atan2f(2*q2*q3 + 2*q0*q1, -2*q1*q1 - 2*q2*q2 + 1)*57.3;     
        pitch =  asinf(2*q1*q3 - 2*q0*q2)*57.3;                                                          
        yaw  =  -atan2f(2*q1*q2 + 2*q0*q3, -2*q2*q2 -2*q3*q3 + 1)*57.3; 
    }
    
    
    
    本回答被题主选为最佳回答 , 对您是否有帮助呢?
    评论

报告相同问题?

问题事件

  • 系统已结题 8月15日
  • 已采纳回答 8月7日
  • 创建了问题 8月6日

悬赏问题

  • ¥15 r语言蛋白组学相关问题
  • ¥15 Python时间序列如何拟合疏系数模型
  • ¥15 求学软件的前人们指明方向🥺
  • ¥50 如何增强飞上天的树莓派的热点信号强度,以使得笔记本可以在地面实现远程桌面连接
  • ¥20 双层网络上信息-疾病传播
  • ¥50 paddlepaddle pinn
  • ¥20 idea运行测试代码报错问题
  • ¥15 网络监控:网络故障告警通知
  • ¥15 django项目运行报编码错误
  • ¥15 STM32驱动继电器