想要获得MPU6050的姿态角数据，从mpu的数据手册上看到有dwp功能可以快速获得四元数及欧拉角。
但是从mpu6050的fifo中读出的数据不对。
我是参考
void i2cinit(void)
{
// 16 M晶振，400K TWI 速率
TWBR=0x0a;
TWAR=0x00;
TWCR=0x94;
TWSR=0x00;
}
void showfloat(unsigned char x,unsigned char y,float f_2show)
{
unsigned char str2show[10];
lcd_gotoxy(x,y);
ftoa( f_2show,10,str2show);
lcd_puts(str2show);
}
void showlong(unsigned char x,unsigned char y,long l_2show)
{
unsigned char str2show[15];
lcd_gotoxy(x,y);
ltoa( l_2show,str2show);
lcd_puts(str2show);
}
//以下TWI的基本操作函数是从马潮老师的书中复制而来，在此表示谢意
unsigned char i2cstart(void)
{
TWCR=(1<<TWINT)|(1<<TWSTA)|(1<<TWEN);
while(!(TWCR&(1<<TWINT))){};
return 1;
}
void i2cstop(void)
{
TWCR=(1<<TWINT)|(1<<TWEN)|(1<<TWSTO);
}
unsigned char i2cwbyte(unsigned char c)
{
unsigned char ack=1;
TWDR=c;
TWCR=(1<<TWINT)|(1<<TWEN);
while(!(TWCR&(1<<TWINT))){};
if((TWSR&0xF8)!=TW_MT_SLA_ACK)
ack=0;
return ack;
}
unsigned char i2crbyte(unsigned char ack)
{
if(ack)
TWCR=(1<<TWINT)|(1<<TWEN)|(1<<TWEA);
else
TWCR=(1<<TWINT)|(1<<TWEN);
while(!(TWCR&(1<<TWINT))){};
return(TWDR);
}
unsigned char i2cread(unsigned char address)
{
unsigned char data;
i2cstart();
i2cwbyte(i2cw);
i2cwbyte(address);
i2cstart();
i2cwbyte(i2cr);
data=i2crbyte(0);
i2cstop();
return data;
}
void i2cwrite(unsigned char address,unsigned char data)
{
i2cstart();
i2cwbyte(i2cw);
i2cwbyte(address);
i2cwbyte(data);
i2cstop();
}
void readdmp(void)
{
unsigned char dmpnum;
i2cstart();
i2cwbyte(i2cw);
i2cwbyte(0x74);
i2cstart();
i2cwbyte(i2cr);
for (dmpnum=0;dmpnum<41;dmpnum++)
{
dmpdatas[dmpnum]=i2crbyte(1);
};
dmpdatas[41]=i2crbyte(0);
i2cstop();
}
long getdmplong(unsigned char address)
{
long dmptempl;
dmptempl = dmpdatas[address];
dmptempl <<= 8;
dmptempl |= dmpdatas[address+1];
dmptempl <<= 8;
dmptempl |= dmpdatas[address+2];
dmptempl <<= 8;
dmptempl |= dmpdatas[address+3];
return dmptempl;
}
void getquaternion(void)
{
quartf[0] = getdmplong(0)/1073741824.0;
quartf[1] = getdmplong(4)/1073741824.0;
quartf[2] = getdmplong(8)/1073741824.0;
quartf[3] = getdmplong(12)/1073741824.0;
}
void getgyro(void)
{
gyrof[0]=getdmplong(16)/16.384;
gyrof[1]=getdmplong(20)/16.384;
gyrof[2]=getdmplong(24)/16.384;
}
void getaccel(void)
{
accelf[0]=getdmplong(28)/32768.0;
accelf[1]=getdmplong(32)/32768.0;
accelf[2]=getdmplong(36)/32768.0;
}
void init6050(void)
{
i2cwrite(0x6b , 0b10000000);
delay_ms(200);
i2cwrite(0x6b , 0b00000001); //No sleep , 3 PLL with X axis gyroscope reference
i2cwrite(0x23 , 0b00000000); //FIFO 11111000 ,temp,gx,gy,gz,accel
i2cwrite(0x19 , 0b00000101); //SMPLRT_DIV,+1 ,要能被 FIFO rate 整除
i2cwrite(0x1a , 0b00000011); //00 111,8k ,other 1K DLPF
i2cwrite(0x1b , 0b00011000); //00011000 GYRO-CFG +-2000d/s
i2cwrite(0x1c , 0b00000000); //ACCEL +-2g/s
}
void loadfirmware(void)
{
unsigned int datanum=0;
unsigned char bank;
unsigned lastbank=1;
for (datanum=0;datanum<1929;datanum++)
{
bank=datanum/256;
if(lastbank!=bank){
i2cwrite(0x6d,bank);
i2cwrite(0x6e,0);
}
i2cwrite(0x6f,dmpmemorydata[datanum]);
lastbank=bank;
};
}
void loadcfgupd(void)
{
unsigned char line;
unsigned char bank;
unsigned char datacounts=0;
unsigned char bytes2write;
unsigned char offset;
unsigned char writingcounts;
for (line=0;line<37;line++)
{
bank=dmpcfgupddata[datacounts];
datacounts++;
offset=dmpcfgupddata[datacounts];
datacounts++;
bytes2write=dmpcfgupddata[datacounts];
i2cwrite(0x6d,bank);
i2cwrite(0x6e,offset);
for (writingcounts=0;writingcounts<bytes2write;writingcounts++)
{
datacounts++;
i2cwrite(0x6f,dmpcfgupddata[datacounts]);
};
datacounts++;
};
}
void getyawpitchroll(void)
{
// Roll = Atan2(2 *(Y * Z + W * X) , W * W -X * X -Y * Y + Z * Z)
yprf[0] = atan2(2 *(quartf[2] * quartf[3] + quartf[0] * quartf[1]) , quartf[0] * quartf[0] -quartf[1] * quartf[1] -quartf[2] * quartf[2] + quartf[3] * quartf[3])*57.3;
// Pitch = asin(-2 * (X * Z - W * Y))
yprf[1]=asin(-2*(quartf[1]*quartf[3]-quartf[0]*quartf[2]))*57.3;
// Yaw = atan2(2 * (X * Y + W * Z) ,W * W + X * X - Y * Y - Z * Z)
yprf[2]=atan2(2*(quartf[1] * quartf[2] + quartf[0] * quartf[3]) , quartf[0] * quartf[0] +quartf[1] * quartf[1] -quartf[2] * quartf[2] - quartf[3] * quartf[3])*57.3;
}
void main(void)
{
unsigned int fifocounts;
// Bit Rate: 400.000 kHz
TWBR=0x0a;
TWAR=0x00;
TWCR=0x94;
TWSR=0x00;
// Alphanumeric LCD initialization
// Connections specified in the
// Project|Configure|C Compiler|Libraries|Alphanumeric LCD menu:
// RS - PORTA Bit 2
// RD - PORTA Bit 1
// EN - PORTA Bit 3
// D4 - PORTA Bit 4
// D5 - PORTA Bit 5
// D6 - PORTA Bit 6
// D7 - PORTA Bit 7
// Characters/line: 16
lcd_init(16);
i2cinit();
lcd_clear();
init6050();
loadfirmware();
loadcfgupd();
i2cwrite(0x70 , 0b00000011);
i2cwrite(0x71 , 0b00000000);
i2cwrite(0x6a , 0b11001100);
while(1)
{
do
{
do {
fifocounts=i2cread(0x72);
fifocounts=fifocounts<<8;
fifocounts=fifocounts+i2cread(0x73); //读出FIFO里有多少数据
//showlong(12,3,fifocounts);
}
while (fifocounts<42);
readdmp();
fifocounts=i2cread(0x72);
fifocounts=fifocounts<<8;
fifocounts=fifocounts+i2cread(0x73);
}while(fifocounts>=42);
getquaternion();
//getgyro();
//getaccel();
getyawpitchroll();
showlong(12,0,dmpdatas[0]); //显示一下原始数据，看有没有规律
showlong(12,1,dmpdatas[1]);
showfloat(0,0,yprf[0]); //显示 ROLL PITCH YAW 值
showfloat(0,1,yprf[1]);
showfloat(0,2,yprf[2]);
delay_ms(200);
lcd_clear();
}
}
写的代码（不是用的库函数）。
有没有大虾能都指导一下不用dwp的库函数如何正确读出fifo中的内容。
|
楼主
标题置顶
标题高亮
