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的問題嗎

帳號		自動登錄	找回密碼
密碼			立即注冊

久久久久久久999_99精品久久精品一区二区爱城_成人欧美一区二区三区在线播放_国产精品日本一区二区不卡视频_国产午夜视频_欧美精品在线观看免费

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