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