logzhan
/
GeekIMU
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity
GeekIMU/2.Firmware/ICM42688P_HAL/Core/Src/imu.c

443 lines
12 KiB
C
Raw Normal View History

更新GeekIMU-42688P适配固件，更新基于Python的可视化工具
2025-02-20 23:29:58 +08:00
#include "imu.h"
#define DCM_KP_ACC 0.400f //<2F><><EFBFBD>ٶȲ<D9B6><C8B2><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>PI<50><49><EFBFBD><EFBFBD>
#define DCM_KI_ACC 0.001f
#define DCM_KP_MAG 1.000f //<2F><><EFBFBD><EFBFBD><EFBFBD>Ʋ<EFBFBD><C6B2><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>PI<50><49><EFBFBD><EFBFBD>
#define DCM_KI_MAG 0.000f
#define IMU_SMALL_ANGLE 15.0f //<2F><><EFBFBD><EFBFBD>ˮƽ״̬<D7B4><CCAC><EFBFBD><EFBFBD>С<EFBFBD>Ƕȣ<C7B6><C8A3><EFBFBD>λdeg<65><67>
#define SPIN_RATE_LIMIT 20 //<2F><>ת<EFBFBD><D7AA><EFBFBD><EFBFBD>
//float Kp = 0.4f; /*<2A><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>*/
//float Ki = 0.001f; /*<2A><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>*/
//float exInt = 0.0f;
//float eyInt = 0.0f;
//float ezInt = 0.0f; /*<2A><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ۼ<EFBFBD>*/
float imuAttitudeYaw;//<2F><>Χ<EFBFBD><CEA7>-180~180<38><30><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ϴ<EFBFBD><CFB4><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>λ<EFBFBD><CEBB><EFBFBD><EFBFBD>֧<EFBFBD>ַ<EFBFBD>Χ-180~180<38><30>
static float q0 = 1.0f, q1 = 1.0f, q2 = 0.0f, q3 = 0.0f, q4 = 0.0f;//<2F><>Ԫ<EFBFBD><D4AA>
static float rMat[3][3];//<2F><>Ԫ<EFBFBD><D4AA><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ת<EFBFBD><D7AA><EFBFBD><EFBFBD>
static float smallAngleCosZ;//ˮƽ<CBAE><C6BD>С<EFBFBD><D0A1><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ֵ
//<2F><>ת<EFBFBD><D7AA><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ϵ<EFBFBD><CFB5><EFBFBD>ٶȵ<D9B6>NEU<45><55><EFBFBD><EFBFBD>ϵ
void imuTransformVectorBodyToEarth(Axis3f *v)
{
const float x = rMat[0][0] * v->x + rMat[0][1] * v->y + rMat[0][2] * v->z;
const float y = rMat[1][0] * v->x + rMat[1][1] * v->y + rMat[1][2] * v->z;
const float z = rMat[2][0] * v->x + rMat[2][1] * v->y + rMat[2][2] * v->z;
v->x = x;
v->y = -y;//
v->z = z;
}
//NEU<45><55><EFBFBD><EFBFBD>ϵ<EFBFBD><CFB5><EFBFBD>ٶȵ<D9B6>NEU<45><55><EFBFBD><EFBFBD>ϵ
void imuTransformVectorEarthToBody(Axis3f *v)
{
v->y = -v->y;
/* From earth frame to body frame */
const float x = rMat[0][0] * v->x + rMat[1][0] * v->y + rMat[2][0] * v->z;
const float y = rMat[0][1] * v->x + rMat[1][1] * v->y + rMat[2][1] * v->z;
const float z = rMat[0][2] * v->x + rMat[1][2] * v->y + rMat[2][2] * v->z;
v->x = x;
v->y = y;
v->z = z;
}
float degreesToRadians(int16_t degrees1)
{
return degrees1 * RAD;
}
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ԫ<EFBFBD><D4AA><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ת<EFBFBD><D7AA><EFBFBD><EFBFBD>
static void imuComputeRotationMatrix(void)
{
float q1q1 = q1 * q1;
float q2q2 = q2 * q2;
float q3q3 = q3 * q3;
float q0q1 = q0 * q1;
float q0q2 = q0 * q2;
float q0q3 = q0 * q3;
float q1q2 = q1 * q2;
float q1q3 = q1 * q3;
float q2q3 = q2 * q3;
rMat[0][0] = 1.0f - 2.0f * q2q2 - 2.0f * q3q3;
rMat[0][1] = 2.0f * (q1q2 + -q0q3);
rMat[0][2] = 2.0f * (q1q3 - -q0q2);
rMat[1][0] = 2.0f * (q1q2 - -q0q3);
rMat[1][1] = 1.0f - 2.0f * q1q1 - 2.0f * q3q3;
rMat[1][2] = 2.0f * (q2q3 + -q0q1);
rMat[2][0] = 2.0f * (q1q3 + -q0q2);
rMat[2][1] = 2.0f * (q2q3 - -q0q1);
rMat[2][2] = 1.0f - 2.0f * q1q1 - 2.0f * q2q2;
}
void imuInit(void)
{
smallAngleCosZ = cos_approx(degreesToRadians(IMU_SMALL_ANGLE));//<2F><>С<EFBFBD><D0A1><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ֵ
imuComputeRotationMatrix();//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ԫ<EFBFBD><D4AA><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ת<EFBFBD><D7AA><EFBFBD><EFBFBD>
}
static float invSqrt(float x)
{
return 1.0f / sqrtf(x);
}
//<2F><><EFBFBD><EFBFBD><EFBFBD>ƿ<EFBFBD><C6BF><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
static float imuMagFastPGainSaleFactor(void)
{
//<2F><><EFBFBD><EFBFBD><EFBFBD>ϵ<EFBFBD><CFB5><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ں<EFBFBD><DABA><EFBFBD>Ҫһ<D2AA><D2BB>ʱ<EFBFBD><CAB1>
//Ϊ<>˿<EFBFBD><CBBF><EFBFBD><EFBFBD>ںϣ<DABA>ǰ100<30><30>ʹ<EFBFBD>ÿ<EFBFBD><C3BF><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
static int32_t FastPGainCount = 100;
if(FastPGainCount>0)//
{
FastPGainCount--;
return 10.0f;
}
else
return 1.0f;
}
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
static float imuACCFastPGainSaleFactor(void)
{
//<2F><><EFBFBD><EFBFBD><EFBFBD>ϵ<EFBFBD><CFB5><EFBFBD>ACC<43>ں<EFBFBD><DABA><EFBFBD>Ҫһ<D2AA><D2BB>ʱ<EFBFBD><CAB1>
//Ϊ<>˿<EFBFBD><CBBF><EFBFBD><EFBFBD>ںϣ<DABA>ǰ100<30><30>ʹ<EFBFBD>ÿ<EFBFBD><C3BF><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
static int32_t FastPGainCount = 100;
if(FastPGainCount>0)//
{
FastPGainCount--;
return 100.0f;
}
else
return 1.0f;
}
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ԫ<EFBFBD><D4AA><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ת<EFBFBD><D7AA><EFBFBD><EFBFBD>
void imuMahonyAHRSupdate(float gx, float gy, float gz,float ax, float ay, float az,float mx, float my, float mz,bool useMag,float dt)
{
static float integralAccX = 0.0f, integralAccY = 0.0f, integralAccZ = 0.0f; //<2F><><EFBFBD>ٶȻ<D9B6><C8BB><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
static float integralMagX = 0.0f, integralMagY = 0.0f, integralMagZ = 0.0f; //<2F><><EFBFBD><EFBFBD><EFBFBD>ƻ<EFBFBD><C6BB><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
float ex, ey, ez;
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ת<EFBFBD><D7AA><EFBFBD><EFBFBD>(rad/s)
const float spin_rate_sq = sq(gx) + sq(gy) + sq(gz);
//Step 1: Yaw correction
if (useMag)
{
const float magMagnitudeSq = mx * mx + my * my + mz * mz;
float kpMag = DCM_KP_MAG * imuMagFastPGainSaleFactor();
if (magMagnitudeSq > 0.01f)
{
//<2F><>λ<EFBFBD><CEBB><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ʋ<EFBFBD><C6B2><EFBFBD>ֵ
const float magRecipNorm = invSqrt(magMagnitudeSq);
mx *= magRecipNorm;
my *= magRecipNorm;
mz *= magRecipNorm;
// printf(" mx:%f ",mx);
// printf(" my:%f ",my);
// printf(" mz:%f ",mz);
// printf(" magRecipNorm:%f ",magRecipNorm);
// printf(" \r\n ");
//<2F><><EFBFBD><EFBFBD>X\Y<><59><EFBFBD><EFBFBD><EFBFBD>Ĵ<EFBFBD>ͨ<EFBFBD><CDA8><EFBFBD>ű<EFBFBD><C5B1><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ͨ
const float hx = rMat[0][0] * mx + rMat[0][1] * my + rMat[0][2] * mz;
const float hy = rMat[1][0] * mx + rMat[1][1] * my + rMat[1][2] * mz;
const float bx = sqrtf(hx * hx + hy * hy);
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ǹ<EFBFBD><C7B9>ƴű<C6B4><C5B1>Ͳ<EFBFBD><CDB2><EFBFBD><EFBFBD>ű<EFBFBD>֮<EFBFBD><D6AE><EFBFBD>Ľ<EFBFBD><C4BD><EFBFBD><EFBFBD>˻<EFBFBD>
const float ez_ef = -(hy * bx);
//<2F><>ת<EFBFBD><D7AA><EFBFBD><EFBFBD><EEB5BD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ϵ
ex = rMat[2][0] * ez_ef;
ey = rMat[2][1] * ez_ef;
ez = rMat[2][2] * ez_ef;
}
else
{
ex = 0;
ey = 0;
ez = 0;
}
//<2F>ۼ<EFBFBD><DBBC><EFBFBD><EFBFBD><EFBFBD><EEB2B9>
if (DCM_KI_MAG > 0.0f)
{
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ת<EFBFBD><D7AA><EFBFBD>ʴ<EFBFBD><CAB4><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ֵ<EFBFBD><D6B5>ֹͣ<CDA3><D6B9><EFBFBD><EFBFBD>
if (spin_rate_sq < sq(DEGREES_TO_RADIANS(SPIN_RATE_LIMIT)))
{
integralMagX += DCM_KI_MAG * ex * dt;
integralMagY += DCM_KI_MAG * ey * dt;
integralMagZ += DCM_KI_MAG * ez * dt;
gx += integralMagX;
gy += integralMagY;
gz += integralMagZ;
}
}
//<2F><><EFBFBD><EFBFBD><EEB2B9>
gx += kpMag * ex;
gy += kpMag * ey;
gz += kpMag * ez;
}
//Step 2: Roll and pitch correction
if(!((ax == 0.0f) && (ay == 0.0f) && (az == 0.0f)))//<2F><><EFBFBD>ٶȼ<D9B6><C8BC><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Чʱ,<2C><><EFBFBD>ü<EFBFBD><C3BC>ٶȼƲ<C8BC><C6B2><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
{
//<2F><>λ<EFBFBD><CEBB><EFBFBD><EFBFBD><EFBFBD>ټƲ<D9BC><C6B2><EFBFBD>ֵ
const float accRecipNorm = invSqrt(ax * ax + ay * ay + az * az);
ax *= accRecipNorm;
ay *= accRecipNorm;
az *= accRecipNorm;
//<2F><><EFBFBD>ټƶ<D9BC>ȡ<EFBFBD>ķ<EFBFBD><C4B7><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ټƷ<D9BC><C6B7><EFBFBD><EFBFBD>IJ<EFBFBD>ֵ<EFBFBD><D6B5><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>˼<EFBFBD><CBBC><EFBFBD>
ex = (ay * rMat[2][2] - az * rMat[2][1]);
ey = (az * rMat[2][0] - ax * rMat[2][2]);
ez = (ax * rMat[2][1] - ay * rMat[2][0]);
//<2F>ۼ<EFBFBD><DBBC><EFBFBD><EFBFBD><EFBFBD><EEB2B9>
if (DCM_KI_ACC > 0.0f)
{
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ת<EFBFBD><D7AA><EFBFBD>ʴ<EFBFBD><CAB4><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ֵ<EFBFBD><D6B5>ֹͣ<CDA3><D6B9><EFBFBD><EFBFBD>
if (spin_rate_sq < sq(DEGREES_TO_RADIANS(SPIN_RATE_LIMIT)))
{
/*<2A><><EFBFBD><EFBFBD><EFBFBD>ۼƣ<DBBC><C6A3><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ֳ<EFBFBD><D6B3><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>*/
integralAccX += DCM_KI_ACC * ex * dt;
integralAccY += DCM_KI_ACC * ey * dt;
integralAccZ += DCM_KI_ACC * ez * dt;
gx += integralAccX;
gy += integralAccY;
gz += integralAccZ;
}
}
//<2F><><EFBFBD><EFBFBD><EEB2B9> <20>ò<EFBFBD><C3B2><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>PI<50><49><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ƫ<EFBFBD><C6AB><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ݶ<EFBFBD><DDB6><EFBFBD><EFBFBD>е<EFBFBD>ƫ<EFBFBD><C6AB><EFBFBD><EFBFBD>
float ak = imuACCFastPGainSaleFactor();
//printf(" ak:%f ",ak);
gx += DCM_KP_ACC * ex * ak;
gy += DCM_KP_ACC * ey * ak;
gz += DCM_KP_ACC * ez * ak;
}
//һ<>׽<EFBFBD><D7BD><EFBFBD><EFBFBD>㷨<EFBFBD><E3B7A8><EFBFBD><EFBFBD>Ԫ<EFBFBD><D4AA><EFBFBD>˶<EFBFBD>ѧ<EFBFBD><D1A7><EFBFBD>̵<EFBFBD><CCB5><EFBFBD>ɢ<EFBFBD><C9A2><EFBFBD><EFBFBD>ʽ<EFBFBD>ͻ<EFBFBD><CDBB><EFBFBD>
gx *= (0.5f * dt);
gy *= (0.5f * dt);
gz *= (0.5f * dt);
const float qa = q0;
const float qb = q1;
const float 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);
//<2F><>λ<EFBFBD><CEBB><EFBFBD><EFBFBD>Ԫ<EFBFBD><D4AA>
const float quatRecipNorm = invSqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3);
q0 *= quatRecipNorm;
q1 *= quatRecipNorm;
q2 *= quatRecipNorm;
q3 *= quatRecipNorm;
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ԫ<EFBFBD><D4AA><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ת<EFBFBD><D7AA><EFBFBD><EFBFBD>
imuComputeRotationMatrix();
}
//<2F><><EFBFBD><EFBFBD>ŷ<EFBFBD><C5B7><EFBFBD><EFBFBD>
static void imuUpdateEulerAngles(attitude_t *attitude)
{
attitude->roll = RADIANS_TO_DEGREES(atan2_approx(rMat[2][1], rMat[2][2]));
attitude->pitch = RADIANS_TO_DEGREES((0.5f * M_PIf) - acos_approx(-rMat[2][0]));//arcsin = 0.5PI - arccos
attitude->yaw = RADIANS_TO_DEGREES(atan2_approx(rMat[1][0], rMat[0][0]));
imuAttitudeYaw = attitude->yaw;
if (attitude->yaw < 0.0f)//ת<><D7AA>λ0~360
attitude->yaw += 360.0f;
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>С<EFBFBD><D0A1><EFBFBD><EFBFBD>״̬
if (rMat[2][2] > smallAngleCosZ)
ENABLE_STATE(SMALL_ANGLE);
else
DISABLE_STATE(SMALL_ANGLE);
}
//<2F><>Ԫ<EFBFBD><D4AA><EFBFBD><EFBFBD>ŷ<EFBFBD><C5B7><EFBFBD>Ǽ<EFBFBD><C7BC><EFBFBD>
void imuUpdateAttitude(const sensorData_t *sensorData, state_t *state, float dt)
{
bool useMag = 0;//compassIsHealthy();
Axis3f gyro = sensorData->gyro;
Axis3f acc = sensorData->acc;
Axis3f mag = sensorData->mag;
//<2F><><EFBFBD>ٶȵ<D9B6>λ<EFBFBD>ɶ<EFBFBD>תΪ<D7AA><CEAA><EFBFBD><EFBFBD>
gyro.x = gyro.x * DEG2RAD;
gyro.y = gyro.y * DEG2RAD;
gyro.z = gyro.z * DEG2RAD;
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ԫ<EFBFBD><D4AA><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ת<EFBFBD><D7AA><EFBFBD><EFBFBD>
imuMahonyAHRSupdate(gyro.x, gyro.y, gyro.z,
acc.x, acc.y, acc.z,
mag.x, mag.y, mag.z,
useMag,dt);
//<2F><><EFBFBD><EFBFBD>ŷ<EFBFBD><C5B7><EFBFBD><EFBFBD>
imuUpdateEulerAngles(&state->attitude);
}
float beta = 0.2;//0.04 <20><><EFBFBD>ټƺʹ<C6BA><CDB4><EFBFBD><EFBFBD>Ʋ<EFBFBD><C6B2><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ӣ<EFBFBD><D3A3><EFBFBD>ֵԽ<D6B5><D4BD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Խ<EFBFBD>죬Խ<ECA3AC><D4BD><EFBFBD>ȶ<EFBFBD>
void MadgwickQuaternionUpdate(Axis3f acc, Axis3f gyro, Axis3f mag, attitude_t *attitude , float dt)
{
float norm;
float hx, hy, _2bx, _2bz;
float s1, s2, s3, s4;
float qDot1, qDot2, qDot3, qDot4;
//<2F><>ǰ<EFBFBD><C7B0><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ҫ<EFBFBD>õ<EFBFBD><C3B5><EFBFBD><EFBFBD><EFBFBD>ֵ<EFBFBD><D6B5>ֹ<EFBFBD>ظ<EFBFBD><D8B8><EFBFBD><EFBFBD>㣬<EFBFBD><E3A3AC><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
float _2q1mx;
float _2q1my;
float _2q1mz;
float _2q2mx;
float _4bx;
float _4bz;
float _2q1 = 2.0f * q1;
float _2q2 = 2.0f * q2;
float _2q3 = 2.0f * q3;
float _2q4 = 2.0f * q4;
float _2q1q3 = 2.0f * q1 * q3;
float _2q3q4 = 2.0f * q3 * q4;
float q1q1 = q1 * q1;
float q1q2 = q1 * q2;
float q1q3 = q1 * q3;
float q1q4 = q1 * q4;
float q2q2 = q2 * q2;
float q2q3 = q2 * q3;
float q2q4 = q2 * q4;
float q3q3 = q3 * q3;
float q3q4 = q3 * q4;
float q4q4 = q4 * q4;
//<2F><>ת<EFBFBD><D7AA><EFBFBD><EFBFBD>
gyro.x = gyro.x * DEG2RAD; /* <20><>ת<EFBFBD><D7AA><EFBFBD><EFBFBD> */
gyro.y = gyro.y * DEG2RAD;
gyro.z = gyro.z * DEG2RAD;
//<2F><>λ<EFBFBD><CEBB><EFBFBD><EFBFBD><EFBFBD>ٶ<EFBFBD><D9B6><EFBFBD><EFBFBD><EFBFBD>
norm = sqrtf(acc.x * acc.x + acc.y * acc.y + acc.z * acc.z);
if (norm == 0.0f) return; // handle NaN
norm = 1.0f/norm;
acc.x *= norm;
acc.y *= norm;
acc.z *= norm;
//<2F><>λ<EFBFBD><CEBB><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
norm = sqrtf(mag.x * mag.x + mag.y * mag.y + mag.z * mag.z);
if (norm == 0.0f) return; // handle NaN
norm = 1.0f/norm;
mag.x *= norm;
mag.y *= norm;
mag.z *= norm;
//<2F><>ǰ<EFBFBD><C7B0><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ҫ<EFBFBD>õ<EFBFBD><C3B5><EFBFBD>ֵ
_2q1mx = 2.0f * q1 * mag.x;
_2q1my = 2.0f * q1 * mag.y;
_2q1mz = 2.0f * q1 * mag.z;
_2q2mx = 2.0f * q2 * mag.x;
//<2F><><EFBFBD><EFBFBD><EFBFBD>ƴӻ<C6B4><D3BB>嵽<EFBFBD><E5B5BD><EFBFBD><EFBFBD>
hx = mag.x * q1q1 - _2q1my * q4 + _2q1mz * q3 + mag.x * q2q2 + _2q2 * mag.y * q3 + _2q2 * mag.z * q4 - mag.x * q3q3 - mag.x * q4q4;
hy = _2q1mx * q4 + mag.y * q1q1 - _2q1mz * q2 + _2q2mx * q3 - mag.y * q2q2 + mag.y * q3q3 + _2q3 * mag.z * q4 - mag.y * q4q4;
/*<2A>õ<EFBFBD><C3B5><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ϵ<EFBFBD><CFB5>X<EFBFBD><58>ָ<EFBFBD><D6B8><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>*/
_2bx = sqrtf(hx * hx + hy * hy);
_2bz = -_2q1mx * q3 + _2q1my * q2 + mag.z * q1q1 + _2q2mx * q4 - mag.z * q2q2 + _2q3 * mag.y * q4 - mag.z * q3q3 + mag.z * q4q4;
_4bx = 2.0f * _2bx;
_4bz = 2.0f * _2bz;
//<2F>ݶ<EFBFBD><DDB6>½<EFBFBD><C2BD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
s1 = -_2q3 * (2.0f * q2q4 - _2q1q3 - acc.x) + _2q2 * (2.0f * q1q2 + _2q3q4 - acc.y) - _2bz * q3 * (_2bx * (0.5f - q3q3 - q4q4) + _2bz * (q2q4 - q1q3) - mag.x) + (-_2bx * q4 + _2bz * q2) * (_2bx * (q2q3 - q1q4) + _2bz * (q1q2 + q3q4) - mag.y) + _2bx * q3 * (_2bx * (q1q3 + q2q4) + _2bz * (0.5f - q2q2 - q3q3) - mag.z);
s2 = _2q4 * (2.0f * q2q4 - _2q1q3 - acc.x) + _2q1 * (2.0f * q1q2 + _2q3q4 - acc.y) - 4.0f * q2 * (1.0f - 2.0f * q2q2 - 2.0f * q3q3 - acc.z) + _2bz * q4 * (_2bx * (0.5f - q3q3 - q4q4) + _2bz * (q2q4 - q1q3) - mag.x) + (_2bx * q3 + _2bz * q1) * (_2bx * (q2q3 - q1q4) + _2bz * (q1q2 + q3q4) - mag.y) + (_2bx * q4 - _4bz * q2) * (_2bx * (q1q3 + q2q4) + _2bz * (0.5f - q2q2 - q3q3) - mag.z);
s3 = -_2q1 * (2.0f * q2q4 - _2q1q3 - acc.x) + _2q4 * (2.0f * q1q2 + _2q3q4 - acc.y) - 4.0f * q3 * (1.0f - 2.0f * q2q2 - 2.0f * q3q3 - acc.z) + (-_4bx * q3 - _2bz * q1) * (_2bx * (0.5f - q3q3 - q4q4) + _2bz * (q2q4 - q1q3) - mag.x) + (_2bx * q2 + _2bz * q4) * (_2bx * (q2q3 - q1q4) + _2bz * (q1q2 + q3q4) - mag.y) + (_2bx * q1 - _4bz * q3) * (_2bx * (q1q3 + q2q4) + _2bz * (0.5f - q2q2 - q3q3) - mag.z);
s4 = _2q2 * (2.0f * q2q4 - _2q1q3 - acc.x) + _2q3 * (2.0f * q1q2 + _2q3q4 - acc.y) + (-_4bx * q4 + _2bz * q2) * (_2bx * (0.5f - q3q3 - q4q4) + _2bz * (q2q4 - q1q3) - mag.x) + (-_2bx * q1 + _2bz * q3) * (_2bx * (q2q3 - q1q4) + _2bz * (q1q2 + q3q4) - mag.y) + _2bx * q2 * (_2bx * (q1q3 + q2q4) + _2bz * (0.5f - q2q2 - q3q3) - mag.z);
//<2F><>λ<EFBFBD><CEBB><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>õ<EFBFBD><C3B5><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
norm = sqrtf(s1 * s1 + s2 * s2 + s3 * s3 + s4 * s4);
norm = 1.0f/norm;
s1 *= norm;
s2 *= norm;
s3 *= norm;
s4 *= norm;
// <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>õ<EFBFBD><C3B5><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ԫ<EFBFBD><D4AA>
qDot1 = 0.5f * (-q2 * gyro.x - q3 * gyro.y - q4 * gyro.z) - beta * s1;
qDot2 = 0.5f * (q1 * gyro.x + q3 * gyro.z - q4 * gyro.y) - beta * s2;
qDot3 = 0.5f * (q1 * gyro.y - q2 * gyro.z + q4 * gyro.x) - beta * s3;
qDot4 = 0.5f * (q1 * gyro.z + q2 * gyro.y - q3 * gyro.x) - beta * s4;
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ԫ<EFBFBD><D4AA><EFBFBD><EFBFBD>ֵ
q1 += qDot1 * dt;
q2 += qDot2 * dt;
q3 += qDot3 * dt;
q4 += qDot4 * dt;
norm = sqrtf(q1 * q1 + q2 * q2 + q3 * q3 + q4 * q4); // normalise quaternion
norm = 1.0f/norm;
q1 = q1 * norm;
q2 = q2 * norm;
q3 = q3 * norm;
q4 = q4 * norm;
//<2F><>Ԫ<EFBFBD><D4AA>ת<EFBFBD><D7AA>Ϊŷ<CEAA><C5B7><EFBFBD><EFBFBD>
attitude->yaw = atan2(2.0f * (q2 * q3 + q1 * q4), q1 * q1 + q2 * q2 - q3 * q3 - q4 * q4) * RAD2DEG;
attitude->pitch = -asin(2.0f * (q2 * q4 - q1 * q3)) * RAD2DEG;
attitude->roll = atan2(2.0f * (q1 * q2 + q3 * q4), q1 * q1 - q2 * q2 - q3 * q3 + q4 * q4) * RAD2DEG;
}