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;
} */