12 explicit Mahony(const float kp = 2.0f, const float ki = 0.01f) :

13 kp(kp), ki(ki)

14 {

15 q[0] = 1.0f;

16 q[1] = 0.0f;

17 q[2] = 0.0f;

18 q[3] = 0.0f;

19 }

22 void update(float gx, float gy, float gz, float ax, float ay, float az, float dt)

23 {

24 float recipNorm;

25 float halfvx, halfvy, halfvz;

26 float halfex, halfey, halfez;

27 float qa, qb, qc;

28

29 // 1. 只有在加速度计数据有效时才进行融合

30 if (!((ax == 0.0f) && (ay == 0.0f) && (az == 0.0f)))

31 {

32

33 // 2. 加速度计归一化

34 recipNorm = invSqrt(ax * ax + ay * ay + az * az);

35 ax *= recipNorm;

36 ay *= recipNorm;

37 az *= recipNorm;

38

39 // 3. 估计重力方向（将四元数换算成旋转矩阵的第三行）

40 // 注意：这里计算的是重力向量的一半，为了和后面的叉积计算抵消系数

41 halfvx = q[1] * q[3] - q[0] * q[2];

42 halfvy = q[0] * q[1] + q[2] * q[3];

43 halfvz = q[0] * q[0] - 0.5f + q[3] * q[3];

44

45 // 4. 计算误差（叉积）：测量重力 vs 估计重力

46 halfex = (ay * halfvz - az * halfvy);

47 halfey = (az * halfvx - ax * halfvz);

48 halfez = (ax * halfvy - ay * halfvx);

49

50 // 5. 积分误差（消除陀螺仪零偏）

51 if (ki > 0.0f)

52 {

53 integralFBx += ki * halfex * dt;

54 integralFBy += ki * halfey * dt;

55 integralFBz += ki * halfez * dt;

56 gx += integralFBx;

57 gy += integralFBy;

58 gz += integralFBz;

59 }

60

61 // 6. 比例增益修正

62 gx += kp * halfex;

63 gy += kp * halfey;

64 gz += kp * halfez;

65 }

66

67 // 7. 四元数一阶龙格库塔积分

68 gx *= (0.5f * dt);

69 gy *= (0.5f * dt);

70 gz *= (0.5f * dt);

71

72 qa = q[0];

73 qb = q[1];

74 qc = q[2];

75

76 q[0] += (-qb * gx - qc * gy - q[3] * gz);

77 q[1] += (qa * gx + qc * gz - q[3] * gy);

78 q[2] += (qa * gy - qb * gz + q[3] * gx);

79 q[3] += (qa * gz + qb * gy - qc * gx);

80

81 // 8. 四元数归一化

82 recipNorm = invSqrt(q[0] * q[0] + q[1] * q[1] + q[2] * q[2] + q[3] * q[3]);

83 q[0] *= recipNorm;

84 q[1] *= recipNorm;

85 q[2] *= recipNorm;

86 q[3] *= recipNorm;

87 };

90 void getEulerAngle(float& pitch, float& roll, float& yaw)

91 {

92 // 这里的转换公式取决于你的定义的旋转顺序，通常RM使用 Z-Y-X

93 // 结果为弧度，如果需要角度需 * 57.29578f

94

95 // Roll (绕X轴)

96 roll = atan2f(2.0f * (q[0] * q[1] + q[2] * q[3]), 1.0f - 2.0f * (q[1] * q[1] + q[2] * q[2]));

97

98 // Pitch (绕Y轴)

99 const float sinp = 2.0f * (q[0] * q[2] - q[3] * q[1]);

100 if (fabsf(sinp) >= 1)

101 pitch = copysignf(std::numbers::pi_v<float> / 2, sinp); // Use 90 degrees if out of range

102 else

103 pitch = asinf(sinp);

104

105 // Yaw (绕Z轴)

106 yaw = atan2f(2.0f * (q[0] * q[3] + q[1] * q[2]), 1.0f - 2.0f * (q[2] * q[2] + q[3] * q[3]));

107 };