标题: Mahony AHRS姿态控制源码 [打印本页]

---

作者: @lins    时间: 2018-4-5 18:41
标题: Mahony AHRS姿态控制源码
单片机源程序如下:

1. #define sampleFreq 512.0f
   
2. #define twoKpDef   (2.0f*0.5f)
   
3. #define twoKiDef   (2.0f*0.5f)
   
4. volatile float twoKp=twoKpDef;
   
5. volatile float twoKi=twoKiDef;
   
6. volatile float q0=1.0f,q1=1.0f,q2=1.0f,q3=1.0f;
   
7. volatile float integralFBx=0.0f,integralFBy=0.0f,integralFBz=0.0f;
   
8. //---------------------------------------------------------------------------------------------------
   
9. // Fast inverse square-root
   
10. // See: http://en.wikipedia.org/wiki/Fast_inverse_square_root
    
11. float invSqrt(float x) {
    
12. float halfx = 0.5f * x;
    
13. float y = x;
    
14. long i = *(long*)&y;
    
15. i = 0x5f3759df - (i>>1);
    
16. y = *(float*)&i;
    
17. y = y * (1.5f - (halfx * y * y));
    
18. return y;
    
19. }
    
20. 
    
21. //---------------------------------------------------------------------------------------------------
    
22. // IMU algorithm update
    
23. 
    
24. void MahonyAHRSupdateIMU(float gx, float gy, float gz, float ax, float ay, float az) {
    
25. 
    
26. float recipNorm;
    
27. float halfvx, halfvy, halfvz;
    
28. float halfex, halfey, halfez;
    
29. float qa, qb, qc;
    
30. 
    
31. // Compute feedback only if accelerometer measurement valid (avoids NaN in accelerometer normalisation)
    
32. if(!((ax == 0.0f) && (ay == 0.0f) && (az == 0.0f))) {
    
33. 
    
34. 
    
35. // Normalise accelerometer measurement
    
36. recipNorm = invSqrt(ax * ax + ay * ay + az * az);
    
37. ax *= recipNorm;
    
38. ay *= recipNorm;
    
39. az *= recipNorm;        
    
40. 
    
41. // Estimated direction of gravity and vector perpendicular to magnetic flux
    
42. halfvx = q1 * q3 - q0 * q2;
    
43. halfvy = q0 * q1 + q2 * q3;
    
44. halfvz = q0 * q0 - 0.5f + q3 * q3;
    
45. 
    
46. // Error is sum of cross product between estimated and measured direction of gravity
    
47. halfex = (ay * halfvz - az * halfvy);
    
48. halfey = (az * halfvx - ax * halfvz);
    
49. halfez = (ax * halfvy - ay * halfvx);
    
50. 
    
51. // Compute and apply integral feedback if enabled
    
52. if(twoKi > 0.0f) {
    
53. 
    
54. integralFBx += twoKi * halfex * (1.0f / sampleFreq);// integral error scaled by Ki
    
55. integralFBy += twoKi * halfey * (1.0f / sampleFreq);
    
56. integralFBz += twoKi * halfez * (1.0f / sampleFreq);
    
57. gx += integralFBx;// apply integral feedback
    
58. gy += integralFBy;
    
59. gz += integralFBz;
    
60. 
    
61. }
    
62. else {
    
63. 
    
64. integralFBx = 0.0f;// prevent integral windup
    
65. integralFBy = 0.0f;
    
66. integralFBz = 0.0f;
    
67. 
    
68. }
    
69. 
    
70. // Apply proportional feedback
    
71. gx += twoKp * halfex;
    
72. gy += twoKp * halfey;
    
73. gz += twoKp * halfez;
    
74. 
    
75. }
    
76. 
    
77. // Integrate rate of change of quaternion
    
78. gx *= (0.5f * (1.0f / sampleFreq));// pre-multiply common factors
    
79. gy *= (0.5f * (1.0f / sampleFreq));
    
80. gz *= (0.5f * (1.0f / sampleFreq));
    
81. qa = q0;
    
82. qb = q1;
    
83. qc = q2;
    
84. q0 += (-qb * gx - qc * gy - q3 * gz);
    
85. q1 += (qa * gx + qc * gz - q3 * gy);
    
86. q2 += (qa * gy - qb * gz + q3 * gx);
    
87. q3 += (qa * gz + qb * gy - qc * gx);
    
88. 
    
89. // Normalise quaternion
    
90. recipNorm = invSqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3);
    
91. q0 *= recipNorm;
    
92. q1 *= recipNorm;
    
93. q2 *= recipNorm;
    
94. q3 *= recipNorm;
    
95. 
    
96. }
    
97. 
    
98. //---------------------------------------------------------------------------------------------------
    
99. //
    
100. typedef struct{
     
101.                                 float roll;
     
102.                                 float pitch;
     
103.                                 float yaw;}T_float_angle;
     
104. T_float_angle                 Att_Angle;        //×Ë쬽Ç
     
105. /*gx,gy,gzíóÂYòÇ¡ax,ay,az¼óËù¶è¡￠￠*/
     
106. void MahonyAHRSupdate(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz,T_float_angle *angle ) {  
     
107.     float recipNorm;  
     
108.     float q0q0, q0q1, q0q2, q0q3, q1q1, q1q2, q1q3, q2q2, q2q3, q3q3;   
     
109.     float hx, hy, bx, bz;  
     
110.     float halfvx, halfvy, halfvz, halfwx, halfwy, halfwz;  
     
111.     float halfex, halfey, halfez;  
     
112.     float qa, qb, qc;  
     
113. 
     
114.     // Use IMU algorithm if magnetometer measurement invalid (avoids NaN in magnetometer normalisation)  
     
115.     if((mx == 0.0f) && (my == 0.0f) && (mz == 0.0f)) {  
     
116.         MahonyAHRSupdateIMU(gx, gy, gz, ax, ay, az);  
     
117.         return;  
     
118.     }  
     
119. 
     
120.     // Compute feedback only if accelerometer measurement valid (avoids NaN in accelerometer normalisation)  
     
121.     if(!((ax == 0.0f) && (ay == 0.0f) && (az == 0.0f))) {  
     
122. 
     
123.         // Normalise accelerometer measurement  
     
124.         recipNorm = invSqrt(ax * ax + ay * ay + az * az);  
     
125.         ax *= recipNorm;  
     
126.         ay *= recipNorm;  
     
127.         az *= recipNorm;      
     
128. 
     
129.         // Normalise magnetometer measurement  
     
130.         recipNorm = invSqrt(mx * mx + my * my + mz * mz);  
     
131.         mx *= recipNorm;  
     
132.         my *= recipNorm;  
     
133.         mz *= recipNorm;     
     
134. 
     
135.         // Auxiliary variables to avoid repeated arithmetic  
     
136.         q0q0 = q0 * q0;  
     
137.         q0q1 = q0 * q1;  
     
138.         q0q2 = q0 * q2;  
     
139.         q0q3 = q0 * q3;  
     
140.         q1q1 = q1 * q1;  
     
141.         q1q2 = q1 * q2;  
     
142.         q1q3 = q1 * q3;  
     
143.         q2q2 = q2 * q2;  
     
144.         q2q3 = q2 * q3;  
     
145.         q3q3 = q3 * q3;     
     
146. 
     
147.         // Reference direction of Earth's magnetic field  
     
148.         hx = 2.0f * (mx * (0.5f - q2q2 - q3q3) + my * (q1q2 - q0q3) + mz * (q1q3 + q0q2));  
     
149.         hy = 2.0f * (mx * (q1q2 + q0q3) + my * (0.5f - q1q1 - q3q3) + mz * (q2q3 - q0q1));  
     
150.         bx = sqrt(hx * hx + hy * hy);  
     
151.         bz = 2.0f * (mx * (q1q3 - q0q2) + my * (q2q3 + q0q1) + mz * (0.5f - q1q1 - q2q2));  
     
152. 
     
153.         // Estimated direction of gravity and magnetic field  
     
154.         halfvx = q1q3 - q0q2;  
     
155.         halfvy = q0q1 + q2q3;  
     
156.         halfvz = q0q0 - 0.5f + q3q3;  
     
157.         halfwx = bx * (0.5f - q2q2 - q3q3) + bz * (q1q3 - q0q2);  
     
158.         halfwy = bx * (q1q2 - q0q3) + bz * (q0q1 + q2q3);  
     
159.         halfwz = bx * (q0q2 + q1q3) + bz * (0.5f - q1q1 - q2q2);   
     
160. 
     
161.         // Error is sum of cross product between estimated direction and measured direction of field vectors  
     
162.         halfex = (ay * halfvz - az * halfvy) + (my * halfwz - mz * halfwy);  
     
163.         halfey = (az * halfvx - ax * halfvz) + (mz * halfwx - mx * halfwz);  
     
164.         halfez = (ax * halfvy - ay * halfvx) + (mx * halfwy - my * halfwx);  
     
165. 
     
166.         // Compute and apply integral feedback if enabled  
     
167.         if(twoKi > 0.0f) {  
     
168.             integralFBx += twoKi * halfex * (1.0f / sampleFreq);    // integral error scaled by Ki  
     
169.             integralFBy += twoKi * halfey * (1.0f / sampleFreq);  
     
170.             integralFBz += twoKi * halfez * (1.0f / sampleFreq);  
     
171.             gx += integralFBx;  // apply integral feedback  
     
172.             gy += integralFBy;  
     
173.             gz += integralFBz;  
     
174.         }  
     
175.         else {  
     
176.             integralFBx = 0.0f; // prevent integral windup  
     
177.             integralFBy = 0.0f;  
     
178.             integralFBz = 0.0f;  
     
179.         }  
     
180. 
     
181.         // Apply proportional feedback  
     
182.         gx += twoKp * halfex;  
     
183.         gy += twoKp * halfey;  
     
184.         gz += twoKp * halfez;  
     
185.     }  
     
186. 
     
187.     // Integrate rate of change of quaternion  
     
188.     gx *= (0.5f * (1.0f / sampleFreq));     // pre-multiply common factors  
     
189.     gy *= (0.5f * (1.0f / sampleFreq));  
     
190.     gz *= (0.5f * (1.0f / sampleFreq));  
     
191.     qa = q0;  
     
192.     qb = q1;  
     
193.     qc = q2;  
     
194.     q0 += (-qb * gx - qc * gy - q3 * gz);  
     
195.     q1 += (qa * gx + qc * gz - q3 * gy);  
     
196.     q2 += (qa * gy - qb * gz + q3 * gx);  
     
197.     q3 += (qa * gz + qb * gy - qc * gx);   
     
198. 
     
199.     // Normalise quaternion  
     
200.     recipNorm = invSqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3);  
     
201.     q0 *= recipNorm;  
     
202.     q1 *= recipNorm;  
     
203.     q2 *= recipNorm;  
     
204.     q3 *= recipNorm;        
     
205. }
     
206. 
     

复制代码

---

欢迎光临 (http://www.51hei.com/bbs/)

Powered by Discuz! X3.1