arduino 四旋翼无人机惯导代码 mpu9250

arduino 四旋翼无人机惯导代码 mpu9250,含陀螺仪和加速度计耦合

#include <Wire.h>   
#include <SPI.h>
#include "I2Cdev.h"    //通讯库
#include "MPU6050.h"   //陀螺仪加速度模块库


//#include "IMU.h" 
#include <math.h>   //数学计算库
#define sampleFreq  33.30f      // sample frequency in Hz
#define twoKpDef  (2.0f * 0.5f) // 2 * proportional gain
#define twoKiDef  (2.0f * 0.0f) // 2 * integral gain
#define betaDef    0.1f

MPU6050 accelgyro;

volatile float twoKp = twoKpDef;                      // 2 * proportional gain (Kp)//KP,KI 姿态纠正的快慢
volatile float twoKi = twoKiDef;                      // 2 * integral gain (Ki)
volatile float beta = betaDef;    
volatile float q0 = 1.0f, q1 = 0.0f, q2 = 0.0f, q3 = 0.0f;          // quaternion of sensor frame relative to auxiliary frame
volatile float integralFBx = 0.0f,  integralFBy = 0.0f, integralFBz = 0.0f; // integral error terms scaled by Ki

int16_t ax,ay,az;//加计量
int16_t gx,gy,gz;//重力量
int16_t mx,my,mz;   //地磁量
int k;

//#define GpsSerial Serial1
#define DebugSerial Serial

float roll;
float pitch;
float yaw;
float yaw1;

float pitchoffset,rolloffset,yawoffset;   //欧拉角校正中间变量

void setup()
{
      Wire.begin();
      // initialize device
      DebugSerial.println("Initializing I2C devices...");
      accelgyro.initialize();
}

void loop()   //主循环 
{
  /*  if  (k<1)    //开机校正mpu9150/9250 函数    暂时不打开
  {
  CalibrateToZero();    //调用mpu9150/9250校正函数
  k=k+1;
  }   */
  
  accelgyro.getMotion9(&ax, &ay, &az, &gx, &gy, &gz, &mx, &my, &mz);

  gx = gx*3.1415926f/131.00f/180.00f;
  gy = gy*3.1415926f/131.00f/180.00f;
  gz = gz*3.1415926f/131.00f/180.00f;

IMUupdate(float gx, float gy, float gz, float ax, float ay, float az)   //调用四元数微分计算,加计融合

DCAHRSupdateIMU(float mx, float my, float mz)   //调用地磁耦合函数

/*  pitch+=pitchoffset;    //校正
roll+=rolloffset;
yaw+=yawoffset;   */

 //DebugSerial.print(" ");   //串口输出测试 计算值
//DebugSerial.print(q0);    
//DebugSerial.print(" ");
//DebugSerial.print(q1);
//DebugSerial.print(" ");
//DebugSerial.print(q2);
//DebugSerial.print(" ");
//DebugSerial.print(q3);
//DebugSerial.print(" ");  
DebugSerial.print(roll);       // 欧拉角  滚动
DebugSerial.print(" ");
DebugSerial.print(pitch);    // 欧拉角  俯仰  
DebugSerial.print(" "); 
DebugSerial.print(yaw);      // 欧拉角  偏航 
DebugSerial.println(" "); 

}

void IMUupdate(float gx, float gy, float gz, float ax, float ay, float az)   //四元数微分计算,加计融合
  {
    float recipNorm;
    float halfvx, halfvy, halfvz;
    float halfex, halfey, halfez;
    float qa, qb, qc;

        
         
  // normalise the measurements
   if(!((ax == 0.0f) && (ay == 0.0f) && (az == 0.0f))) 
    {
            
          
      recipNorm = invSqrt(ax * ax + ay * ay + az * az);
      ax *= recipNorm;
      ay *= recipNorm;
      az *= recipNorm;   
  //把加计的三维向量转成单位向量。
         
  
          
        halfvx = q1 * q3 - q0 * q2;
        halfvy = q0 * q1 + q2 * q3;
        halfvz = q0 * q0 - 0.5f + q3 * q3;

        //halfvz = q0 * q0 - q1*q1 + q3 * q3;
        //  vz = q0*q0 - q1*q1 - q2*q2 + q3*q3;
        //  是否为q0*q0 - q1*q1 - q2*q2 + q3*q3
  
  
  //这是把四元数换算成《方向余弦矩阵》中的第三列的三个元素。
  //根据余弦矩阵和欧拉角的定义,地理坐标系的重力向量,转到机体坐标系,正好是这三个元素。
  //所以这里的vx\y\z,其实就是当前的欧拉角(即四元数)的机体坐标参照系上,换算出来的重力单位向量。
  
  
          // error is sum of cross product between reference direction of field and direction measured by sensor
        
         halfex = (ay * halfvz - az * halfvy);
         halfey = (az * halfvx - ax * halfvz);
         halfez = (ax * halfvy - ay * halfvx);
  
  //axyz是机体坐标参照系上,加速度计测出来的重力向量,也就是实际测出来的重力向量。
  //axyz是测量得到的重力向量,vxyz是陀螺积分后的姿态来推算出的重力向量,它们都是机体坐标参照系上的重力向量。
  //那它们之间的误差向量,就是陀螺积分后的姿态和加计测出来的姿态之间的误差。
  //向量间的误差,可以用向量叉积(也叫向量外积、叉乘)来表示,exyz就是两个重力向量的叉积。
  //这个叉积向量仍旧是位于机体坐标系上的,而陀螺积分误差也是在机体坐标系,而且叉积的大小与陀螺积分误差成正比,正好拿来纠正陀螺。(你可以自己拿东西想象一下)由于陀螺是对机体直接积分,所以对陀螺的纠正量会直接体现在对机体坐标系的纠正。
  
  
  
           if(twoKi > 0.0f) {
      integralFBx += twoKi * halfex * (1.0f / sampleFreq);  // integral error scaled by Ki
      integralFBy += twoKi * halfey * (1.0f / sampleFreq);
      integralFBz += twoKi * halfez * (1.0f / sampleFreq);
      gx += integralFBx;  // apply integral feedback
      gy += integralFBy;
      gz += integralFBz;
    }
    else {
      integralFBx = 0.0f; // prevent integral windup
      integralFBy = 0.0f;
      integralFBz = 0.0f;
   
  }
 // Apply proportional feedback
    gx += twoKp * halfex;
    gy += twoKp * halfey;
    gz += twoKp * halfez;
    
    }
         
  //用叉积误差来做PI修正陀螺零偏
  
         
   // Integrate rate of change of quaternion
  gx *= (0.5f * (1.0f / sampleFreq));   // pre-multiply common factors
  gy *= (0.5f * (1.0f / sampleFreq));
  gz *= (0.5f * (1.0f / sampleFreq));
  
  qa = q0;
  qb = q1;
  qc = q2;
  
  q0 += (-qb * gx - qc * gy - q3 * gz);
  q1 += (qa * gx + qc * gz - q3 * gy);
  q2 += (qa * gy - qb * gz + q3 * gx);
  q3 += (qa * gz + qb * gy - qc * gx); 
  
  // Normalise quaternion
  recipNorm = invSqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3);
  q0 *= recipNorm;
  q1 *= recipNorm;
  q2 *= recipNorm;
  q3 *= recipNorm;

   
  
  roll = atan2(2*(q2*q3+q0*q1),(-2*q1*q1-2*q2*q2+1))*57.3;         欧拉角  滚动    要乘以57.3转换为角度        测试用
  pitch = asin(2*(q1*q3-q0*q2))*57.3;                                            欧拉角  俯仰    要乘以57.3转换为角度
  // yaw =- atan2(2*(q1*q2+q0*q3),(-2*q2*q2-2*q3*q3+1))*57.3;   欧拉角  偏航    要乘以57.3转换为角度
   
   //临时测试断点
     //     q0 = ax;
     //    q1 = ay;
     //     q2 = az;
  }

void DCAHRSupdateIMU(float mx, float my, float mz)  //自定义地磁耦合函数
{
     float norm;
     float hx, hy;
     //double yaw1;    //通过地磁计得到的yaw
     #define Ki1 0.4f; 
        
        hx=mx*cos(pitch)+my*sin(pitch)*sin(roll)-mz*cos(roll)*sin(pitch);
        hy=my*cos(roll)+mz*sin(roll);
        
        yaw=-atan2(hy,hx)*57.3;

        //yaw=(yaw1-yaw)*Ki1+yaw;
        
 
}

/* void CalibrateToZero(void)   //开机校正mpu9150/9250 函数    暂时不打开
{
      u8 t=0;
      float sumpitch=0,sumroll=0,sumyaw=0;
         
      for (t=0;t<150;t++)
      {
     IMUupdate(float gx, float gy, float gz, float ax, float ay, float az)   //调用四元数微分计算,加计融合

     DCAHRSupdateIMU(float mx, float my, float mz)   //调用地磁耦合函数
      delay(20);
        if (t>=100)
        {
          sumpitch+=pitch;
          sumroll+=roll;
          sumyaw+=yaw;
        }
      }
    
        pitchoffset=-sumpitch/50.0f;
        rolloffset=-sumroll/50.0f;
        yawoffset=-sumyaw/50.0f;
}    */

上一篇:四旋翼无人机从0到1的实现(二十三)无人机工程中姿态解析


下一篇:Android SQLiteDatabase使用总结