关键词不能为空

当前您在: 主页 > 英语 >

四元素算法

作者:高考题库网
来源: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

四元素算法的相关文章

  • 余华爱情经典语录,余华爱情句子

    余华的经典语录——余华《第七天》40、我不怕死,一点都不怕,只怕再也不能看见你——余华《第七天》4可是我再也没遇到一个像福贵这样令我难忘的人了,对自己的经历如此清楚,

    语文
  • 心情低落的图片压抑,心情低落的图片发朋友圈

    心情压抑的图片(心太累没人理解的说说带图片)1、有时候很想找个人倾诉一下,却又不知从何说起,最终是什么也不说,只想快点睡过去,告诉自己,明天就好了。有时候,突然会觉得

    语文
  • 经典古训100句图片大全,古训名言警句

    古代经典励志名言100句译:好的药物味苦但对治病有利;忠言劝诫的话听起来不顺耳却对人的行为有利。3良言一句三冬暖,恶语伤人六月寒。喷泉的高度不会超过它的源头;一个人的事

    语文
  • 关于青春奋斗的名人名言鲁迅,关于青年奋斗的名言鲁迅

    鲁迅名言名句大全励志1、世上本没有路,走的人多了自然便成了路。下面是我整理的鲁迅先生的名言名句大全,希望对你有所帮助!当生存时,还是将遭践踏,将遭删刈,直至于死亡而

    语文
  • 三国群英单机版手游礼包码,三国群英手机单机版攻略

    三国群英传7五神兽洞有什么用那是多一个武将技能。青龙飞升召唤出东方的守护兽,神兽之一的青龙。玄武怒流召唤出北方的守护兽,神兽之一的玄武。白虎傲啸召唤出西方的守护兽,

    语文
  • 不收费的情感挽回专家电话,情感挽回免费咨询

    免费的情感挽回机构(揭秘情感挽回机构骗局)1、牛牛(化名)向上海市公安局金山分局报案,称自己为了挽回与女友的感情,被一家名为“实花教育咨询”的情感咨询机构诈骗4万余元。

    语文