关键词不能为空

当前您在： 主页 > 英语 >

四元素算法

作者：高考题库网
来源：https://www.bjmy2z.cn/gaokao
2021-02-01 18:34
tags:

-

2021年2月1日发(作者：direct是什么意思)


Magic


Wand


（摇摇棒）现货发售！附 使用教程！



本帖最后由


blackblue


于


2014-6-18 09:46


编辑




说明以及申明：



1

< br>，先感谢本坛网友


catluoq,


前些时间我在论坛扔 过一个


MPU6050 DMP


的代码


,


虽然我一开始并不知道是他的原著，所以没


有注明作者。不过 经过我的试验，我用


NXC


和


ROBO TC


都无法顺利开启


DMP


功能，能写 入但读


FIFO


不成功，全是


0... ...


再


次感谢他！



2


，本贴所述的不再是开启


DMP

功能，是一种完全软件的方法，用四元素解算姿态。算法的精髓来自于一个


UK


的作者，你搜


Madgwick


就能找到的 ！他是开源的


.....



3,



实现所用的器材


:NXT

< p>
一台


,


需刷


ROBOTC


固件


,MPU6050


一片，必要的连 接线。其实代码有包括磁力计的，这样的话


YA


W


就不会飘了，下一步吧



4,

其他一些注意事项看代码中的注解吧


........


< /p>


运行以后图就不上了，反正肯定能行，能得到比较稳定快速的


RO LL


和


PITCH


！

< br>




#pragma config(Sensor, S2,





,











sensorI2CCustomFastSkipStates)



//ROBOTC


的


好处是，


I2C


的速度比


N XC


快得多了，我这里把它设到最快速度！



//*!!Code automatically generated by 'ROBOTC' configuration wizard











!!*//



#pragma systemFile





#define Adr 0xD0



//MPU6050 I2C-Adr


#define pi 3.14159


#define SMOOTH



0.2



//accel low_pass filter Kp,


平滑加计输出用的



#define Kp



2.0



// proportional gain governs rate of convergence to accelerometer/magnetometer


#define Ki



0.005



//


全局变量定义



int gx_offset,gy_offset,gz_offset,ax_of fset,ay_offset,az_offset;


float roll,pi tch,yaw,roll_g,pitch_g,yaw_g,roll_a,pitch_a,yaw_a;


float q0,q1,q2,q3,exInt,eyInt,ezInt;




long last,now;


//---------------------------------



//I2C


操作时用的结构体，也可以用数组 方式



typedef struct{


ubyte nMsgSize;




ubyte nDeviceAddress;




ubyte nLocationPtr;


ubyte nCommand;




} TI2C_Output;




TI2C_Output sOutput;


typedef struct{


ubyte nMsgSize;




ubyte nDeviceAddress;




ubyte nLocationPtr;


} TI2C_Readmsg;




TI2C_Readmsg sReadmsg;




ubyte I2CReply[14]; //I2C


读操作数据暂存




//


初始化函数



void MPU6050_init()


{




//


第一唤醒操作，然后给控制寄予存器写入值，具体看


MPU6050

寄存器手册吧




< p>
//


注意，里面的所有


WAIT

< br>是我经过试验确定的，不要过小，过小的话，初始化会不灵光的！





ze = 3;




eAddress = Adr;




ionPtr = 0x6B;




nd = 0x80;






sendI2CMsg(S2, &ze, 0);




while (nI2CStatus[S2] == STAT_COMM_PENDING)




wait1Msec(100);




ze = 3;




eAddress = Adr;




ionPtr = 0x19;




nd = 0x00;




sendI2CMsg(S2, &ze, 0);




while (nI2CStatus[S2] == STAT_COMM_PENDING)




wait1Msec(100);




ze = 3;




eAddress = Adr;




ionPtr = 0x1A;




nd = 0x00;




sendI2CMsg(S2, &ze, 0);




while (nI2CStatus[S2] == STAT_COMM_PENDING)




wait1Msec(50);




ze = 3;




eAddress = Adr;




ionPtr = 0x6B;




nd = 0x03;




sendI2CMsg(S2, &ze, 0);




while (nI2CStatus[S2] == STAT_COMM_PENDING)




wait1Msec(50);




ze = 3;




eAddress = Adr;




ionPtr = 0x1B;




nd = 0x18;







//2000dps -16.4




sendI2CMsg(S2, &ze, 0);




while (nI2CStatus[S2] == STAT_COMM_PENDING)




wait1Msec(50);




ze = 3;




eAddress = Adr;




ionPtr = 0x1C;




nd = 0x10;









//4g 4096




sendI2CMsg(S2, &ze, 0);




while (nI2CStatus[S2] == STAT_COMM_PENDING)




wait1Msec(50);


}//end off init MPU6050



//


一次读


14


个数 据



void



MPU6050GetRawData(int &_ax,int &_ay,int &_az,int &_gx,int &_gy,int &_gz)


{









ze = 2;








eAddress = Adr;








ionPtr = 0x3B;








sendI2CMsg(S2,&ze, 14);








while (nI2CStatus[S2] == STAT_COMM_PENDING) memset(I2CReply, 0, 14);








if (nI2CStatus[S2] == NO_ERR)



{








readI2CReply(S2,&I2CReply[0] , 14);










_ax = (I2CReply[0]<<8) | I2CReply[1]










_ay = (I2CReply[2]<<8) | I2CReply[3]










_az = (I2CReply[4]<<8) | I2CReply[5]










_gx = (I2CReply[8]<<8) | I2CReply[9]










_gy = (I2CReply[10]<<8) | I2CReply[11] ;










_gz = (I2CReply[12]<<8) | I2CReply[13] ;








}






else { nxtDisplayTextLine(0,



}



// END MPU6050 IIC



Get All Data



//


静止状态下确定零飘 ，注意这里做了


ACC


和


GYRO


的，其实


ACC


的不需要做



void GetOffsetData(int &_gxoffset,int &_gyoffset,int &_gzoffset,int &_axoffset,int &_ayoffset,int &_azoffse


t)


{


int Gx,Gy,Gz,Ax,Ay,Az,i,gx0,gy0,gz0,ax0,ay0,az0;


for (i=0;i<500;i++)


{






MPU6050GetRawData(Gx,Gy,Gz,Ax,Ay,Az);






gx0 +=Gx;






gy0 +=Gy;






gz0 +=Gz;

-


-


-


-


-


-


-


-



本文更新与2021-02-01 18:34，由作者提供，不代表本网站立场，转载请注明出处：https://www.bjmy2z.cn/gaokao/594485.html

返回列表：英语

四元素算法的相关文章

四元素算法随机文章
四元素算法热门标签