/*=====================================================================================================*/
void Quaternion_Normalize( Quaternion *pNumQ )
{
float Normalize = 0.0f;
Normalize = invSqrtf(squa(pNumQ->q0) + squa(pNumQ->q1) + squa(pNumQ->q2) + squa(pNumQ->q3));
pNumQ->q0 = pNumQ->q0*Normalize;
pNumQ->q1 = pNumQ->q1*Normalize;
pNumQ->q2 = pNumQ->q2*Normalize;
pNumQ->q3 = pNumQ->q3*Normalize;
}
/*====================================================================================================*/
void AHRS_GetQ( Quaternion *pNumQ )
{
fp32 ErrX, ErrY, ErrZ;
fp32 AccX, AccY, AccZ;
fp32 GyrX, GyrY, GyrZ;
fp32 Normalize;
static fp32 exInt = 0.0f, eyInt = 0.0f, ezInt = 0.0f;
Gravity V;
// 加速度归一化
Normalize = Q_rsqrt(squa(sensor.acc.averag.x)+ squa(sensor.acc.averag.y) +squa(sensor.acc.averag.z));
AccX = sensor.acc.averag.x*Normalize;
AccY = sensor.acc.averag.y*Normalize;
AccZ = sensor.acc.averag.z*Normalize;
// 提取重力分量
V = Quaternion_vectorGravity(&NumQ);
// 向量差乘
ErrX = (AccY*V.z - AccZ*V.y);
ErrY = (AccZ*V.x - AccX*V.z);
ErrZ = (AccX*V.y - AccY*V.x);
exInt = exInt + ErrX * KiDef;
eyInt = eyInt + ErrY * KiDef;
ezInt = ezInt + ErrZ * KiDef;
GyrX = Rad(sensor.gyro.averag.x) + KpDef * VariableParameter(ErrX) * ErrX + exInt;
GyrY = Rad(sensor.gyro.averag.y) + KpDef * VariableParameter(ErrY) * ErrY + eyInt;
GyrZ = Rad(sensor.gyro.averag.z) + KpDef * VariableParameter(ErrZ) * ErrZ + ezInt;
// 一阶龙格库塔法, 更新四元数
Quaternion_RungeKutta(&NumQ, GyrX, GyrY, GyrZ, SampleRateHalf);
// 四元数归一化
Quaternion_Normalize(&NumQ);
}
/*=====================================================================================================*/
void AHRS_Update(void)
{
float tempX = 0, tempY = 0;
float Normalize;
float gx, gy, gz;
// float hx, hy, hz;
// float wx, wy, wz;
// float bx, bz;
float ErrX, ErrY, ErrZ;
float AccX, AccY, AccZ;
float GyrX, GyrY, GyrZ;
// float MegX, MegY, MegZ;
float /*Mq11, Mq12, */Mq13,/* Mq21, Mq22, */Mq23,/* Mq31, Mq32, */Mq33;
static float AngZ_Temp = 0.0f;
static float exInt = 0.0f, eyInt = 0.0f, ezInt = 0.0f;
// Mq11 = NumQ.q0*NumQ.q0 + NumQ.q1*NumQ.q1 - NumQ.q2*NumQ.q2 - NumQ.q3*NumQ.q3;
// Mq12 = 2.0f*(NumQ.q1*NumQ.q2 + NumQ.q0*NumQ.q3);
Mq13 = 2.0f * (NumQ.q1 * NumQ.q3 - NumQ.q0 * NumQ.q2);
// Mq21 = 2.0f*(NumQ.q1*NumQ.q2 - NumQ.q0*NumQ.q3);
// Mq22 = NumQ.q0*NumQ.q0 - NumQ.q1*NumQ.q1 + NumQ.q2*NumQ.q2 - NumQ.q3*NumQ.q3;
Mq23 = 2.0f * (NumQ.q0 * NumQ.q1 + NumQ.q2 * NumQ.q3);
// Mq31 = 2.0f*(NumQ.q0*NumQ.q2 + NumQ.q1*NumQ.q3);
// Mq32 = 2.0f*(NumQ.q2*NumQ.q3 - NumQ.q0*NumQ.q1);
Mq33 = NumQ.q0 * NumQ.q0 - NumQ.q1 * NumQ.q1 - NumQ.q2 * NumQ.q2 + NumQ.q3 * NumQ.q3;
Normalize = invSqrtf(squa(Acc.TrueX) + squa(Acc.TrueY) + squa(Acc.TrueZ));
AccX = Acc.TrueX * Normalize;
AccY = Acc.TrueY * Normalize;
AccZ = Acc.TrueZ * Normalize;
// Normalize = invSqrtf(squa(Meg.TrueX) + squa(Meg.TrueY) + squa(Meg.TrueZ));
// MegX = Meg.TrueX*Normalize;
// MegY = Meg.TrueY*Normalize;
// MegZ = Meg.TrueZ*Normalize;
gx = Mq13;
gy = Mq23;
gz = Mq33;
// hx = MegX*Mq11 + MegY*Mq21 + MegZ*Mq31;
// hy = MegX*Mq12 + MegY*Mq22 + MegZ*Mq32;
// hz = MegX*Mq13 + MegY*Mq23 + MegZ*Mq33;
// bx = sqrtf(squa(hx) + squa(hy));
// bz = hz;
// wx = bx*Mq11 + bz*Mq13;
// wy = bx*Mq21 + bz*Mq23;
// wz = bx*Mq31 + bz*Mq33;
ErrX = (AccY * gz - AccZ * gy)/* + (MegY*wz - MegZ*wy)*/;
ErrY = (AccZ * gx - AccX * gz)/* + (MegZ*wx - MegX*wz)*/;
ErrZ = (AccX * gy - AccY * gx)/* + (MegX*wy - MegY*wx)*/;
exInt = exInt + ErrX * Ki;
eyInt = eyInt + ErrY * Ki;
ezInt = ezInt + ErrZ * Ki;
GyrX = toRad(Gyr.TrueX);
GyrY = toRad(Gyr.TrueY);
GyrZ = toRad(Gyr.TrueZ);
GyrX = GyrX + Kp * ErrX + exInt;
GyrY = GyrY + Kp * ErrY + eyInt;
GyrZ = GyrZ + Kp * ErrZ + ezInt;
Quaternion_RungeKutta(&NumQ, GyrX, GyrY, GyrZ, SampleRateHelf);
Quaternion_Normalize(&NumQ);
Quaternion_ToAngE(&NumQ, &AngE);
tempX = (Mag.X * arm_cos_f32(Mag.EllipseSita) + Mag.Y * arm_sin_f32(Mag.EllipseSita)) / Mag.EllipseB;
tempY = (-Mag.X * arm_sin_f32(Mag.EllipseSita) + Mag.Y * arm_cos_f32(Mag.EllipseSita)) / Mag.EllipseA;
AngE.Yaw = atan2f(tempX, tempY);
AngE.Pitch = toDeg(AngE.Pitch);
AngE.Roll = toDeg(AngE.Roll);
AngE.Yaw = toDeg(AngE.Yaw) + 180.0f;
/* 互補濾波 Complementary Filter */
#define CF_A 0.9f
#define CF_B 0.1f
AngZ_Temp = AngZ_Temp + GyrZ * SampleRate;
AngZ_Temp = CF_A * AngZ_Temp + CF_B * AngE.Yaw;
if (AngZ_Temp > 360.0f)
AngE.Yaw = AngZ_Temp - 360.0f;
else if (AngZ_Temp < 0.0f)
AngE.Yaw = AngZ_Temp + 360.0f;
else
AngE.Yaw = AngZ_Temp;
}
