51单片机使用mpu6050的的dmp 输出的Yaw一直乱飘是怎么一回事？

ID:928549 · 发表于 2021-7-24 10:49

#include<STC8A.h>
#include<intrins.h>
#include<MPU_I2C.h>
#include<math.h>
#include<DMP.h>
#include<test.h>

#define q30 1073741824.0f;
uchar dmpdatas[42];
signed long dmpdata_group[4];
float q0 = 1.0f,q1 = 0.0f,q2 = 2.0f,q3 = 0.0f;//四元数
float Yaw,Pitch,Roll,Temp; //加载并配置DMP 数字运动处理引擎
uchar dmpInitialize(void)
{
uchar hwRevision,otpValid,mpuIntStatus;
uchar xgOffsetTC,ygOffsetTC,zgOffsetTC;
uint fifoCount;
I2C_writebit(PWR_MGMT_1,7,1);//复位MPU6050
Delay10ms();
I2C_writebit(PWR_MGMT_1,6,0);
I2C_send(0x6D,0x70);
I2C_send(0x6E,0x06);
I2C_Recv_t(0x6F,&hwRevision);//读取
//hwRevision = I2C_Recv(0x6f);
I2C_send(0x6D,0); //重置内存bank选择
readBit(0x00,0,&otpValid); //读取otp bank 有效标志
readBits(0x00,6,6,&xgOffsetTC); //读取陀螺仪偏置TC值 x
readBits(0x01,6,6,&ygOffsetTC);  //读取陀螺仪偏置TC值 y
readBits(0x02,6,6,&zgOffsetTC);  //读取陀螺仪偏置TC值 z
////////////////////////////////
I2C_writebit(USER_CTRL,1,1); //I2C总线主控复位
Delay10ms(); if ((loadfirmware()) == 0) return 0;//加载dmp代码到内存
//loadfirmware();
// uart1sendstr("TEST\r\n");
if ((loadcfgupd()) ==0) return 0;//配置dmp
I2C_writebits(PWR_MGMT_1,2,3,0x03); //设置脉冲源z陀螺
I2C_send(0x38,0x12);//设置DMP和FIFO_OFLOW启用中断
I2C_send(0x19,4);//设置采样率为200 hz  (1khz / (1 + 4) = 200 Hz)
I2C_writebits(0x1A,5,3,0x1); //设置外部帧同步TEMP_UT_L[0]
I2C_writebits(0x1A,2,3,0x03);//设置DLPF带宽42赫兹
I2C_writebits(0x1B,4,2,0x03);//陀螺仪灵敏度+-2000
I2C_send(0x70,0x03); //设置DMP配置字节
I2C_send(0x71,0x00); //设置DMP配置字节???
I2C_writebit(0x00,0,0);//清除otp bank 标志
  I2C_writebits(0x00,6,6,xgOffsetTC); //清除陀螺仪偏置TC值 x
I2C_writebits(0x01,6,6,ygOffsetTC);  //清除陀螺仪偏置TC值 y
I2C_writebits(0x02,6,6,zgOffsetTC);  //清除陀螺仪偏置TC值 z  dmp_update(0);
dmp_update(5);
I2C_writebit(0x6A,2,1); //复位fifo
fifoCount = getFIFOcount(); //读取FIFO计数
I2C_writebit(0x6A,2,1); //复位fifo
///////////////////////////////////////////////////////////////////////////////////////////
I2C_send(0x1F,2); //运动检测阈值设置为2
I2C_send(0x21,156); //零运动检测阈值为156 （是否修改）
I2C_send(0x20,80);//设置运动检测持续时间至80
I2C_send(0x22,0);//设置运动检测时间0
I2C_writebit(0x6A,2,1); //复位fifo
I2C_writebit(0x6A,6,1); //使能fifo
I2C_writebit(0x6A,7,1); //使能dmp
I2C_writebit(0x6A,3,1); //复位dmp  dmp_update(12);
  dmp_update(17);
  dmp_update(28);
while ((fifoCount = getFIFOcount()) < 3 ); //等待fifo计数大于2
I2C_writebit(0x6A,2,1);//复位fifo
I2C_Recv_t(0x3A,&mpuIntStatus);//读取中断状态
//mpuIntStatus = I2C_Recv(0x3A);
// if(dmp_update(35)==0) return 0; //最后更新6/7(函数未知)dmpUpdates数组第六行
  dmp_update(35);
while ((fifoCount = getFIFOcount()) < 3 );
I2C_writebit(0x6A,2,1);  //复位fifo

I2C_Recv_t(0x3A,&mpuIntStatus);//读取中断状态
//mpuIntStatus = I2C_Recv(0x3A);
// if(dmp_update(40)==0) return 0; //最后更新7/7(函数未知)dmpUpdates数组第七行
  dmp_update(40);
  I2C_writebit(0x6A,7,0); //禁用dmp
I2C_writebit(0x6A,2,1); //复位fifo
I2C_Recv_t(0x3A,&mpuIntStatus);//读取中断状态
//mpuIntStatus = I2C_Recv(0x3A);
return 1;
}
uchar getDevideID(void) //验证id
{
uchar b = 0;
readBits(0x75,6,6,&b);
return b== 0x34;

}
void init_MPU6050(void)
{
I2C_writebits(PWR_MGMT_1,2,3,0x01); //电源管理
I2C_send(GYRO_CONFIG, 0x00);
I2C_send(ACCEL_CONFIG, 0x01);
I2C_writebit(PWR_MGMT_1,6,1); //睡眠状态
if (getDevideID())
{ if(!(dmpInitialize()))
while(1);
} I2C_writebit(0x6A,2,1); //复位fifo
  I2C_writebit(0x6A,7,1); //使能dmp
}
void read_FIFO(void)
{
uchar zd;
uint i;
i = getFIFOcount();//读取fifo计数
I2C_Recv_t(0x3A,&zd);//读取中断状态 //uart1sendstr("ddd\r\n");
if ((zd & 0x10)||i>=840) //判断fifo是否溢出
{
uart1sendstr("yichu\r\n");
I2C_writebit(0x6A,2,1); //复位fifo
}
else if (zd &0x02)
{
uart1sendstr("eee\r\n");
while (i<42) i=getFIFOcount();
if (readdmp(dmpdatas)) //读取fifo数据
{
dmpdata_group[0] = ((long)dmpdatas[0] << 24) | ((long)dmpdatas[1] << 16) | ((long)dmpdatas[2] << 8) | dmpdatas[3];
   dmpdata_group[1] = ((long)dmpdatas[4] << 24) | ((long)dmpdatas[5] << 16) | ((long)dmpdatas[6] << 8) | dmpdatas[7];
   dmpdata_group[2] = ((long)dmpdatas[8] << 24) | ((long)dmpdatas[9] << 16) | ((long)dmpdatas[10] << 8) | dmpdatas[11];
   dmpdata_group[3] = ((long)dmpdatas[12] << 24) | ((long)dmpdatas[13] << 16) | ((long)dmpdatas[14] << 8) | dmpdatas[15];
   q0 = (float)dmpdata_group[0] / q30;    //取四元数
//q0 = 131.1f;
      q1 = (float)dmpdata_group[1] / q30;
      q2 = (float)dmpdata_group[2] / q30;
      q3 = (float)dmpdata_group[3] / q30;
// Roll = Atan2(2 *(Y * Z + W * X) , W * W -X * X -Y * Y + Z * Z)
   Roll = atan2(2 * q2 * q3 + 2 * q0 * q1, -2 * q1 * q1 - 2 * q2* q2 + 1)* 42.975;
// Pitch = asin(-2 * (X * Z - W * Y))
   Pitch = asin(-2 * q1 * q3 + 2 * q0* q2)* 42.975;
// Yaw = atan2(2 * (X * Y + W * Z) ,W * W + X * X - Y * Y - Z * Z)
   Yaw = atan2(2*(q1*q2 + q0*q3),q0*q0+q1*q1-q2*q2-q3*q3) * 42.975; }
}
}

ID:928549 · 发表于 2021-7-24 10:50

void Delay10ms() //@24.000MHz
{
unsigned char i;
uint j;
for (j=0;j<1000;j++)
{
_nop_();
_nop_();
i = 57;
while (--i);
}
}
uchar loadfirmware(void)
{
uint datanum = 0; //dmp固件写入标志
uchar ye,i,j;
uchar bank = 0;
uchar addr = 0;
for (;bank<8;bank++)
{
if(bank ==7)
i=8;
else
i=16;
for (ye=0;ye<i;ye++)
{

I2C_send(0x6d,bank);
I2C_send(0x6e,addr);
MPU_I2C_start();
MPU_I2C_SendData(SlaveAddress);
      MPU_I2C_RecvACK();
MPU_I2C_SendData(0x6f);
MPU_I2C_RecvACK();
      //uart1sendstr("TEST\r\n");
for (j=0;j<16;j++)
{
MPU_I2C_SendData(dmpmemorydata[datanum++]);
MPU_I2C_RecvACK();
}
addr +=16;
MPU_STOP();
}
}
I2C_send(0x6d,7);
I2C_send(0x6e,addr);
MPU_I2C_start();
MPU_I2C_SendData(SlaveAddress);
MPU_I2C_RecvACK();
MPU_I2C_SendData(0x6f);
MPU_I2C_RecvACK();
for (i=0;i<9;i++)
{
MPU_I2C_SendData(dmpmemorydata[datanum++]);
MPU_I2C_RecvACK();
}
MPU_STOP();
return 1;
}
uchar loadcfgupd(void)  //DMP设置
{
uchar line; //一共写入30行数据
uchar bank; //页
uchar datacounts = 0;//DMP设置数据标志位
uchar bytes2write; //数据长度
uchar offset; //偏移地址
uchar writingcounts; //数据写入标志byte2write一同使用
uchar special;
for (line=0;line<30;line++)
{
bank = dmpcfgupddata[datacounts++];
offset = dmpcfgupddata[datacounts++];
bytes2write = dmpcfgupddata[datacounts++];
I2C_send(0x6d,bank);
I2C_send(0x6e,offset);
   MPU_I2C_start();
MPU_I2C_SendData(SlaveAddress);
   MPU_I2C_RecvACK();
MPU_I2C_SendData(0x6f);
MPU_I2C_RecvACK();
for (writingcounts=0;writingcounts<bytes2write;writingcounts++)
{
MPU_I2C_SendData(dmpcfgupddata[datacounts++]);
MPU_I2C_RecvACK();
}
if(0 == bytes2write) //?????
{
special = dmpcfgupddata[datacounts++];
if (0x01 == special)
{
I2C_send(0x38,0x32);
}
else
return 0;
}
}
MPU_STOP();
return 1;
}
/*最后更新*/
uchar dmp_update(uchar datacounts)
{
uchar writingcounts,bank,offset,bytes2write;
bank = dmpUpdates[datacounts++];
offset = dmpUpdates[datacounts++];
bytes2write = dmpcfgupddata[datacounts++];
I2C_send(0x6d,bank);
I2C_send(0x6e,offset);
MPU_I2C_start();
MPU_I2C_SendData(SlaveAddress);
MPU_I2C_RecvACK();
MPU_I2C_SendData(0x6f);
MPU_I2C_RecvACK();
for (writingcounts=0;writingcounts<bytes2write;writingcounts++)
{
MPU_I2C_SendData(dmpcfgupddata[datacounts++]);//写入DMP配置数据
MPU_I2C_RecvACK();
}
MPU_STOP();
return 1;
}
/*读取FIFO计数*/
uint getFIFOcount()
{
uchar i[2];
I2C_Recvs(0x72,2,i);
return ((i[0]<<8)+i[1]);
}
uchar readdmp(uchar *Data)
{
return I2C_Recvs(0x74,42,Data);
}

ID:928549 · 发表于 2021-7-24 11:29

1093562897 发表于 2021-7-24 10:50
void Delay10ms() //@24.000MHz
{
unsigned char i;

ID:928549 · 发表于 2021-7-24 14:43

图发不出来，我描述一下，就是输出的数据基本是错误的一会+50，一会-20，一会+110，又一会+20这样的，也不像是零飘的样子，感觉就是单纯的数据错误

ID:928549 · 发表于 2021-7-24 17:34

换了一块陀螺仪还是同样的问题，感觉是程序的问题，但不知道哪里错了，挣扎了好久

ID:928549 · 发表于 2021-7-30 15:10

求求了，有没有大佬指导下，按照手册重写了代码，（虽然发现了不少问题）但最后发现还是上次的问题还是没有解决，呜呜呜

ID:928549 · 发表于 2021-7-30 17:17

会不会是陀螺仪收到电磁干扰严重才会这样？，但看着输出的数据又不太像，由于电路是我的一个师兄设计的，当时也是初学者，也不排除有这个可能

ID:928549 · 发表于 2021-8-13 11:25

大家好，我找到问题所在了，是stc8a8k系列芯片硬件I2c有点问题，换成软件模拟I2C就没问题了

ID:928549 · 发表于 2021-8-13 11:28

这是我的测试代码，写的有点乱

ID:587208 · 发表于 2021-12-23 11:33

成长了。

ID:994053 · 发表于 2021-12-24 14:18

stc8a8k系列芯片硬件I2c有点问题

ID:493177 · 发表于 2021-12-27 15:52

可以加个好友探讨一下6050的问题吗

帐号		自动登录	找回密码
密码			立即注册