用ROBOTC写的四元素姿态解算代码

blackblue

说明以及申明：
1，先感谢本坛网友catluoq,前些时间我在论坛扔过一个MPU6050 DMP的代码,虽然我一开始并不知道是他的原著，所以没有注明作者。不过经过我的试验，我用NXC和ROBOTC都无法顺利开启DMP功能，能写入但读FIFO不成功，全是0......再次感谢他！
2，本贴所述的不再是开启DMP功能，是一种完全软件的方法，用四元素解算姿态。算法的精髓来自于一个UK的作者，你搜Madgwick就能找到的！他是开源的.....
3,  实现所用的器材:NXT一台,需刷ROBOTC固件,MPU6050一片，必要的连接线。其实代码有包括磁力计的，这样的话YAW就不会飘了，下一步吧
4, 其他一些注意事项看代码中的注解吧........
运行以后图就不上了，反正肯定能行，能得到比较稳定快速的ROLL和PITCH！


#pragma config(Sensor, S2,     ,               sensorI2CCustomFastSkipStates)   //ROBOTC的好处是，I2C的速度比NXC快得多了，我这里把它设到最快速度！
//*!!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_offset,ay_offset,az_offset;
float roll,pitch,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寄存器手册吧
   //注意，里面的所有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.4
   sendI2CMsg(S2, &sOutput.nMsgSize, 0);
   while (nI2CStatus[S2] == STAT_COMM_PENDING)
   wait1Msec(50);
   sOutput.nMsgSize = 3;
   sOutput.nDeviceAddress = Adr;
   sOutput.nLocationPtr = 0x1C;
   sOutput.nCommand = 0x10;             //4g 4096
   sendI2CMsg(S2, &sOutput.nMsgSize, 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)
{

        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, "i2c err %d", nI2CStatus[S2]);}

}  // END MPU6050 IIC  Get All Data

//静止状态下确定零飘，注意这里做了ACC和GYRO的，其实ACC的不需要做
void GetOffsetData(int &_gxoffset,int &_gyoffset,int &_gzoffset,int &_axoffset,int &_ayoffset,int &_azoffset)
{
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;
      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;
} //end of offset val

//让MPU6050输出GYRO和ACC对应的ROLL，PITCH，YAW，单位是弧度，注意一定是弧度
void GetMPU6050Data(float &_Rgx,float &_Pgy,float &_Ygz,float &_Rax,float &_Pay,float &_Yaz)
{
   int Gx,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 * DT
float dot()
  {
    last=now;
    now=nPgmTime;
   float dt=(now-last)/1000.0;
   //nxtDisplayTextLine(7, "dt_time %f",dt);
   return dt;
  }
  //四元素表示姿态时，是很不直观的，所以我们需要转成欧拉角，*57.3(180.0/3.14)是将弧度转为更直观的角度
   void toEuler()
  {
    /* STANDARD ZYX
     y=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;
  }

//这个就是最关键的四元素解算算法,建议别指望全部看懂，能用也是本事.......
void updateIMU(float gx, float gy, float gz, float ax, float ay, float az) {
    float norm,halfT;
    float vx, vy, vz;
    float ex, ey, ez;
    halfT=dot()/2.0;
    // normalise the measurements
    norm = sqrt(ax*ax + ay*ay + az*az);
    ax = ax / norm;
    ay = ay / norm;
    az = az / norm;
    // estimated direction of gravity
    vx = 2*(q1*q3 - q0*q2);
    vy = 2*(q0*q1 + q2*q3);
    vz = q0*q0 - q1*q1 - q2*q2 + q3*q3;
    // error is sum of cross product between reference direction of field and direction measured by sensor
    ex = (ay*vz - az*vy);
    ey = (az*vx - ax*vz);
    ez = (ax*vy - ay*vx);
    // integral error scaled integral gain
    exInt = exInt + ex*Ki;
    eyInt = eyInt + ey*Ki;
    ezInt = ezInt + ez*Ki;
    // adjusted gyroscope measurements
    gx = gx + Kp*ex + exInt;
    gy = gy + Kp*ey + eyInt;
    gz = gz + Kp*ez + ezInt;
    // integrate quaternion rate and normalise
    q0 = q0 + (-q1*gx - q2*gy - q3*gz)*halfT;
    q1 = q1 + (q0*gx + q2*gz - q3*gy)*halfT;
    q2 = q2 + (q0*gy - q1*gz + q3*gx)*halfT;
    q3 = q3 + (q0*gz + q1*gy - q2*gx)*halfT;
    // normalise quaternion
    norm = sqrt(q0*q0 + q1*q1 + q2*q2 + q3*q3);
    q0 = q0 / norm;
    q1 = q1 / norm;
    q2 = q2 / norm;
    q3 = q3 / norm;
    toEuler();
  }






task main()
{
    exInt=0;
    eyInt=0;
    ezInt=0;
    q0=1.0;
    q1=q2=q3=0;
  SensorType[S2] = sensorI2CCustomFastSkipStates;
  wait10Msec(500);
  nI2CBytesReady[S2]=0;
  while (nI2CStatus[S2] == STAT_COMM_PENDING)
  wait1Msec(200);
  MPU6050_init();
  GetOffsetData(gx_offset,gy_offset,gz_offset,ax_offset,ay_offset,az_offset);
  now=nPgmTime;
while(true)
   {
     GetMPU6050Data(roll_g,pitch_g,yaw_g,roll_a,pitch_a,yaw_a);
     updateIMU(roll_g,pitch_g,yaw_g,roll_a,pitch_a,yaw_a);

     nxtDisplayTextLine(3, "Roll %f",roll);
     nxtDisplayTextLine(4, "pitch %f",pitch);
     nxtDisplayTextLine(5, "yaw %f",yaw);

   }
}

jerryxiaodoudou

lz辛苦了，姿态演算应该算比较高端的东西了吧，等我弄到姿态演算的时候一定要请教您