( G5 T( X6 e; c0 V, ]

尝试制作这个四旋翼飞控的过程，感触颇多，整理了思绪之后，把重要的点一一记下来；

这个飞控是基于STM32，整合了MPU6050，即陀螺仪和重力加速计，但没有融合电子罗盘；

) ^, G- c5 f* _) B  s! _. \; R3 ~

另外，四旋翼飞行器的运动方式请百度百科，不太复杂，具体不再赘述；

) o: n2 X8 A1 v/ y

这是飞控程序的控制流程（一个执行周期）：

比较重要的地方：

1.i2c通信方式；

　　因为我不是学电类专业，最开始对i2c这些是没有一点概念，最后通过Google了解了一些原理，然后发现STM32的开发库是带有i2c通信的相关函数的，但是我最后还是没有用这些函数。

我通过GPIO模拟i2c,这样也能获得mpu6050的数据，虽然代码多了一些，但是比较好的理解i2c的原理。

　　STM32库实现的模拟i2c代码（注释好像因为编码问题跪了）：

1. /*******************************************************************************
   
2. // file                :    i2c_conf.h
   4 Y* o: n( Y# X
3. // MCU                : STM32F103VET6
   
4. // IDE                : Keil uVision4
   " i% r  @) [5 C# m+ Q- ~
5. // date                £º2014.2.28
   5 O% U8 P( e9 U* {3 j
6. *******************************************************************************/
   
7. #include "stm32f10x.h"
   
8. 
   7 b) l% t  ?% ~% l
9. #define   uchar unsigned char
   
10. #define   uint unsigned int
    
11. 
    
12. #define      FALSE 0  
    
13. #define   TRUE  1
    ; q. i* _) q- V- }$ r9 ]' g8 s
14. 
    
15. void I2C_GPIO_Config(void);
    - S( h, P5 b6 D
16. void I2C_delay(void);
    1 m% g5 d" q: t, O
17. void delay5ms(void);
    : ?; l+ e, v' K' S* A$ w0 I) u- b9 Z
18. int I2C_Start(void);
    
19. void I2C_Stop(void);
    " Z/ \6 ?  |. U9 N4 L. }
20. void I2C_Ack(void);
    
21. void I2C_NoAck(void);
    
22. int I2C_WaitAck(void);
    
23. void I2C_SendByte(u8 SendByte);
    ) L, G2 ]  B9 t+ u* S& ~, i& Y5 `
24. unsigned char I2C_RadeByte(void);
    
25. int Single_Write(uchar SlaveAddress,uchar REG_Address,uchar REG_data);
    & c3 Y! l) ]; y" V8 h2 @
26. unsigned char Single_Read(unsigned char SlaveAddress,unsigned char REG_Address);

复制代码

1. /*******************************************************************************
   9 @; I1 E' ~5 @; d% P* ]; C( k+ U
2. // file                :  i2c_conf.c
   
3. // MCU                : STM32F103VET6
   
4. // IDE                : Keil uVision4
   
5. // date                £º2014.2.28
   
6. *******************************************************************************/
   
7. 
   
8. #include "i2c_conf.h"
   
9. 
   # J2 Q7 z& H- W% E( y! @5 m$ _" q
10. #define SCL_H         GPIOB->BSRR = GPIO_Pin_6      
    
11. #define SCL_L         GPIOB->BRR  = GPIO_Pin_6         
    5 q" u6 z8 [+ i) S
12. #define SDA_H         GPIOB->BSRR = GPIO_Pin_7
    * H9 @) {& V1 X# ^! }: D* a
13. #define SDA_L         GPIOB->BRR  = GPIO_Pin_7
    9 h: B$ l! k4 ^; U$ h& G
14. 
    " f# d/ o) ^9 o, h1 e) b9 x. L
15. #define SCL_read      GPIOB->IDR  & GPIO_Pin_6        //IDR:¶Ë¿ÚÊäÈë¼Ä´æÆ÷¡£
    / J$ S3 r' m6 `% w/ L' H
16. #define SDA_read      GPIOB->IDR  & GPIO_Pin_7
    
17. 
    
18.                      
    $ M7 s$ F' V3 `* b2 }
19. 
    ; ?) N( l' D# w) E- @
20. 
    0 v* a- N1 u$ B) h
21. void I2C_GPIO_Config(void)
    
22. {
    
23.   GPIO_InitTypeDef  GPIO_InitStructure;
    ' h% r* I0 Z& b8 T/ O7 m& g; i% Q
24. 
    * R, t& `, P! q+ |+ h8 i. [
25.   GPIO_InitStructure.GPIO_Pin =  GPIO_Pin_6;
    
26.   GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
    0 q. `  C# n: M3 F0 p; V' K
27.   GPIO_InitStructure.GPIO_Mode = GPIO_Mode_Out_OD;   //¿ªÂ©Êä³öģʽ
    
28.   GPIO_Init(GPIOB, &GPIO_InitStructure);
    
29. 
    
30.   GPIO_InitStructure.GPIO_Pin =  GPIO_Pin_7;
    
31.   GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
    4 }% f4 L9 p+ s1 O. G' B% p; h: _
32.   GPIO_InitStructure.GPIO_Mode = GPIO_Mode_Out_OD;
    
33.   GPIO_Init(GPIOB, &GPIO_InitStructure);
    
34. }
    * w" V. U4 B: D. z: M6 P7 K( \) \
35. 
    8 o) ]. G1 x' f6 n. |# p  ^  I; d# l
36. 
    
37. void I2C_delay(void)
    $ S* G  j8 g3 R3 j- S
38. {
    ' b1 j% n! K. Z9 R" E# e3 J/ I4 r/ P
39.         
    
40.    int i=6; //ÕâÀï¿ÉÒÔÓÅ»¯ËٶȠ   £¬¾­²âÊÔ×îµÍµ½5»¹ÄÜдÈë
    8 ~) t4 `0 y) e# y! U4 A& r4 S
41.    while(i)
    
42.    {
    
43.      i--;
    : ~: v9 O4 ?6 [; H* L
44.    }  
    . ^- h+ p- m2 Y  J
45. }
    
46. 
    
47. void delay5ms(void)
    
48. {
    
49.         
    
50.    int i=5000;  
    . x8 b% \" M- u2 g4 p
51.    while(i)
    
52.    {
    3 q9 |, q( ^# W% Q- @
53.      i--;
    9 S' `& O$ H9 d7 ?8 v; j
54.    }  
    
55. }
    
56. 
    $ s" ]. D' R: O
57. 
    
58. int I2C_Start(void)
    
59. {
    
60.     SDA_H;                                            //II2ЭÒé¹æ¶¨±ØÐëÔÚʱÖÓÏßΪµÍµçƽµÄÇ°ÌáÏ£¬²Å¿ÉÒÔÈà Êý¾ÝÏßÐźŸıä
    5 j4 d# K' c1 l
61.     SCL_H;
    
62.     I2C_delay();
    " F6 _+ H- K4 r2 C! \
63.    
    
64.     if(!SDA_read)
    1 S, t; h- {7 V. T" J
65.         return FALSE;                //SDAÏßΪµÍµçƽÔò×ÜÏßæ,Í˳ö
    
66.     SDA_L;                                
    
67.    
    
68.     I2C_delay();
    
69.    
    
70.     if(SDA_read)
    8 i  [8 M  R' C% v9 l2 u
71.         return FALSE;                //SDAÏßΪ¸ßµçƽÔò×ÜÏß³ö´í,Í˳ö
    % U8 F( p4 L8 J2 U0 z0 G
72.     SDA_L;                                
    
73.    
    
74.     I2C_delay();                    
    
75.    
    
76.     return TRUE;
    
77. }
    1 V' w* l# g/ T2 l8 I: R
78. 
    ; o1 L9 T2 t( k. x5 Q" Q0 R
79. 
    
80. 
    ) P7 Y. o2 o. w4 i" u$ u5 h7 u
81. void I2C_Stop(void)
    
82. {
    
83.     SCL_L;
    
84.     I2C_delay();
      \* R- e5 [' C9 s1 r0 S4 [, y
85.     SDA_L;
    
86.     I2C_delay();
    % @) F3 J$ r% [8 r
87.    
    
88.     SCL_H;
    7 d' W# j, `) V1 s4 Y* L' |
89.     I2C_delay();
    3 `# h' x6 Y) O! h
90.     SDA_H;
    ( X! R3 f6 i1 @8 F
91.     I2C_delay();
    
92. }
    # {8 u; B/ |  }5 r+ d& i2 v
93. 
    % d4 m3 }8 q! Y& u! `. _8 s
94. 
    
95. void I2C_Ack(void)
    : U7 P( P" L9 o  w) D: h/ f
96. {   
    . ^9 d7 y/ b2 W- S
97.     SCL_L;
    ' G' M+ ?! o0 Z. Y. h  I" V+ Y. x* O
98.     I2C_delay();
    ; y" Z/ F- \8 r" W; f- t  T
99.     SDA_L;
    
100.     I2C_delay();
     # r3 {6 h8 P. R6 Z6 B( V9 E
101.     SCL_H;
     
102.     I2C_delay();
     : y8 W  D3 D; h6 r
103.     SCL_L;
     
104.     I2C_delay();
     $ `) ?6 ~5 j6 P
105. }   
     $ P9 _; D! u3 g) N( C6 c3 r! q( @
106. 
     $ [% a' X, k: s  n
107. 
     
108. void I2C_NoAck(void)
     
109. {   
     
110.     SCL_L;
     
111.     I2C_delay();
     * w  t! R) F- ^$ }
112.     SDA_H;
     1 H% [! T$ `& z( N1 Y0 ~
113.     I2C_delay();
     ( x# V# i- q0 T; Z8 V' U1 A9 o
114.     SCL_H;
     
115.     I2C_delay();
     
116.     SCL_L;
     ; t, t! Z( P2 E% q- f/ A2 G
117.     I2C_delay();
     $ p! |5 i1 i9 R/ r2 m; M
118. }
     . F+ K7 D/ b5 d: @# m
119. 
     
120. 
     0 u3 W6 C$ t+ s; b
121. 
     ) s/ {; l" a4 Z4 d
122. int I2C_WaitAck(void)      //·µ»ØΪ:=1ÓÐACK,  =0ÎÞACK
     
123. {
     
124.     SCL_L;
     : x% T! Z7 E3 b
125.     I2C_delay();
     
126.     SDA_H;            
     
127.     I2C_delay();
     
128.     SCL_H;
     6 |- J+ N# U2 P7 f7 a4 z; H2 r
129.     I2C_delay();
     , v* w0 H, O5 Q" R2 D
130.     if(SDA_read)
     - g8 Y+ E( a" Q; V% r
131.     {
     ! ]% ?* _: v/ w$ ?. w) S0 o, x
132.       SCL_L;
       S/ \2 y  C* F, g! j4 \; {
133.       I2C_delay();
     " ?5 s* Y; M- q8 Z+ [8 Q) a( X
134.       return FALSE;
     0 [: A2 t) J) z5 b2 q
135.     }
     * n6 l) T$ b! c) s6 x( \* V
136.     SCL_L;
     
137.     I2C_delay();
     
138.     return TRUE;
       t% s- B  v2 Q# H! d: ?
139. }
     
140. 
     
141. 
     ! q" M6 U( M3 l3 i2 V/ _7 r; \# x9 ~
142. 
     * U  Q: P6 _9 S+ g. T* }$ x
143. 
     
144. void I2C_SendByte(u8 SendByte) //Êý¾Ý´Ó¸ßλµ½µÍλ//
     0 w. A7 [9 n3 |5 k. p6 |
145. {
     
146.     u8 i=8;
     
147.     while(i--)
     9 z: t- ~5 R: L4 j& G& N
148.     {
     
149.         SCL_L;
     
150.         I2C_delay();
     : |8 |+ A/ p$ ^& J, [- x- b
151.             
     2 f! z6 l( h  O0 @( g0 ]. }: n
152.       if(SendByte&0x80)        // 0x80 = 1000 0000;
     " {, h& L5 u7 R+ X
153.         SDA_H;  
     # }- i! J& z0 r( T$ ?
154.       else
     8 a! U/ [2 ~9 E& k& L
155.         SDA_L;   
     
156.             
     
157.         SendByte<<=1;   // SendByte×óÒÆһλ¡£
     
158.         I2C_delay();
     
159.             
     / g9 W- r0 Q" N: X/ O# X2 ~& o
160.                 SCL_H;
       M5 `: |6 K& H+ q7 _3 x  j* O" d6 o
161.         I2C_delay();
     9 ^- `1 m0 {/ R" h% J4 ]/ Q
162.     }
     & i; L1 F# _& C
163.     SCL_L;
     
164. }  
     
165. 
     
166. 
     8 A: n9 g0 }/ {  H
167. 
     6 Y" ?2 [0 l  p- l& X
168. 
     + ]  f& Y6 U3 [! t
169. unsigned char I2C_RadeByte(void)  //Êý¾Ý´Ó¸ßλµ½µÍλ//
     ' _( f  o0 u7 \) h4 O& t; e- {
170. {
     . ?% O# H& L; Q. i
171.     u8 i=8;
     
172.     u8 ReceiveByte=0;
     ; j) `' c  ~- v- O0 E4 U& n
173. 
     , J1 I7 k6 I# o+ _6 I+ J- q* k9 {4 y1 H
174.     SDA_H;               
     
175.     while(i--)
     " g! l# ^0 ]; _6 W
176.     {
     
177.       ReceiveByte<<=1;      //×óÒÆһ룬
     
178.             
     # y4 {# W' P5 \8 k* \
179.       SCL_L;
     
180.       I2C_delay();
     ( a9 `, P; i0 k; q2 m
181.             
     
182.             SCL_H;
     1 Z  t. _; q2 F: `
183.       I2C_delay();   
     
184.             
     5 J+ I6 Z4 ^! {  n# @7 M
185.       if(SDA_read)
     
186.       {
     
187.         ReceiveByte|=0x01; //дÈë
     
188.       }
     / Z) r+ r! w  _
189.     }
     2 o) b) Y: h5 s+ q7 [
190.     SCL_L;
     
191.     return ReceiveByte;
     
192. }
     
193. 
     6 @3 r4 }7 t& J/ p+ q4 \! X+ J
194. 
     9 w3 ~( I; [" q4 h0 J: k
195. 
     
196. int Single_Write(uchar SlaveAddress,uchar REG_Address,uchar REG_data)            
     % A9 G# j$ a0 w7 w
197. {
     
198.       if(!I2C_Start())
     
199.             return FALSE;
     
200.     I2C_SendByte(SlaveAddress);   //·¢ËÍÉ豸µØÖ·+дÐźŠ   //I2C_SendByte(((REG_Address & 0x0700) >>7) | SlaveAddress & 0xFFFE);    //ÉèÖøßÆðʼµØÖ·+Æ÷¼þµØÖ·
     / U* i. X$ `& l; e- c
201.    
     , X7 _9 x$ V2 i
202.         if(!I2C_WaitAck())
     
203.         {
     4 t; r3 Q. q6 ], O" T6 \1 B
204.             I2C_Stop();
     " h0 [* `$ C) d% ]$ [0 [9 W0 ?
205.             return FALSE;
     
206.         }
     
207.    
     0 i7 m- x5 E8 H  d
208.         I2C_SendByte(REG_Address );   //ÉèÖõÍÆðʼµØÖ·      
     / p& l7 c& z7 D7 F! B
209.     I2C_WaitAck();   
     
210.    
     2 _" C0 t0 _+ q+ J1 [
211.         I2C_SendByte(REG_data);
     ; L, y; h" G; }8 J. y: e# _
212.     I2C_WaitAck();   
       a  k5 T0 f. [. E7 A+ x5 l
213.    
     ; G: x9 A6 E, Q
214.         I2C_Stop();
     
215.     delay5ms();
     + N9 ]# d: G3 N7 ]5 y: f4 ]! V
216.     return TRUE;
     ! N, o, ^3 o  N, D  y. I6 l
217. }
     2 r, H4 O0 Q4 M
218. 
     
219. 
     # [  T# y+ \) Q& D* k% s; [
220. 
     3 M+ V( C& a' ]) G# a; Z& v+ b  @+ H
221. unsigned char Single_Read(unsigned char SlaveAddress,unsigned char REG_Address)
     . ^, x( g9 O5 m8 J1 g  C
222. {   
     
223.     unsigned char REG_data;
     
224. 
     
225.    
     
226.         if(!I2C_Start())
     
227.             return FALSE;
     
228.     I2C_SendByte(SlaveAddress); //I2C_SendByte(((REG_Address & 0x0700) >>7) | REG_Address & 0xFFFE);//ÉèÖøßÆðʼµØÖ·+Æ÷¼þµØÖ·
     
229.     if(!I2C_WaitAck())
     & u" a5 ?2 ]" D' G5 D2 T
230.         {
     
231.             I2C_Stop();
     
232. 
     1 X( H/ x- c$ K
233.             return FALSE;
     
234.         }
     
235.     I2C_SendByte((u8) REG_Address);   //ÉèÖõÍÆðʼµØÖ·      
     
236.     I2C_WaitAck();
     
237.         I2C_Start();
     . O9 B, `# ^+ W4 x& B& D- q) L
238.    
     4 B& L" {* y" [# m0 c; `" D
239.         I2C_SendByte(SlaveAddress+1);
     
240.     I2C_WaitAck();
     
241. 
     
242.         REG_data= I2C_RadeByte();
     , k9 A! G# m) M6 X
243.     I2C_NoAck();
     
244.     I2C_Stop();
     
245. 
     
246.     return REG_data;
     
247. 
     
248. }

复制代码

& F8 R( S7 U: ]0 z. O# {) y

2.mpu6050;

　　然后用写好的模拟i2c函数读取mpu6050，根据mpu6050手册的各寄存器地址，读取到了重力加速计和陀螺仪的各分量;

$ ~& m5 @6 C+ X+ N9 a! F

传感器采样率设置为200Hz,这个值是因为我电调频率为200Hz，也就是说，我的程序循环一次0.005s,一般来说，采样率高点没问题，别比执行一次闭环控制的周期长就行了；

1 z: Y4 V* A3 a: S6 _

陀螺仪量程±2000°/s，加速计量程±2g， 量程越大，取值越不精确；

这里注意，由于我们没有采用磁力计，而陀螺仪存在零偏，所以最终在yaw方向上没有绝对的参考系，不能建立绝对的地理坐标系，这样最好的结果也仅仅是在yaw上存在缓慢漂移。

3.互补滤波；

　　融合时，陀螺仪的积分运算很大程度上决定了飞行器的瞬时运动情况，而重力加速计通过长时间的累积不断矫正陀螺仪产生的误差，最终得到准确的机体姿态。

　　这里我们采用Madgwick提供的UpdateIMU算法来得到姿态角所对应的四元数，之后只需要经过简单运算便可转换为实时欧拉角。感谢Madgwick大大为开源做出的贡献。

1. #define sampleFreq    512.0f        // sample frequency in Hz
   
2. #define betaDef        0.1f        // 2 * proportional gain
   % s1 \  B1 m# d( G1 F
3. 
   , {1 x, i. J% h/ y  R
4. 
   + ^$ N. R" Q" s% M* S
5. volatile float beta = betaDef;
   
6. volatile float q0 = 1.0f, q1 = 0.0f, q2 = 0.0f, q3 = 0.0f;
   
7. 
   
8. float invSqrt(float x);
   6 K& i) y- l7 M! V* _$ m
9. 
   
10. void MadgwickAHRSupdateIMU(float gx, float gy, float gz, float ax, float ay, float az) {
    9 Y6 l0 U3 t- `. y2 t1 V( i
11.     float recipNorm;
    
12.     float s0, s1, s2, s3;
    * Z4 k4 N' r/ H2 j/ Q. c
13.     float qDot1, qDot2, qDot3, qDot4;
    + B' q( N3 C, m- D
14.     float _2q0, _2q1, _2q2, _2q3, _4q0, _4q1, _4q2 ,_8q1, _8q2, q0q0, q1q1, q2q2, q3q3;
    
15. 
    ' t# l# r: `! V2 ~- s; k: W- F
16.     // Rate of change of quaternion from gyroscope
    
17.     qDot1 = 0.5f * (-q1 * gx - q2 * gy - q3 * gz);
    1 [! I0 [8 N6 \6 r
18.     qDot2 = 0.5f * (q0 * gx + q2 * gz - q3 * gy);
    
19.     qDot3 = 0.5f * (q0 * gy - q1 * gz + q3 * gx);
    , c6 T# f' E7 X4 x. n2 v: n
20.     qDot4 = 0.5f * (q0 * gz + q1 * gy - q2 * gx);
    
21. 
    * N# X+ l7 @* G' D8 P0 z
22.     // Compute feedback only if accelerometer measurement valid (avoids NaN in accelerometer normalisation)
    
23.     if(!((ax == 0.0f) && (ay == 0.0f) && (az == 0.0f))) {
    + f9 l' M+ L* n; [% B# ^
24. 
    & e. Y5 o. w# R9 {6 Q  a  U0 H
25.         // Normalise accelerometer measurement
    
26.         recipNorm = invSqrt(ax * ax + ay * ay + az * az);
    
27.         ax *= recipNorm;
    % l6 {% Q. ~' x: K
28.         ay *= recipNorm;
    
29.         az *= recipNorm;
    
30. 
    % k8 B8 G# K2 M* W- {" g
31.         // Auxiliary variables to avoid repeated arithmetic
    
32.         _2q0 = 2.0f * q0;
    : p/ T2 A. p* P
33.         _2q1 = 2.0f * q1;
    
34.         _2q2 = 2.0f * q2;
    % K+ u* M, A2 G9 Y9 `
35.         _2q3 = 2.0f * q3;
      b  W( X1 w8 p5 r4 z
36.         _4q0 = 4.0f * q0;
    : y# _/ @) R8 [7 {2 K
37.         _4q1 = 4.0f * q1;
    6 _+ m! Q) h8 r1 I' J) n
38.         _4q2 = 4.0f * q2;
    7 Q1 E+ ]+ K8 D
39.         _8q1 = 8.0f * q1;
    * I" n0 Q; z7 g7 Z9 K
40.         _8q2 = 8.0f * q2;
    
41.         q0q0 = q0 * q0;
    
42.         q1q1 = q1 * q1;
    
43.         q2q2 = q2 * q2;
    $ U! S2 q# Y/ b8 N! X  H0 u
44.         q3q3 = q3 * q3;
    4 A4 E0 ~* X* d- J$ A1 J
45. 
    ! J% I  h' i6 }" n$ x
46.         // Gradient decent algorithm corrective step
    
47.         s0 = _4q0 * q2q2 + _2q2 * ax + _4q0 * q1q1 - _2q1 * ay;
    # z; ~$ h" E: p# ]" [2 u. o
48.         s1 = _4q1 * q3q3 - _2q3 * ax + 4.0f * q0q0 * q1 - _2q0 * ay - _4q1 + _8q1 * q1q1 + _8q1 * q2q2 + _4q1 * az;
    
49.         s2 = 4.0f * q0q0 * q2 + _2q0 * ax + _4q2 * q3q3 - _2q3 * ay - _4q2 + _8q2 * q1q1 + _8q2 * q2q2 + _4q2 * az;
    * R2 k# h8 n* y9 E
50.         s3 = 4.0f * q1q1 * q3 - _2q1 * ax + 4.0f * q2q2 * q3 - _2q2 * ay;
    / \% n, r$ x, ^4 @5 q0 P9 H. k' T' N
51.         recipNorm = invSqrt(s0 * s0 + s1 * s1 + s2 * s2 + s3 * s3); // normalise step magnitude
    
52.         s0 *= recipNorm;
    
53.         s1 *= recipNorm;
    
54.         s2 *= recipNorm;
    " J" j! s/ M, F3 w
55.         s3 *= recipNorm;
    5 m: B. |$ h  D2 y3 I
56. 
    & h0 M, s0 S8 f: U
57.         // Apply feedback step
    
58.         qDot1 -= beta * s0;
    ' Y% z  G' H" f' x. w8 N$ q
59.         qDot2 -= beta * s1;
    
60.         qDot3 -= beta * s2;
    % _: q# l# H( h) ^( ^  i/ `& Z( Q! z
61.         qDot4 -= beta * s3;
    
62.     }
    & n! K4 W* F/ {# s
63. 
    
64.     // Integrate rate of change of quaternion to yield quaternion
    
65.     q0 += qDot1 * (1.0f / sampleFreq);
    
66.     q1 += qDot2 * (1.0f / sampleFreq);
    
67.     q2 += qDot3 * (1.0f / sampleFreq);
    
68.     q3 += qDot4 * (1.0f / sampleFreq);
    / p- h2 K' l& Y: A  Z6 V! _
69. 
    
70.     // Normalise quaternion
    
71.     recipNorm = invSqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3);
    
72.     q0 *= recipNorm;
    
73.     q1 *= recipNorm;
    
74.     q2 *= recipNorm;
    % m) [2 W8 Y# z" f* M  s5 U1 }1 E0 o
75.     q3 *= recipNorm;
    , l( d& m+ z. ?' `! _
76. }
    ; A4 I& w& P9 m0 N2 L) M
77. 
    
78. 
    2 u8 Q2 Q# @; c; I: B7 N+ h
79. float invSqrt(float x) {
    ( A( d/ g# r5 r# E0 ?
80.     float halfx = 0.5f * x;
    
81.     float y = x;
    
82.     long i = *(long*)&y;
    
83.     i = 0x5f3759df - (i>>1);
    
84.     y = *(float*)&i;
    
85.     y = y * (1.5f - (halfx * y * y));
    ' B. r/ R* M3 E
86.     return y;
      P0 b- W3 {: L6 t( w
87. }

复制代码

4.获取期望姿态；

　　也就是遥控部分了，让用户介入控制。

　　本着拿来主义的原则，用上”圆点博士开源项目”提供的安卓的开源蓝牙控制端。

　　圆点博士给出了数据包格式，同过HC-06蓝牙模块接连到STM32串口1，再无线连接到控制端，这样我们就可以获得控制端不断发送的数据包了，并实时更新期望姿态角，这里只需要注意输出的姿态角和实时姿态角方向一致以及数据包的校验就行了。

7 c/ ]( r) R% T, C2 p

5.PID控制算法；

　　由于简单的线性控制不可能满足四轴飞行器这个灵敏的系统，引入PID控制器来更好的纠正系统。

　　简介：PID实指“比例proportional”、“积分integral”、“微分derivative”，这三项构成PID基本要素。每一项完成不同任务，对系统功能产生不同的影响。

以Pitch为例：

　　

error为期望角减去实时角度得到的误差；

iState为积分i参数对应累积过去时间里的误差总和；

if语句限定iState范围，繁殖修正过度；

微分d参数为当前姿态减去上次姿态，估算当前速度（瞬间速度）；

总调整量为p,i,d三者之和；

这样，P代表控制系统的响应速度，越大，响应越快。

I，用来累积过去时间内的误差，修正P无法达到的期望姿态值（静差）；

D，加强对机体变化的快速响应，对P有抑制作用。

PID各参数的整定需要综合考虑控制系统的各个方面，才能达到最佳效果。

; A6 j5 ~9 z9 ~. H- x' e; ]

输出PWM信号：

PID计算完成之后，便可以通过STM32自带的定时资源很容易的调制出四路pwm信号，采用的电调pwm格式为50Hz，高电平持续时间0.5ms-2.5ms；

我以1.0ms-2.0ms为每个电机的油门行程，这样，1ms的宽度均匀的对应电调的从最低到最高转速。

至此，一个用stm32和mpu6050搭建的飞控系统就算实现了。

剩下的调试PID参数了。

  g8 ?2 s' `& ], i* r% o; R转载自：Moran_