四元素算法

MagicWand（摇摇棒）现货发售！附使用教程！本帖最后由blackblue于2014-6-1809:46编辑说明以及申明：1，先感谢本坛网友catluoq,前些时间我在论坛扔过一个MPU6050DMP的代码,虽然我一开始并不知道是他的原著，所以没有注明作者。不过经过我的试验，我用NXC和ROBOTC都无法顺利开启DMP功能，能写入但读FIFO不成功，全是0......再次感谢他！2，本贴所述的不再是开启DMP功能，是一种完全软件的方法，用四元素解算姿态。算法的精髓来自于一个UK的作者，你搜Madgwick就能找到的！他是开源的.....3,实现所用的器材:NXT一台,需刷ROBOTC固件,MPU6050一片，必要的连接线。其实代码有包括磁力计的，这样的话YAW就不会飘了，下一步吧4,其他一些注意事项看代码中的注解吧........运行以后图就不上了，反正肯定能行，能得到比较稳定快速的ROLL和PITCH！#pragmaconfig(Sensor,S2,,sensorI2CCustomFastSkipStates)//ROBOTC的好处是，I2C的速度比NXC快得多了，我这里把它设到最快速度！//*!!Codeautomaticallygeneratedby'ROBOTC'configurationwizard!!*//#pragmasystemFile#defineAdr0xD0//MPU6050I2C-Adr#definepi3.14159#defineSMOOTH0.2//accellow_passfilterKp,平滑加计输出用的#defineKp2.0//proportionalgaingovernsrateofconvergencetoaccelerometer/magnetometer#defineKi0.005//全局变量定义intgx_offset,gy_offset,gz_offset,ax_offset,ay_offset,az_offset;floatroll,pitch,yaw,roll_g,pitch_g,yaw_g,roll_a,pitch_a,yaw_a;floatq0,q1,q2,q3,exInt,eyInt,ezInt;longlast,now;//---------------------------------//I2C操作时用的结构体，也可以用数组方式typedefstruct{ubytenMsgSize;ubytenDeviceAddress;ubytenLocationPtr;ubytenCommand;}TI2C_Output;TI2C_OutputsOutput;typedefstruct{ubytenMsgSize;ubytenDeviceAddress;ubytenLocationPtr;}TI2C_Readmsg;TI2C_ReadmsgsReadmsg;ubyteI2CReply[14];//I2C读操作数据暂存//初始化函数voidMPU6050_init(){//第一唤醒操作，然后给控制寄予存器写入值，具体看MPU6050寄存器手册吧//注意，里面的所有WAIT是我经过试验确定的，不要过小，过小的话，初始化会不灵光的！sOutput.nMsgSize=3;sOutput.nDeviceAddress=Adr;sOutput.nLocationPtr=0x6B;sOutput.nCommand=0x80;sendI2CMsg(S2,&sOutput.nMsgSize,0);while(nI2CStatus[S2]==STAT_COMM_PENDING)wait1Msec(100);sOutput.nMsgSize=3;sOutput.nDeviceAddress=Adr;sOutput.nLocationPtr=0x19;sOutput.nCommand=0x00;sendI2CMsg(S2,&sOutput.nMsgSize,0);while(nI2CStatus[S2]==STAT_COMM_PENDING)wait1Msec(100);sOutput.nMsgSize=3;sOutput.nDeviceAddress=Adr;sOutput.nLocationPtr=0x1A;sOutput.nCommand=0x00;sendI2CMsg(S2,&sOutput.nMsgSize,0);while(nI2CStatus[S2]==STAT_COMM_PENDING)wait1Msec(50);sOutput.nMsgSize=3;sOutput.nDeviceAddress=Adr;sOutput.nLocationPtr=0x6B;sOutput.nCommand=0x03;sendI2CMsg(S2,&sOutput.nMsgSize,0);while(nI2CStatus[S2]==STAT_COMM_PENDING)wait1Msec(50);sOutput.nMsgSize=3;sOutput.nDeviceAddress=Adr;sOutput.nLocationPtr=0x1B;sOutput.nCommand=0x18;//2000dps-16.4sendI2CMsg(S2,&sOutput.nMsgSize,0);while(nI2CStatus[S2]==STAT_COMM_PENDING)wait1Msec(50);sOutput.nMsgSize=3;sOutput.nDeviceAddress=Adr;sOutput.nLocationPtr=0x1C;sOutput.nCommand=0x10;//4g4096sendI2CMsg(S2,&sOutput.nMsgSize,0);while(nI2CStatus[S2]==STAT_COMM_PENDING)wait1Msec(50);}//endoffinitMPU6050//一次读14个数据voidMPU6050GetRawData(int&_ax,int&_ay,int&_az,int&_gx,int&_gy,int&_gz){sReadmsg.nMsgSize=2;sReadmsg.nDeviceAddress=Adr;sReadmsg.nLocationPtr=0x3B;sendI2CMsg(S2,&sReadmsg.nMsgSize,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,i2cerr%d,nI2CStatus[S2]);}}//ENDMPU6050IICGetAllData//静止状态下确定零飘，注意这里做了ACC和GYRO的，其实ACC的不需要做voidGetOffsetData(int&_gxoffset,int&_gyoffset,int&_gzoffset,int&_axoffset,int&_ayoffset,int&_azoffset){intGx,Gy,Gz,Ax,Ay,Az,i,gx0,gy0,gz0,ax0,ay0,az0;for(i=0;i500;i++){MPU6050GetRawData(Gx,Gy,Gz,Ax,Ay,Az);gx0+=Gx;gy0+=Gy;gz0+=Gz;ax0+=Ax;ay0+=Ay;az0+=Az;}_gxoffset=gx0/500.0;_gyoffset=gy0/500.0;_gzoffset=gz0/500.0;_axoffset=ax0/500.0;_ayoffset=ay0/500.0;_azoffset=az0/500.0;}//endofoffsetval//让MPU6050输出GYRO和ACC对应的ROLL，PITCH，YAW，单位是弧度，注意一定是弧度voidGetMPU6050Data(float&_Rgx,float&_Pgy,float&_Ygz,float&_Rax,float&_Pay,float&_Yaz){intGx,Gy,Gz,Ax,Ay,Az;MPU6050GetRawData(Ax,Ay,Az,Gx,Gy,Gz);_Rgx=(Gx-gx_offset)*pi/(180.0*16.4);//_Pgy=(Gy-gy_offset)*pi/(180.0*16.4);_Ygz=(Gz-gz_offset)*pi/(180.0*16.4);_Rax=(_Rax*(1-SMOOTH)+Ax*SMOOTH)/4096.0;//加计在这里做了低通平滑,以消除一些振动对加计的影响_Pay=(_Pay*(1-SMOOTH)+Ay*SMOOTH)/4096.0;_Yaz=(_Yaz*(1-SMOOTH)+Az*SMOOTH)/4096.0;}这个函数是用来看解算过程需要花多少时间，后面知道了其实可以直接给HALFT赋值，HALFT=1/2*DTfloatdot(){last=now;now=nPgmTime;floatdt=(now-last)/1000.0;//nxtDisplayTextLine(7,dt_time%f,dt);returndt;}//四元素表示姿态时，是很不直观的，所以我们需要转成欧拉角，*57.3(180.0/3.14)是将弧度转为更直观的角度voidtoEuler(){/*STANDARDZYXy=atan2(2*q1*q2-2*q0*q3,2*q0*q0+2*q1*q1-1);p=-asin(2*q1*q3+2*q0*q2);r=atan2(2*q2*q3-2*q0*q1,2*q0*q0+2*q3*q3-1);*/yaw=(atan2(2*q1*q2+2*q0*q3,2*q0*q0+2*q1*q1-1))*57.3;pitch=(-asin(2*q1*q3-2*q0*q2))*57.3;roll=(atan2(2*q2*q3+2*q0*q1,2*q0*q0+2*q3*q3-1))*57.3;}//这个就是最关键的四元素解算算法,建议别指望全部看懂，能用也是本事.......voidupdateIMU(floatgx,floatgy,floatgz,floatax,floatay,floataz){floatnorm,halfT;floatvx,vy,vz;floatex,ey,ez;halfT=dot()/2.0;//normalisethemeasurementsnorm=sqrt(ax*ax+ay*ay+az*az);ax=ax/norm;ay=ay/norm;az=az/norm;//estimateddirectionofgravityvx=2*(q1*q3-q0*q2);vy=2*(q0*q1+q2*q3);vz=q0*q0-q1*q1-q2*q2+q3*q3;//errorissumofcrossproductbetweenreferencedirectionoffieldanddirectionmeasuredbysensorex=(ay*vz-az*vy);ey=(az*vx-ax*vz);ez=(ax*vy-ay*vx);//integralerrorscaledintegralgai