用51单片机写mpu6050的姿态角程序发现了问题有没有人可以帮我一下

烈酒和刀 · 发表于 2015-10-7 22:45:07

#include"imu.h"
#include <math.H>
#define pi 3.14159265f
#define Kp 0.8f
#define Ki 0.001f
#define halfT 0.004f
float idata q0=1,q1=0,q2=0,q3=0;
float idata exInt=0,eyInt=0,ezInt=0;
void dmp(float gx, float gy, float gz, float ax, float ay, float az,double *rx,double *ry,double *rz)
{
float idata norm;
float idata vx, vy, vz;
float idata ex, ey, ez;

float idata q0q0 = q0*q0;
float idata q0q1 = q0*q1;
float idata q0q2 = q0*q2;
float idata q0q3 = q0*q3;
float idata q1q1 = q1*q1;
float idata q1q2 = q1*q2;
float idata q1q3 = q1*q3;
float idata q2q2 = q2*q2;
float idata q2q3 = q2*q3;
float idata q3q3 = q3*q3;

norm = sqrt(ax*ax + ay*ay + az*az);
ax = ax /norm;
ay = ay / norm;
az = az / norm;

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) ;

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;

*rx=asin(2*(q0*q2-q1*q3 ))* 57.2957795f; // 俯仰
*ry=asin(2*(q0*q1+q2*q3 ))* 57.2957795f; // 横滚
*rz=atan2(2*(q1*q2 + q0*q3),q0*q0+q1*q1 -q2*q2-q3*q3) * 57.3;
}

imu(Angle_gx*0.0174533,Angle_gy*0.0174533,Angle_gz*0.0174533,Angle_ax,Angle_ay,Angle_az); 其中数据都是经过原始数据/LSB的。
然后发现了得到的数据不正常，因为得到的数据都是随着时间缓慢变化，不是跟随着芯片运动而变化的

jsxh538 · 发表于 2015-10-7 22:45:08

回复【7楼】烈酒和刀:
---------------------------------
12T的单片机，你跑浮点数没有死机就算你幸运了。用这个已经淘汰的、慢得比蜗牛还慢的不行的。你必须换速度快，特别是运算浮点数能力强的。STC15W4K48S4这个便宜，7块钱一块。最好是STM32F4。我最开始不懂，用89做，直接跪了。后来换了STC15W4K48S4系列，但是遇到了很多奇葩问题。有时候某寄存器某位写1了就直接不干活了。最后用的STM32F405RGT6.感觉非常不错，就是死贵。22块钱涨到36块钱。