擷取陀螺儀和加速度計原始資料 short gryox,gyroy,gyroz;
short accx,accy,accz;
void IMU_GetData(void)
{
MPU_Get_Gyroscope(&gyrox,&gyroy,&gyroz);
MPU_Get_Accelerometer(&accx,&accy,&accz);
}
定義變量 #define RtA 57.295779f //弧度->角度
#define AtR 0.0174533f //角度->弧度
#define Acc_G 0.0011963f
#define Gyro_G 0.0610351f
#define Gyro_Gr 0.0010653f
#define Kp 18.0f
#define Ki 0.008f
#define halfT 0.008f
float q0 = 1, q1 = 0, q2 = 0, q3 = 0;
float exInt = 0, eyInt = 0, ezInt = 0;
float Angle[3]={0};//最終角度
函數聲明 /*************************
*函數名：IMU_Update
*輸入：陀螺儀，加速度計原始資料（short類型)
*************************/
void IMU_Update(short gyrox,short gyroy,short gyroz,short accx,short accy,short accz);
函數實作 void IMU_Update(short gyrox,short gyroy,short gyroz,short accx,short accy,short accz)
{
float ax=accx,ay=accy,az=accz;
float gx=gyrox,gy=gyroy,gz=gyroz;
float norm;
float vx, vy, vz;
float ex, ey, ez;
float q0q0 = q0*q0;
float q0q1 = q0*q1;
float q0q2 = q0*q2;
float q1q1 = q1*q1;
float q1q3 = q1*q3;
float q2q2 = q2*q2;
float q2q3 = q2*q3;
float q3q3 = q3*q3;
if(ax*ay*az==0)//此時任意方向角加速度為0
return;
gx *= Gyro_Gr;
gy *= Gyro_Gr;
gz *= Gyro_Gr;
norm = sqrt(ax*ax + ay*ay + az*az);
ax = ax /norm;
ay = ay / norm;
az = az / norm;
// estimated direction of gravity and flux (v and w)
vx = 2*(q1q3 - q0q2);
vy = 2*(q0q1 + q2q3);
vz = q0q0 - q1q1 - q2q2 + q3q3 ;
// error is sum of cross product between reference direction of fields and direction measured by sensors
ex = (ay*vz - az*vy) ;
ey = (az*vx - ax*vz) ;
ez = (ax*vy - ay*vx) ;
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;
// angle->yaw = atan2(2 * q1 * q2 + 2 * q0 * q3, -2 * q2*q2 - 2 * q3* q3 + 1)* RtA; // yaw
ANgle[0] = asin(-2 * q1 * q3 + 2 * q0* q2)* RtA ; //pitch
ANgle[1] = atan2(2 * q2 * q3 + 2 * q0 * q1, -2 * q1 * q1 - 2 * q2* q2 + 1)* RtA; //roll
}
IMU.h //IMU.h
#ifndef _IMU_H
#define _IMU_H
/*************************
*函數名：IMU_Update
*輸入：陀螺儀，加速度計原始資料（short類型)
*************************/
void IMU_Update(short gyrox,short gyroy,short gyroz,short accx,short accy,short accz);
#endif
IMU.c //IMU.c
#include "IMU.h"
short gryox,gyroy,gyroz;
short accx,accy,accz;
void IMU_GetData(void)
{
MPU_Get_Gyroscope(&gyrox,&gyroy,&gyroz);
MPU_Get_Accelerometer(&accx,&accy,&accz);
}
#define RtA 57.295779f //弧度->角度
#define AtR 0.0174533f //角度->弧度
#define Acc_G 0.0011963f
#define Gyro_G 0.0610351f
#define Gyro_Gr 0.0010653f
#define Kp 18.0f
#define Ki 0.008f
#define halfT 0.008f
float q0 = 1, q1 = 0, q2 = 0, q3 = 0;
float exInt = 0, eyInt = 0, ezInt = 0;
float Angle[3]={0};//最終角度
void IMU_Update(short gyrox,short gyroy,short gyroz,short accx,short accy,short accz)
{
float ax=accx,ay=accy,az=accz;
float gx=gyrox,gy=gyroy,gz=gyroz;
float norm;
float vx, vy, vz;
float ex, ey, ez;
float q0q0 = q0*q0;
float q0q1 = q0*q1;
float q0q2 = q0*q2;
float q1q1 = q1*q1;
float q1q3 = q1*q3;
float q2q2 = q2*q2;
float q2q3 = q2*q3;
float q3q3 = q3*q3;
if(ax*ay*az==0)//此時任意方向角加速度為0
return;
gx *= Gyro_Gr;
gy *= Gyro_Gr;
gz *= Gyro_Gr;
norm = sqrt(ax*ax + ay*ay + az*az);
ax = ax /norm;
ay = ay / norm;
az = az / norm;
// estimated direction of gravity and flux (v and w)
vx = 2*(q1q3 - q0q2);
vy = 2*(q0q1 + q2q3);
vz = q0q0 - q1q1 - q2q2 + q3q3 ;
// error is sum of cross product between reference direction of fields and direction measured by sensors
ex = (ay*vz - az*vy) ;
ey = (az*vx - ax*vz) ;
ez = (ax*vy - ay*vx) ;
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;
// angle->yaw = atan2(2 * q1 * q2 + 2 * q0 * q3, -2 * q2*q2 - 2 * q3* q3 + 1)* RtA; // yaw
ANgle[0] = asin(-2 * q1 * q3 + 2 * q0* q2)* RtA ; //pitch
ANgle[1] = atan2(2 * q2 * q3 + 2 * q0 * q1, -2 * q1 * q1 - 2 * q2* q2 + 1)* RtA; //roll
}
四元數姿态解算詳細步驟 四元數姿态解算詳細步驟 四元數姿态解算詳細步驟 
