51單片機(jī)使用mpu6050的的dmp 輸出的Yaw一直亂飄是怎么一回事？

ID:928549 · 發(fā)表于 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;//四元數(shù)
float Yaw,Pitch,Roll,Temp; //加載并配置DMP 數(shù)字運(yùn)動(dòng)處理引擎
uchar dmpInitialize(void)
{
uchar hwRevision,otpValid,mpuIntStatus;
uchar xgOffsetTC,ygOffsetTC,zgOffsetTC;
uint fifoCount;
I2C_writebit(PWR_MGMT_1,7,1);//復(fù)位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); //重置內(nèi)存bank選擇
readBit(0x00,0,&otpValid); //讀取otp bank 有效標(biāo)志
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總線主控復(fù)位
Delay10ms(); if ((loadfirmware()) == 0) return 0;//加載dmp代碼到內(nèi)存
//loadfirmware();
// uart1sendstr("TEST\r\n");
if ((loadcfgupd()) ==0) return 0;//配置dmp
I2C_writebits(PWR_MGMT_1,2,3,0x03); //設(shè)置脈沖源z陀螺
I2C_send(0x38,0x12);//設(shè)置DMP和FIFO_OFLOW啟用中斷
I2C_send(0x19,4);//設(shè)置采樣率為200 hz  (1khz / (1 + 4) = 200 Hz)
I2C_writebits(0x1A,5,3,0x1); //設(shè)置外部幀同步TEMP_UT_L[0]
I2C_writebits(0x1A,2,3,0x03);//設(shè)置DLPF帶寬42赫茲
I2C_writebits(0x1B,4,2,0x03);//陀螺儀靈敏度+-2000
I2C_send(0x70,0x03); //設(shè)置DMP配置字節(jié)
I2C_send(0x71,0x00); //設(shè)置DMP配置字節(jié)???
I2C_writebit(0x00,0,0);//清除otp bank 標(biāo)志
  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); //復(fù)位fifo
fifoCount = getFIFOcount(); //讀取FIFO計(jì)數(shù)
I2C_writebit(0x6A,2,1); //復(fù)位fifo
///////////////////////////////////////////////////////////////////////////////////////////
I2C_send(0x1F,2); //運(yùn)動(dòng)檢測(cè)閾值設(shè)置為2
I2C_send(0x21,156); //零運(yùn)動(dòng)檢測(cè)閾值為156 （是否修改）
I2C_send(0x20,80);//設(shè)置運(yùn)動(dòng)檢測(cè)持續(xù)時(shí)間至80
I2C_send(0x22,0);//設(shè)置運(yùn)動(dòng)檢測(cè)時(shí)間0
I2C_writebit(0x6A,2,1); //復(fù)位fifo
I2C_writebit(0x6A,6,1); //使能fifo
I2C_writebit(0x6A,7,1); //使能dmp
I2C_writebit(0x6A,3,1); //復(fù)位dmp  dmp_update(12);
  dmp_update(17);
  dmp_update(28);
while ((fifoCount = getFIFOcount()) < 3 ); //等待fifo計(jì)數(shù)大于2
I2C_writebit(0x6A,2,1);//復(fù)位fifo
I2C_Recv_t(0x3A,&mpuIntStatus);//讀取中斷狀態(tài)
//mpuIntStatus = I2C_Recv(0x3A);
// if(dmp_update(35)==0) return 0; //最后更新6/7(函數(shù)未知)dmpUpdates數(shù)組第六行
  dmp_update(35);
while ((fifoCount = getFIFOcount()) < 3 );
I2C_writebit(0x6A,2,1);  //復(fù)位fifo

I2C_Recv_t(0x3A,&mpuIntStatus);//讀取中斷狀態(tài)
//mpuIntStatus = I2C_Recv(0x3A);
// if(dmp_update(40)==0) return 0; //最后更新7/7(函數(shù)未知)dmpUpdates數(shù)組第七行
  dmp_update(40);
  I2C_writebit(0x6A,7,0); //禁用dmp
I2C_writebit(0x6A,2,1); //復(fù)位fifo
I2C_Recv_t(0x3A,&mpuIntStatus);//讀取中斷狀態(tài)
//mpuIntStatus = I2C_Recv(0x3A);
return 1;
}
uchar getDevideID(void) //驗(yàn)證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); //睡眠狀態(tài)
if (getDevideID())
{ if(!(dmpInitialize()))
while(1);
} I2C_writebit(0x6A,2,1); //復(fù)位fifo
  I2C_writebit(0x6A,7,1); //使能dmp
}
void read_FIFO(void)
{
uchar zd;
uint i;
i = getFIFOcount();//讀取fifo計(jì)數(shù)
I2C_Recv_t(0x3A,&zd);//讀取中斷狀態(tài) //uart1sendstr("ddd\r\n");
if ((zd & 0x10)||i>=840) //判斷fifo是否溢出
{
uart1sendstr("yichu\r\n");
I2C_writebit(0x6A,2,1); //復(fù)位fifo
}
else if (zd &0x02)
{
uart1sendstr("eee\r\n");
while (i<42) i=getFIFOcount();
if (readdmp(dmpdatas)) //讀取fifo數(shù)據(jù)
{
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;    //取四元數(shù)
//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 · 發(fā)表于 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固件寫入標(biāo)志
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設(shè)置
{
uchar line; //一共寫入30行數(shù)據(jù)
uchar bank; //頁(yè)
uchar datacounts = 0;//DMP設(shè)置數(shù)據(jù)標(biāo)志位
uchar bytes2write; //數(shù)據(jù)長(zhǎng)度
uchar offset; //偏移地址
uchar writingcounts; //數(shù)據(jù)寫入標(biāo)志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配置數(shù)據(jù)
MPU_I2C_RecvACK();
}
MPU_STOP();
return 1;
}
/*讀取FIFO計(jì)數(shù)*/
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 · 發(fā)表于 2021-7-24 11:29

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

ID:928549 · 發(fā)表于 2021-7-24 14:43

圖發(fā)不出來(lái)，我描述一下，就是輸出的數(shù)據(jù)基本是錯(cuò)誤的一會(huì)+50，一會(huì)-20，一會(huì)+110，又一會(huì)+20這樣的，也不像是零飄的樣子，感覺就是單純的數(shù)據(jù)錯(cuò)誤

ID:928549 · 發(fā)表于 2021-7-24 17:34

換了一塊陀螺儀還是同樣的問(wèn)題，感覺是程序的問(wèn)題，但不知道哪里錯(cuò)了，掙扎了好久

ID:928549 · 發(fā)表于 2021-7-30 15:10

求求了，有沒(méi)有大佬指導(dǎo)下，按照手冊(cè)重寫了代碼，（雖然發(fā)現(xiàn)了不少問(wèn)題）但最后發(fā)現(xiàn)還是上次的問(wèn)題還是沒(méi)有解決，嗚嗚嗚

ID:928549 · 發(fā)表于 2021-7-30 17:17

會(huì)不會(huì)是陀螺儀收到電磁干擾嚴(yán)重才會(huì)這樣？，但看著輸出的數(shù)據(jù)又不太像，由于電路是我的一個(gè)師兄設(shè)計(jì)的，當(dāng)時(shí)也是初學(xué)者，也不排除有這個(gè)可能

ID:928549 · 發(fā)表于 2021-8-13 11:25

大家好，我找到問(wèn)題所在了，是stc8a8k系列芯片硬件I2c有點(diǎn)問(wèn)題，換成軟件模擬I2C就沒(méi)問(wèn)題了

ID:928549 · 發(fā)表于 2021-8-13 11:28

這是我的測(cè)試代碼，寫的有點(diǎn)亂

ID:587208 · 發(fā)表于 2021-12-23 11:33

成長(zhǎng)了。

ID:994053 · 發(fā)表于 2021-12-24 14:18

stc8a8k系列芯片硬件I2c有點(diǎn)問(wèn)題

ID:493177 · 發(fā)表于 2021-12-27 15:52

可以加個(gè)好友探討一下6050的問(wèn)題嗎

帳號(hào)		自動(dòng)登錄	找回密碼
密碼			立即注冊(cè)