/*====================================================================================================*/ void AHRS_Geteuler(void) { fp32 sin_pitch,sin_roll,cos_roll,cos_pitch; AHRS_getValues(); // 获取四元数 AHRS_GetQ(&NumQ); // 四元数转欧拉角 Quaternion_ToAngE(&NumQ, &AngE); // 计算欧拉角的三角函数值 sin_roll = sin(AngE.Roll); sin_pitch = sin(AngE.Pitch); cos_roll = cos(AngE.Roll); cos_pitch = cos(AngE.Pitch); // 地磁不存在或地磁数据不正常则停用地磁数据 if(!flag.MagIssue && flag.MagExist){ // 地磁倾角补偿 fp32 hx = MAG[0]*cos_pitch + MAG[1]*sin_pitch*sin_roll - MAG[2]*cos_roll*sin_pitch; fp32 hy = MAG[1]*cos_roll + MAG[2]*sin_roll; // 利用地磁解算航向角 fp32 mag_yaw = -Degree(atan2((fp64)hy,(fp64)hx)); // 陀螺仪积分解算航向角 AngE.Yaw += Degree(sensor.gyro.averag.z * Gyro_Gr * 2 * SampleRateHalf); // 地磁解算的航向角与陀螺仪积分解算的航向角进行互补融合 if((mag_yaw>90 && AngE.Yaw<-90) || (mag_yaw<-90 && AngE.Yaw>90)) AngE.Yaw = -AngE.Yaw * 0.998f + mag_yaw * 0.002f; else AngE.Yaw = AngE.Yaw * 0.998f + mag_yaw * 0.002f; } else AngE.Yaw = Degree(AngE.Yaw); AngE.Roll = Degree(AngE.Roll); // roll AngE.Pitch = Degree(AngE.Pitch); // pitch }
/*=====================================================================================================*/ 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; }