Códigos Utilizados
A.3 Driver MPU9250
A.3.1 MPU9250.h
32 # d e f i n e A K 8 9 6 3 _ A S A Z 0 x12
33
34 # d e f i n e S E L F _ T E S T _ X _ G Y R O 0 x00
35 # d e f i n e S E L F _ T E S T _ Y _ G Y R O 0 x01
36 # d e f i n e S E L F _ T E S T _ Z _ G Y R O 0 x02
37
38 /*
39 # d e f i n e X _ F I N E _ G A I N 0 x03 // [ 7 : 0 ] f i n e g a i n
40 # d e f i n e Y _ F I N E _ G A I N 0 x04
41 # d e f i n e Z _ F I N E _ G A I N 0 x05
42 # d e f i n e X A _ O F F S E T _ H 0 x06 // User - d e f i n e d t r i m v a l u e s for a c c e l e r o m e t e r
43 # d e f i n e X A _ O F F S E T _ L _ T C 0 x07
44 # d e f i n e Y A _ O F F S E T _ H 0 x08
45 # d e f i n e Y A _ O F F S E T _ L _ T C 0 x09
46 # d e f i n e Z A _ O F F S E T _ H 0 x0A
47 # d e f i n e Z A _ O F F S E T _ L _ T C 0 x0B */
48
49 # d e f i n e S E L F _ T E S T _ X _ A C C E L 0 x0D
50 # d e f i n e S E L F _ T E S T _ Y _ A C C E L 0 x0E
51 # d e f i n e S E L F _ T E S T _ Z _ A C C E L 0 x0F
52
53 # d e f i n e S E L F _ T E S T _ A 0 x10
54
55 # d e f i n e X G _ O F F S E T _ H 0 x13 // User - d e f i n e d t r i m v a l u e s for g y r o s c o p e
56 # d e f i n e X G _ O F F S E T _ L 0 x14
57 # d e f i n e Y G _ O F F S E T _ H 0 x15
58 # d e f i n e Y G _ O F F S E T _ L 0 x16
59 # d e f i n e Z G _ O F F S E T _ H 0 x17
60 # d e f i n e Z G _ O F F S E T _ L 0 x18
61 # d e f i n e S M P L R T _ D I V 0 x19
62 # d e f i n e C O N F I G 0 x1A
63 # d e f i n e G Y R O _ C O N F I G 0 x1B
64 # d e f i n e A C C E L _ C O N F I G 0 x1C
65 # d e f i n e A C C E L _ C O N F I G 2 0 x1D
66 # d e f i n e L P _ A C C E L _ O D R 0 x1E
67 # d e f i n e W O M _ T H R 0 x1F
68
69 // D u r a t i o n c o u n t e r t h r e s h o l d for m o t i o n i n t e r r u p t g e n e r a t i o n , 1 kHz rate ,
70 // LSB = 1 ms
71 # d e f i n e M O T _ D U R 0 x20
72 // Zero - m o t i o n d e t e c t i o n t h r e s h o l d b i t s [ 7 : 0 ]
73 # d e f i n e Z M O T _ T H R 0 x21
74 // D u r a t i o n c o u n t e r t h r e s h o l d for z e r o m o t i o n i n t e r r u p t g e n e r a t i o n , 16 Hz rate ,
75 // LSB = 64 ms
76 # d e f i n e Z R M O T _ D U R 0 x22
77
78 # d e f i n e F I F O _ E N 0 x23
79 # d e f i n e I 2 C _ M S T _ C T R L 0 x24
80 # d e f i n e I 2 C _ S L V 0 _ A D D R 0 x25
81 # d e f i n e I 2 C _ S L V 0 _ R E G 0 x26
82 # d e f i n e I 2 C _ S L V 0 _ C T R L 0 x27
83 # d e f i n e I 2 C _ S L V 1 _ A D D R 0 x28
84 # d e f i n e I 2 C _ S L V 1 _ R E G 0 x29
85 # d e f i n e I 2 C _ S L V 1 _ C T R L 0 x2A
86 # d e f i n e I 2 C _ S L V 2 _ A D D R 0 x2B
87 # d e f i n e I 2 C _ S L V 2 _ R E G 0 x2C
88 # d e f i n e I 2 C _ S L V 2 _ C T R L 0 x2D
89 # d e f i n e I 2 C _ S L V 3 _ A D D R 0 x2E
90 # d e f i n e I 2 C _ S L V 3 _ R E G 0 x2F
91 # d e f i n e I 2 C _ S L V 3 _ C T R L 0 x30
92 # d e f i n e I 2 C _ S L V 4 _ A D D R 0 x31
93 # d e f i n e I 2 C _ S L V 4 _ R E G 0 x32
94 # d e f i n e I 2 C _ S L V 4 _ D O 0 x33
95 # d e f i n e I 2 C _ S L V 4 _ C T R L 0 x34
96 # d e f i n e I 2 C _ S L V 4 _ D I 0 x35
97 # d e f i n e I 2 C _ M S T _ S T A T U S 0 x36
98 # d e f i n e I N T _ P I N _ C F G 0 x37
99 # d e f i n e I N T _ E N A B L E 0 x38
100 # d e f i n e D M P _ I N T _ S T A T U S 0 x39 // C h e c k DMP i n t e r r u p t
101 # d e f i n e I N T _ S T A T U S 0 x3A
102 # d e f i n e A C C E L _ X O U T _ H 0 x3B
103 # d e f i n e A C C E L _ X O U T _ L 0 x3C
104 # d e f i n e A C C E L _ Y O U T _ H 0 x3D
105 # d e f i n e A C C E L _ Y O U T _ L 0 x3E
107 # d e f i n e A C C E L _ Z O U T _ L 0 x40
108 # d e f i n e T E M P _ O U T _ H 0 x41
109 # d e f i n e T E M P _ O U T _ L 0 x42
110 # d e f i n e G Y R O _ X O U T _ H 0 x43
111 # d e f i n e G Y R O _ X O U T _ L 0 x44
112 # d e f i n e G Y R O _ Y O U T _ H 0 x45
113 # d e f i n e G Y R O _ Y O U T _ L 0 x46
114 # d e f i n e G Y R O _ Z O U T _ H 0 x47
115 # d e f i n e G Y R O _ Z O U T _ L 0 x48
116 # d e f i n e E X T _ S E N S _ D A T A _ 0 0 0 x49
117 # d e f i n e E X T _ S E N S _ D A T A _ 0 1 0 x4A
118 # d e f i n e E X T _ S E N S _ D A T A _ 0 2 0 x4B
119 # d e f i n e E X T _ S E N S _ D A T A _ 0 3 0 x4C
120 # d e f i n e E X T _ S E N S _ D A T A _ 0 4 0 x4D
121 # d e f i n e E X T _ S E N S _ D A T A _ 0 5 0 x4E
122 # d e f i n e E X T _ S E N S _ D A T A _ 0 6 0 x4F
123 # d e f i n e E X T _ S E N S _ D A T A _ 0 7 0 x50
124 # d e f i n e E X T _ S E N S _ D A T A _ 0 8 0 x51
125 # d e f i n e E X T _ S E N S _ D A T A _ 0 9 0 x52
126 # d e f i n e E X T _ S E N S _ D A T A _ 1 0 0 x53
127 # d e f i n e E X T _ S E N S _ D A T A _ 1 1 0 x54
128 # d e f i n e E X T _ S E N S _ D A T A _ 1 2 0 x55
129 # d e f i n e E X T _ S E N S _ D A T A _ 1 3 0 x56
130 # d e f i n e E X T _ S E N S _ D A T A _ 1 4 0 x57
131 # d e f i n e E X T _ S E N S _ D A T A _ 1 5 0 x58
132 # d e f i n e E X T _ S E N S _ D A T A _ 1 6 0 x59
133 # d e f i n e E X T _ S E N S _ D A T A _ 1 7 0 x5A
134 # d e f i n e E X T _ S E N S _ D A T A _ 1 8 0 x5B
135 # d e f i n e E X T _ S E N S _ D A T A _ 1 9 0 x5C
136 # d e f i n e E X T _ S E N S _ D A T A _ 2 0 0 x5D
137 # d e f i n e E X T _ S E N S _ D A T A _ 2 1 0 x5E
138 # d e f i n e E X T _ S E N S _ D A T A _ 2 2 0 x5F
139 # d e f i n e E X T _ S E N S _ D A T A _ 2 3 0 x60
140 # d e f i n e M O T _ D E T E C T _ S T A T U S 0 x61
141 # d e f i n e I 2 C _ S L V 0 _ D O 0 x63
142 # d e f i n e I 2 C _ S L V 1 _ D O 0 x64
143 # d e f i n e I 2 C _ S L V 2 _ D O 0 x65
144 # d e f i n e I 2 C _ S L V 3 _ D O 0 x66
145 # d e f i n e I 2 C _ M S T _ D E L A Y _ C T R L 0 x67
146 # d e f i n e S I G N A L _ P A T H _ R E S E T 0 x68
147 # d e f i n e M O T _ D E T E C T _ C T R L 0 x69
148 # d e f i n e U S E R _ C T R L 0 x6A // Bit 7 e n a b l e DMP , bit 3 r e s e t DMP
149 # d e f i n e P W R _ M G M T _ 1 0 x6B // D e v i c e d e f a u l t s to the S L E E P m o d e
150 # d e f i n e P W R _ M G M T _ 2 0 x6C
151 # d e f i n e D M P _ B A N K 0 x6D // A c t i v a t e s a s p e c i f i c b a n k in the DMP
152 # d e f i n e D M P _ R W _ P N T 0 x6E // Set r e a d / w r i t e p o i n t e r to a s p e c i f i c s t a r t a d d r e s s in s p e c i f i e d DMP b a n k
153 # d e f i n e D M P _ R E G 0 x6F // R e g i s t e r in DMP f r o m w h i c h to r e a d or to w h i c h to w r i t e
154 # d e f i n e D M P _ R E G _ 1 0 x70
155 # d e f i n e D M P _ R E G _ 2 0 x71
156 # d e f i n e F I F O _ C O U N T H 0 x72
157 # d e f i n e F I F O _ C O U N T L 0 x73
158 # d e f i n e F I F O _ R _ W 0 x74
159 # d e f i n e W H O _ A M _ I _ M P U 9 2 5 0 0 x75 // S h o u l d r e t u r n 0 x71
160 # d e f i n e X A _ O F F S E T _ H 0 x77
161 # d e f i n e X A _ O F F S E T _ L 0 x78
162 # d e f i n e Y A _ O F F S E T _ H 0 x7A
163 # d e f i n e Y A _ O F F S E T _ L 0 x7B
164 # d e f i n e Z A _ O F F S E T _ H 0 x7D
165 # d e f i n e Z A _ O F F S E T _ L 0 x7E
166
167 // U s i n g the MPU - 9 2 5 0 b r e a k o u t board , ADO is set to 0
168 // Seven - bit d e v i c e a d d r e s s is 1 1 0 1 0 0 for ADO = 0 and 1 1 0 1 0 1 for ADO = 1
169 # d e f i n e ADO 0
170 # if ADO
171 # d e f i n e M P U 9 2 5 0 _ A D D R E S S 0 x69 // D e v i c e a d d r e s s w h e n ADO = 1
172 # e l s e
173 # d e f i n e M P U 9 2 5 0 _ A D D R E S S 0 x68 // D e v i c e a d d r e s s w h e n ADO = 0
174 # d e f i n e A K 8 9 6 3 _ A D D R E S S 0 x0C // A d d r e s s of m a g n e t o m e t e r
175 # e n d i f // AD0
176
177 # d e f i n e R E A D _ F L A G 0 x80
178 # d e f i n e N O T _ S P I -1
179 # d e f i n e S P I _ D A T A _ R A T E 1 0 0 0 0 0 0 // 1 MHz is the max s p e e d of the MPU
180 // # d e f i n e S P I _ D A T A _ R A T E 1 0 0 0 0 0 0 // 1 MHz is the max s p e e d of the MPU - 9 2 5 0
181 # d e f i n e S P I _ M O D E S P I _ M O D E 3
182
183 # d e f i n e M P U 9 2 5 0 _ C A L _ S I Z E 68
184 # d e f i n e M P U 9 2 5 0 _ C A L _ E E A D D R 60
185
186 c l a s s M P U 9 2 5 0
187 {
188 p r o t e c t e d:
189 // Set i n i t i a l i n p u t p a r a m e t e r s
190 e n u m A s c a l e
191 {
192 A F S _ 2 G = 0 ,
193 AFS_4G ,
194 AFS_8G ,
195 A F S _ 1 6 G
196 };
197
198 e n u m G s c a l e {
199 G F S _ 2 5 0 D P S = 0 ,
200 G F S _ 5 0 0 D P S ,
201 G F S _ 1 0 0 0 D P S ,
202 G F S _ 2 0 0 0 D P S
203 };
204
205 e n u m M s c a l e {
206 M F S _ 1 4 B I T S = 0 , // 0 . 6 0 mG per LSB
207 M F S _ 1 6 B I T S // 0 . 1 5 mG per LSB
208
209 };
210
211 e n u m M _ M O D E {
212 M _ 8 H Z = 0 x02 , // 8 Hz u p d a t e
213 M _ 1 0 0 H Z = 0 x06 // 100 Hz c o n t i n u o u s m a g n e t o m e t e r
214 };
215
216 // T O D O : Add s e t t e r m e t h o d s for t h i s h a r d c o d e d s t u f f
217 // S p e c i f y s e n s o r f u l l s c a l e
218 u i n t 8 _ t G s c a l e = G F S _ 2 5 0 D P S ;
219 u i n t 8 _ t A s c a l e = A F S _ 2 G ;
220 // C h o o s e e i t h e r 14 - bit or 16 - bit m a g n e t o m e t e r r e s o l u t i o n
221 u i n t 8 _ t M s c a l e = M F S _ 1 6 B I T S ;
222
223 // 2 for 8 Hz , 6 for 100 Hz c o n t i n u o u s m a g n e t o m e t e r d a t a r e a d
224 u i n t 8 _ t M m o d e = M _ 1 0 0 H Z ;
225
226 // SPI c h i p s e l e c t pin
227 i n t 8 _ t _ c s P i n ;
228
229 u i n t 8 _ t w r i t e B y t e W i r e ( uint8_t , uint8_t , u i n t 8 _ t ) ;
230 u i n t 8 _ t w r i t e B y t e S P I ( uint8_t , u i n t 8 _ t ) ;
231 u i n t 8 _ t r e a d B y t e S P I ( u i n t 8 _ t s u b A d d r e s s ) ;
232 u i n t 8 _ t r e a d B y t e W i r e ( u i n t 8 _ t address , u i n t 8 _ t s u b A d d r e s s ) ;
233 b o o l m a g I n i t () ;
234 v o i d k i c k H a r d w a r e () ;
235 v o i d s e l e c t () ;
236 v o i d d e s e l e c t () ;
237 // T O D O : R e m o v e t h i s n e x t l i n e
238 p u b l i c:
239 u i n t 8 _ t a k 8 9 6 3 W h o A m I _ S P I () ;
240
241 p u b l i c:
242 f l o a t pitch , yaw , r o l l ;
243 f l o a t t e m p e r a t u r e ; // S t o r e s the r e a l i n t e r n a l c h i p t e m p e r a t u r e in C e l s i u s
244 i n t 1 6 _ t t e m p C o u n t ; // T e m p e r a t u r e raw c o u n t o u t p u t
245 u i n t 3 2 _ t d e l t _ t = 0; // U s e d to c o n t r o l d i s p l a y o u t p u t r a t e
246
247 u i n t 3 2 _ t c o u n t = 0 , s u m C o u n t = 0; // u s e d to c o n t r o l d i s p l a y o u t p u t r a t e
248 f l o a t d e l t a t = 0.0 f , sum = 0.0 f ; // i n t e g r a t i o n i n t e r v a l for b o t h f i l t e r s c h e m e s
249 u i n t 3 2 _ t l a s t U p d a t e = 0 , f i r s t U p d a t e = 0; // u s e d to c a l c u l a t e i n t e g r a t i o n i n t e r v a l
250 u i n t 3 2 _ t Now = 0; // u s e d to c a l c u l a t e i n t e g r a t i o n i n t e r v a l
251
252 i n t 1 6 _ t g y r o C o u n t [ 3 ] ; // S t o r e s the 16 - bit s i g n e d g y r o
253 i n t 1 6 _ t m a g C o u n t [ 3 ] ; // S t o r e s the 16 - bit s i g n e d m a g n e t o m e t e r s e n s o r o u t p u t
254 // S c a l e r e s o l u t i o n s per LSB for the s e n s o r s
255 f l o a t aRes , gRes , m R e s ;
256 // V a r i a b l e s to h o l d l a t e s t s e n s o r d a t a v a l u e s
257 // f l o a t ax , ay , az , gx , gy , gz , mx , my , mz , t e m p ;
258 i n t 1 6 _ t a c c e l _ t e m p _ r a w [ 4 ] ;
259 i n t 1 6 _ t m a g _ r a w [ 3 ] ;
260 i n t 1 6 _ t g y r o _ r a w [ 3 ] ;
261 u i n t 8 _ t n e w d a t a ;
262 f l o a t cal [ 1 6 ] ;
263
264 // F a c t o r y mag c a l i b r a t i o n and mag b i a s
265 f l o a t f a c t o r y M a g C a l i b r a t i o n [3] = {0 , 0 , 0} , f a c t o r y M a g B i a s [3]
= {0 , 0 , 0};
266 // B i a s c o r r e c t i o n s for gyro , a c c e l e r o m e t e r , and m a g n e t o m e t e r
267 f l o a t g y r o B i a s [3] = {0 , 0 , 0} ,
268 a c c e l B i a s [3] = {0 , 0 , 0} ,
269 m a g B i a s [3] = {0 , 0 , 0} ,
270 m a g S c a l e [3] = {0 , 0 , 0};
271 f l o a t s e l f T e s t [ 6 ] ;
272 // S t o r e s the 16 - bit s i g n e d a c c e l e r o m e t e r s e n s o r o u t p u t
273 i n t 1 6 _ t a c c e l C o u n t [ 3 ] ;
274
275 // P u b l i c m e t h o d d e c l a r a t i o n s
276 M P U 9 2 5 0 ( i n t 8 _ t c s P i n = N O T _ S P I ) ;
277 v o i d g e t M r e s () ;
278 v o i d g e t G r e s () ;
279 v o i d g e t A r e s () ;
280 int r e a d A c c e l D a t a ( i n t 1 6 _ t *) ;
281 v o i d r e a d M o t i o n S e n s o r ( i n t 1 6 _ t * ax , i n t 1 6 _ t * ay , i n t 1 6 _ t * az , i n t 1 6 _ t * gx , i n t 1 6 _ t * gy , i n t 1 6 _ t * gz , i n t 1 6 _ t * mx , i n t 1 6 _ t
* my , i n t 1 6 _ t * mz ) ;
282 v o i d u p d a t e () ;
283 b o o l w r i t e C a l i b r a t i o n (c o n s t v o i d *) ;
284 v o i d g e t C a l i b r a t i o n (f l o a t * , f l o a t * , f l o a t *) ;
285 b o o l a v a i l a b l e () ;
286 int r e a d G y r o D a t a ( i n t 1 6 _ t *) ;
287 int r e a d M a g D a t a ( i n t 1 6 _ t *) ;
288 v o i d r e a d T e m p D a t a ( i n t 1 6 _ t *) ;
289 v o i d u p d a t e T i m e () ;
290 v o i d i n i t A K 8 9 6 3 (f l o a t *) ;
291 v o i d i n i t M P U 9 2 5 0 () ;
292 v o i d c a l i b r a t e M P U 9 2 5 0 (f l o a t * g y r o B i a s , f l o a t * a c c e l B i a s ) ;
293 v o i d M P U 9 2 5 0 S e l f T e s t (f l o a t * d e s t i n a t i o n ) ;
294 v o i d m a g C a l M P U 9 2 5 0 (f l o a t * dest1 , f l o a t * d e s t 2 ) ;
295 u i n t 8 _ t w r i t e B y t e ( uint8_t , uint8_t , u i n t 8 _ t ) ;
296 u i n t 8 _ t r e a d B y t e ( uint8_t , u i n t 8 _ t ) ;
297 u i n t 8 _ t r e a d B y t e s ( uint8_t , uint8_t , uint8_t , u i n t 8 _ t *) ;
298 // T O D O : m a k e SPI / W i r e p r i v a t e
299 u i n t 8 _ t r e a d B y t e s S P I ( uint8_t , uint8_t , u i n t 8 _ t *) ;
300 u i n t 8 _ t r e a d B y t e s W i r e ( uint8_t , uint8_t , uint8_t , u i n t 8 _ t *) ;
301 b o o l i s I n I 2 c M o d e () { r e t u r n _ c s P i n == -1; }
302 b o o l b e g i n () ;
303 }; // c l a s s M P U 9 2 5 0
304
305 # e n d i f // _ M P U 9 2 5 0 _ H _
A.3.2 MPU9250.cpp
1 # i n c l u d e " M P U 9 2 5 0 _ t . h "
2
3 // = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = =
4 // Set of u s e f u l f u n c t i o n to a c c e s s a c c e l e r a t i o n , g y r o s c o p e , m a g n e t o m e t e r , and t e m p e r a t u r e d a t a
5 // = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = =
6
7 M P U 9 2 5 0 :: M P U 9 2 5 0 ( i n t 8 _ t c s p i n /* = N O T _ S P I */)
8 // U s e s I2C c o m m u n i c a t i o n by d e f a u l t
9 {
10 /* Use h a r d w a r e SPI c o m m u n i c a t i o n
11 If u s e d w i t h s p a r k f u n b r e a k o u t b o a r d h t t p s :// www . s p a r k f u n . com / p r o d u c t s / 1 3 7 6 2 , c h a n g e the pre - s o l d e r e d JP2 to e n a b l e SPI ( s o l d e r m i d d l e and l e f t i n s t e a d of m i d d l e and r i g h t ) p a d s are v e r y s m a l l and re - s o l d e r i n g can be v e r y t r i c k y . I2C h i g h l y r e c o m m e n d e d . */
12 if (( c s p i n > N O T _ S P I ) && ( c s p i n < N U M _ D I G I T A L _ P I N S ) ) {
13 _ c s P i n = c s p i n ;
14 SPI . b e g i n () ;
15 p i n M o d e ( _csPin , O U T P U T ) ;
16 d e s e l e c t () ;
17 } e l s e {
18 _ c s P i n = N O T _ S P I ;
19 W i r e . b e g i n () ;
20 W i r e . s e t C l o c k ( 4 0 0 0 0 0 ) ;
21 }
22 }
23
24 v o i d M P U 9 2 5 0 :: r e a d M o t i o n S e n s o r ( i n t 1 6 _ t * ax , i n t 1 6 _ t * ay , i n t 1 6 _ t
* az , i n t 1 6 _ t * gx , i n t 1 6 _ t * gy , i n t 1 6 _ t * gz , i n t 1 6 _ t * mx , i n t 1 6 _ t * my , i n t 1 6 _ t * mz ) {
25 if ( r e a d B y t e ( M P U 9 2 5 0 _ A D D R E S S , I N T _ S T A T U S ) & 0 x01 ) u p d a t e () ;
26 * ax = a c c e l _ t e m p _ r a w [ 0 ] ;
27 * ay = a c c e l _ t e m p _ r a w [ 1 ] ;
28 * az = a c c e l _ t e m p _ r a w [ 2 ] ;
29 * mx = m a g _ r a w [ 0 ] ;
30 * my = m a g _ r a w [ 1 ] ;
31 * mz = m a g _ r a w [ 2 ] ;
32 * gx = g y r o _ r a w [ 0 ] ;
33 * gy = g y r o _ r a w [ 1 ] ;
34 * gz = g y r o _ r a w [ 2 ] ;
35 }
36
37 b o o l M P U 9 2 5 0 :: a v a i l a b l e () {
38 u p d a t e () ;
39 if ( r e a d B y t e ( M P U 9 2 5 0 _ A D D R E S S , I N T _ S T A T U S ) & 0 x01 ) r e t u r n t r u e;
40 r e t u r n f a l s e;
41 }
42
43 v o i d M P U 9 2 5 0 :: g e t M r e s () {
44 s w i t c h ( M s c a l e ) {
45 /* P o s s i b l e m a g n e t o m e t e r s c a l e s ( and t h e i r r e g i s t e r bit
s e t t i n g s ) are : 14 bit r e s o l u t i o n (0) and 16 bit r e s o l u t i o n (1) */
46 c a s e M F S _ 1 4 B I T S :
47 m R e s = 1 0 . 0 f * 4 9 1 2 . 0 f / 8 1 9 0 . 0 f ; // P r o p e r s c a l e to r e t u r n m i l l i G a u s s
48 b r e a k;
49 c a s e M F S _ 1 6 B I T S :
50 m R e s = ( 1 0 . 0 f * 4 9 1 2 . 0 f / 3 2 7 6 0 . 0 f ) / 1 . 5 ; // P r o p e r s c a l e to r e t u r n m i l l i G a u s s
51 b r e a k;
52 }
53 }
54
55 v o i d M P U 9 2 5 0 :: g e t G r e s () {
56 s w i t c h ( G s c a l e ) {
57 /* P o s s i b l e g y r o s c a l e s ( and t h e i r r e g i s t e r bit s e t t i n g s ) are : 250 DPS ( 0 0 ) , 500 DPS ( 0 1 ) , 1 0 0 0 DPS ( 1 0 ) , and 2 0 0 0 DPS ( 1 1 ) . H e r e ’ s a bit of an a l g o r i t h to c a l c u l a t e DPS /( ADC t i c k ) b a s e d on t h a t 2 - bit v a l u e : */
58 c a s e G F S _ 2 5 0 D P S :
59 g R e s = 2 5 0 . 0 f / 3 2 7 6 8 . 0 f ;
60 b r e a k;
61 c a s e G F S _ 5 0 0 D P S :
62 g R e s = 5 0 0 . 0 f / 3 2 7 6 8 . 0 f ;
63 b r e a k;
65 g R e s = 1 0 0 0 . 0 f / 3 2 7 6 8 . 0 f ;
66 b r e a k;
67 c a s e G F S _ 2 0 0 0 D P S :
68 g R e s = 2 0 0 0 . 0 f / 3 2 7 6 8 . 0 f ;
69 b r e a k;
70 }
71 }
72
73 v o i d M P U 9 2 5 0 :: g e t A r e s () {
74 s w i t c h ( A s c a l e ) {
75 /* P o s s i b l e a c c e l e r o m e t e r s c a l e s ( and t h e i r r e g i s t e r bit
s e t t i n g s ) are : 2 Gs ( 0 0 ) , 4 Gs ( 0 1 ) , 8 Gs ( 1 0 ) , and 16 Gs ( 1 1 ) . H e r e ’ s a bit of an a l g o r i t h to c a l c u l a t e DPS /( ADC t i c k ) b a s e d on t h a t 2 - bit v a l u e : */
76 c a s e A F S _ 2 G :
77 a R e s = 2.0 f / 3 2 7 6 8 . 0 f ; b r e a k;
78 c a s e A F S _ 4 G :
79 a R e s = 4.0 f / 3 2 7 6 8 . 0 f ; b r e a k;
80 c a s e A F S _ 8 G :
81 a R e s = 8.0 f / 3 2 7 6 8 . 0 f ; b r e a k;
82 c a s e A F S _ 1 6 G :
83 a R e s = 1 6 . 0 f / 3 2 7 6 8 . 0 f ; b r e a k;
84 }
85 }
86
87 int M P U 9 2 5 0 :: r e a d A c c e l D a t a ( i n t 1 6 _ t * d e s t i n a t i o n ) {
88 u i n t 8 _ t r a w D a t a [ 6 ] ; // x / y / z a c c e l r e g i s t e r d a t a s t o r e d h e r e
89 // R e a d the six raw d a t a r e g i s t e r s i n t o d a t a a r r a y
90 r e a d B y t e s ( M P U 9 2 5 0 _ A D D R E S S , A C C E L _ X O U T _ H , 6 , & r a w D a t a [ 0 ] ) ;
91
92 // T u r n the MSB and LSB i n t o a s i g n e d 16 - bit v a l u e
93 d e s t i n a t i o n [0] = (( i n t 1 6 _ t ) r a w D a t a [0] < < 8) | r a w D a t a [1] ; //
a c c e l x
94 d e s t i n a t i o n [1] = (( i n t 1 6 _ t ) r a w D a t a [2] < < 8) | r a w D a t a [3] ; //
a c c e l y
95 d e s t i n a t i o n [2] = (( i n t 1 6 _ t ) r a w D a t a [4] < < 8) | r a w D a t a [5] ; //
a c c e l z
96 // d e s t i n a t i o n [3] = (( i n t 1 6 _ t ) r a w D a t a [6] < < 8) | r a w D a t a [7] ; //
t e m p e r a t u r e
97
98 r e t u r n 1;
99 }
100 101
102 int M P U 9 2 5 0 :: r e a d G y r o D a t a ( i n t 1 6 _ t * d e s t i n a t i o n ) {
103 u i n t 8 _ t r a w D a t a [ 6 ] ; // x / y / z g y r o r e g i s t e r d a t a s t o r e d h e r e
104 // R e a d the six raw d a t a r e g i s t e r s s e q u e n t i a l l y i n t o d a t a a r r a y
105 r e a d B y t e s ( M P U 9 2 5 0 _ A D D R E S S , G Y R O _ X O U T _ H , 6 , & r a w D a t a [ 0 ] ) ;
106
107 // T u r n the MSB and LSB i n t o a s i g n e d 16 - bit v a l u e
108 d e s t i n a t i o n [0] = (( i n t 1 6 _ t ) r a w D a t a [0] < < 8) | r a w D a t a [1] ;
109 d e s t i n a t i o n [1] = (( i n t 1 6 _ t ) r a w D a t a [2] < < 8) | r a w D a t a [3] ;
110 d e s t i n a t i o n [2] = (( i n t 1 6 _ t ) r a w D a t a [4] < < 8) | r a w D a t a [5] ;
111
112 r e t u r n 1;
113 }
114
115 int M P U 9 2 5 0 :: r e a d M a g D a t a ( i n t 1 6 _ t * d e s t i n a t i o n ) {
116 // x / y / z g y r o r e g i s t e r data , ST2 r e g i s t e r s t o r e d here , m u s t r e a d ST2 at end
117 // of d a t a a c q u i s i t i o n
118 u i n t 8 _ t r a w D a t a [ 7 ] ;
119 // W a i t for m a g n e t o m e t e r d a t a r e a d y bit to be set
120 if (1/* r e a d B y t e ( A K 8 9 6 3 _ A D D R E S S , A K 8 9 6 3 _ S T 1 ) & 0 x01 */) {
121 // R e a d the six raw d a t a and ST2 r e g i s t e r s s e q u e n t i a l l y i n t o d a t a a r r a y
122 r e a d B y t e s ( A K 8 9 6 3 _ A D D R E S S , A K 8 9 6 3 _ X O U T _ L , 7 , & r a w D a t a [ 0 ] ) ;
123 u i n t 8 _ t c = r a w D a t a [ 6 ] ; // End d a t a r e a d by r e a d i n g ST2 r e g i s t e r
124 // C h e c k if m a g n e t i c s e n s o r o v e r f l o w set , if not t h e n r e p o r t d a t a
125 if (!( c & 0 x08 ) ) {
126 // T u r n the MSB and LSB i n t o a s i g n e d 16 - bit v a l u e
127 d e s t i n a t i o n [0] = (( i n t 1 6 _ t ) r a w D a t a [1] < < 8) | r a w D a t a [ 0 ] ;
128 // D a t a s t o r e d as l i t t l e E n d i a n
129 d e s t i n a t i o n [1] = (( i n t 1 6 _ t ) r a w D a t a [3] < < 8) | r a w D a t a [ 2 ] ;
130 d e s t i n a t i o n [2] = (( i n t 1 6 _ t ) r a w D a t a [5] < < 8) | r a w D a t a [ 4 ] ;
131
132 r e t u r n 1;
134 }
135 r e t u r n 0;
136 }
137
138 v o i d M P U 9 2 5 0 :: r e a d T e m p D a t a ( i n t 1 6 _ t * d e s t i n a t i o n ) {
139 u i n t 8 _ t r a w D a t a [ 2 ] ; // x / y / z g y r o r e g i s t e r d a t a s t o r e d h e r e
140 // R e a d the two raw d a t a r e g i s t e r s s e q u e n t i a l l y i n t o d a t a a r r a y
141 r e a d B y t e s ( M P U 9 2 5 0 _ A D D R E S S , T E M P _ O U T _ H , 2 , & r a w D a t a [ 0 ] ) ;
142 // T u r n the MSB and LSB i n t o a 16 - bit v a l u e
143 d e s t i n a t i o n = (( i n t 1 6 _ t ) r a w D a t a [0] < < 8) | r a w D a t a [ 1 ] ;
144 }
145
146 // C a l c u l a t e the t i m e the l a s t u p d a t e t o o k for use in the q u a t e r n i o n f i l t e r s
147 // T O D O : T h i s d o e s n ’ t r e a l l y b e l o n g in t h i s c l a s s .
148 v o i d M P U 9 2 5 0 :: u p d a t e T i m e () {
149 Now = m i c r o s () ;
150
151 // Set i n t e g r a t i o n t i m e by t i m e e l a p s e d s i n c e l a s t f i l t e r u p d a t e
152 d e l t a t = (( Now - l a s t U p d a t e ) / 1 0 0 0 0 0 0 . 0 f ) ;
153 l a s t U p d a t e = Now ;
154
155 sum += d e l t a t ; // sum for a v e r a g i n g f i l t e r u p d a t e r a t e
156 s u m C o u n t ++;
157 }
158
159 v o i d M P U 9 2 5 0 :: i n i t A K 8 9 6 3 (f l o a t * d e s t i n a t i o n ) {
160 // F i r s t e x t r a c t the f a c t o r y c a l i b r a t i o n for e a c h m a g n e t o m e t e r a x i s
161 u i n t 8 _ t r a w D a t a [ 3 ] ; // x / y / z g y r o c a l i b r a t i o n d a t a s t o r e d h e r e
162 // T O D O : T e s t t h i s !! L i k e l y d o e s n ’ t w o r k
163 w r i t e B y t e ( A K 8 9 6 3 _ A D D R E S S , A K 8 9 6 3 _ C N T L , 0 x00 ) ; // P o w e r d o w n m a g n e t o m e t e r
164 d e l a y ( 1 0 ) ;
165 w r i t e B y t e ( A K 8 9 6 3 _ A D D R E S S , A K 8 9 6 3 _ C N T L , 0 x0F ) ; // E n t e r F u s e ROM a c c e s s m o d e
166 d e l a y ( 1 0 ) ;
167
168 // R e a d the x - , y - , and z - a x i s c a l i b r a t i o n v a l u e s
169 r e a d B y t e s ( A K 8 9 6 3 _ A D D R E S S , A K 8 9 6 3 _ A S A X , 3 , & r a w D a t a [ 0 ] ) ;
170
171 // R e t u r n x - a x i s s e n s i t i v i t y a d j u s t m e n t values , etc .
172 d e s t i n a t i o n [0] = (f l o a t) ( r a w D a t a [0] - 1 2 8 ) / 2 5 6 . + 1.;
173 d e s t i n a t i o n [1] = (f l o a t) ( r a w D a t a [1] - 1 2 8 ) / 2 5 6 . + 1.;
174 d e s t i n a t i o n [2] = (f l o a t) ( r a w D a t a [2] - 1 2 8 ) / 2 5 6 . + 1.;
175 w r i t e B y t e ( A K 8 9 6 3 _ A D D R E S S , A K 8 9 6 3 _ C N T L , 0 x00 ) ; // P o w e r d o w n m a g n e t o m e t e r
176 d e l a y ( 1 0 ) ;
177
178 // C o n f i g u r e the m a g n e t o m e t e r for c o n t i n u o u s r e a d and h i g h e s t r e s o l u t i o n .
179 // Set M s c a l e bit 4 to 1 (0) to e n a b l e 16 ( 1 4 ) bit r e s o l u t i o n in C N T L
180 // r e g i s t e r , and e n a b l e c o n t i n u o u s m o d e d a t a a c q u i s i t i o n M m o d e ( b i t s [ 3 : 0 ] ) ,
181 // 0 0 1 0 for 8 Hz and 0 1 1 0 for 100 Hz s a m p l e r a t e s .
182
183 // Set m a g n e t o m e t e r d a t a r e s o l u t i o n and s a m p l e ODR
184 w r i t e B y t e ( A K 8 9 6 3 _ A D D R E S S , A K 8 9 6 3 _ C N T L , M s c a l e < < 4 | M m o d e ) ;
185 d e l a y ( 1 0 ) ;
186 }
187
188 v o i d M P U 9 2 5 0 :: i n i t M P U 9 2 5 0 () {
189 // w a k e up d e v i c e
190 // C l e a r s l e e p m o d e bit (6) , e n a b l e all s e n s o r s
191 w r i t e B y t e ( M P U 9 2 5 0 _ A D D R E S S , P W R _ M G M T _ 1 , 0 x00 ) ;
192 d e l a y ( 1 0 0 ) ; // W a i t for all r e g i s t e r s to r e s e t
193
194 // Get s t a b l e t i m e s o u r c e
195 // A u t o s e l e c t c l o c k s o u r c e to be PLL g y r o s c o p e r e f e r e n c e if r e a d y e l s e
196 w r i t e B y t e ( M P U 9 2 5 0 _ A D D R E S S , P W R _ M G M T _ 1 , 0 x01 ) ;
197 d e l a y ( 2 0 0 ) ;
198
199 // C o n f i g u r e G y r o and T h e r m o m e t e r
200 // D i s a b l e F S Y N C and set t h e r m o m e t e r and g y r o b a n d w i d t h to 41 and 42 Hz ,
201 // r e s p e c t i v e l y ;
202 // m i n i m u m d e l a y t i m e for t h i s s e t t i n g is 5.9 ms , w h i c h m e a n s s e n s o r f u s i o n
203 // u p d a t e r a t e s c a n n o t be h i g h e r t h a n 1 / 0 . 0 0 5 9 = 170 Hz
204 // D L P F _ C F G = b i t s 2:0 = 0 1 1 ; t h i s l i m i t s the s a m p l e r a t e to 1 0 0 0 Hz for b o t h
205 // W i t h the MPU9250 , it is p o s s i b l e to get g y r o s a m p l e r a t e s of 32 kHz (!) ,
206 // 8 kHz , or 1 kHz
207 w r i t e B y t e ( M P U 9 2 5 0 _ A D D R E S S , CONFIG , 0 x03 ) ;
208
209 // Set s a m p l e r a t e = g y r o s c o p e o u t p u t r a t e /(1 + S M P L R T _ D I V )
210 // Use a 200 Hz r a t e ; a r a t e c o n s i s t e n t w i t h the f i l t e r u p d a t e r a t e
211 // d e t e r m i n e d i n s e t in C O N F I G a b o v e .
212 w r i t e B y t e ( M P U 9 2 5 0 _ A D D R E S S , S M P L R T _ D I V , 0 x04 ) ;
213
214 // Set g y r o s c o p e f u l l s c a l e r a n g e
215 // R a n g e s e l e c t s F S _ S E L and A F S _ S E L are 0 - 3 , so 2 - bit v a l u e s are
216 // left - s h i f t e d i n t o p o s i t i o n s 4:3
217
218 // get c u r r e n t G Y R O _ C O N F I G r e g i s t e r v a l u e
219 u i n t 8 _ t c = r e a d B y t e ( M P U 9 2 5 0 _ A D D R E S S , G Y R O _ C O N F I G ) ;
220 // c = c & ~0 xE0 ; // C l e a r self - t e s t b i t s [ 7 : 5 ]
221 c = c & ~0 x02 ; // C l e a r F c h o i c e b i t s [ 1 : 0 ]
222 c = c & ~0 x18 ; // C l e a r AFS b i t s [ 4 : 3 ]
223 c = c | G s c a l e < < 3; // Set f u l l s c a l e r a n g e for the g y r o
224 // Set F c h o i c e for the g y r o to 11 by w r i t i n g its i n v e r s e to b i t s 1:0 of
225 // G Y R O _ C O N F I G
226 // c =| 0 x00 ;
227 // W r i t e new G Y R O _ C O N F I G v a l u e to r e g i s t e r
228 w r i t e B y t e ( M P U 9 2 5 0 _ A D D R E S S , G Y R O _ C O N F I G , c ) ;
229
230 // Set a c c e l e r o m e t e r full - s c a l e r a n g e c o n f i g u r a t i o n
231 // Get c u r r e n t A C C E L _ C O N F I G r e g i s t e r v a l u e
232 c = r e a d B y t e ( M P U 9 2 5 0 _ A D D R E S S , A C C E L _ C O N F I G ) ;
233 // c = c & ~0 xE0 ; // C l e a r self - t e s t b i t s [ 7 : 5 ]
234 c = c & ~0 x18 ; // C l e a r AFS b i t s [ 4 : 3 ]
235 c = c | A s c a l e < < 3; // Set f u l l s c a l e r a n g e for the a c c e l e r o m e t e r
236 // W r i t e new A C C E L _ C O N F I G r e g i s t e r v a l u e
237 w r i t e B y t e ( M P U 9 2 5 0 _ A D D R E S S , A C C E L _ C O N F I G , c ) ;
238
239 // Set a c c e l e r o m e t e r s a m p l e r a t e c o n f i g u r a t i o n
240 // It is p o s s i b l e to get a 4 kHz s a m p l e r a t e f r o m the a c c e l e r o m e t e r by
241 // c h o o s i n g 1 for a c c e l _ f c h o i c e _ b bit [ 3 ] ; in t h i s c a s e the b a n d w i d t h is
242 // 1 . 1 3 kHz
243 // Get c u r r e n t A C C E L _ C O N F I G 2 r e g i s t e r v a l u e
244 c = r e a d B y t e ( M P U 9 2 5 0 _ A D D R E S S , A C C E L _ C O N F I G 2 ) ;
245 c = c & ~0 x0F ; // C l e a r a c c e l _ f c h o i c e _ b ( bit 3) and A _ D L P F G ( b i t s [ 2 : 0 ] )
246 c = c | 0 x03 ; // Set a c c e l e r o m e t e r r a t e to 1 kHz and b a n d w i d t h to 41 Hz
247 // W r i t e new A C C E L _ C O N F I G 2 r e g i s t e r v a l u e
248 w r i t e B y t e ( M P U 9 2 5 0 _ A D D R E S S , A C C E L _ C O N F I G 2 , c ) ;
249 // The a c c e l e r o m e t e r , gyro , and t h e r m o m e t e r are set to 1 kHz s a m p l e rates ,
250 // but all t h e s e r a t e s are f u r t h e r r e d u c e d by a f a c t o r of 5 to 200 Hz b e c a u s e
251 // of the S M P L R T _ D I V s e t t i n g
252
253 // C o n f i g u r e I n t e r r u p t s and B y p a s s E n a b l e
254 // Set i n t e r r u p t pin a c t i v e high , push - pull , h o l d i n t e r r u p t pin l e v e l H I G H
255 // u n t i l i n t e r r u p t cleared , c l e a r on r e a d of I N T _ S T A T U S , and e n a b l e
256 // I 2 C _ B Y P A S S _ E N so a d d i t i o n a l c h i p s can j o i n the I2C bus and all can be
257 // c o n t r o l l e d by the A r d u i n o as m a s t e r .
258 w r i t e B y t e ( M P U 9 2 5 0 _ A D D R E S S , I N T _ P I N _ C F G , 0 x22 ) ;
259 // E n a b l e d a t a r e a d y ( bit 0) i n t e r r u p t
260 w r i t e B y t e ( M P U 9 2 5 0 _ A D D R E S S , I N T _ E N A B L E , 0 x01 ) ;
261 d e l a y ( 1 0 0 ) ;
262 }
263
264 // F u n c t i o n w h i c h a c c u m u l a t e s g y r o and a c c e l e r o m e t e r d a t a a f t e r d e v i c e
265 // i n i t i a l i z a t i o n . It c a l c u l a t e s the a v e r a g e of the at - r e s t r e a d i n g s and t h e n
266 // l o a d s the r e s u l t i n g o f f s e t s i n t o a c c e l e r o m e t e r and g y r o b i a s r e g i s t e r s .
267 v o i d M P U 9 2 5 0 :: c a l i b r a t e M P U 9 2 5 0 (f l o a t * g y r o B i a s , f l o a t * a c c e l B i a s ) {
268 u i n t 8 _ t d a t a [ 1 2 ] ; // d a t a a r r a y to h o l d a c c e l e r o m e t e r and g y r o x , y , z , d a t a
269 u i n t 1 6 _ t ii , p a c k e t _ c o u n t , f i f o _ c o u n t ;
270 i n t 3 2 _ t g y r o _ b i a s [3] = {0 , 0 , 0} , a c c e l _ b i a s [3] = {0 , 0 , 0};
271
272 // r e s e t d e v i c e
273 // W r i t e a one to bit 7 r e s e t bit ; t o g g l e r e s e t d e v i c e
274 w r i t e B y t e ( M P U 9 2 5 0 _ A D D R E S S , P W R _ M G M T _ 1 , R E A D _ F L A G ) ;
275 d e l a y ( 1 0 0 ) ;
276
277 // get s t a b l e t i m e s o u r c e ; A u t o s e l e c t c l o c k s o u r c e to be PLL g y r o s c o p e
278 // r e f e r e n c e if r e a d y e l s e use the i n t e r n a l o s c i l l a t o r , b i t s 2:0 = 001
279 w r i t e B y t e ( M P U 9 2 5 0 _ A D D R E S S , P W R _ M G M T _ 1 , 0 x01 ) ;
280 w r i t e B y t e ( M P U 9 2 5 0 _ A D D R E S S , P W R _ M G M T _ 2 , 0 x00 ) ;
281 d e l a y ( 2 0 0 ) ;
282
283 // C o n f i g u r e d e v i c e for b i a s c a l c u l a t i o n
284 // D i s a b l e all i n t e r r u p t s
285 w r i t e B y t e ( M P U 9 2 5 0 _ A D D R E S S , I N T _ E N A B L E , 0 x00 ) ;
286 // D i s a b l e F I F O
287 w r i t e B y t e ( M P U 9 2 5 0 _ A D D R E S S , FIFO_EN , 0 x00 ) ;
288 // T u r n on i n t e r n a l c l o c k s o u r c e
289 w r i t e B y t e ( M P U 9 2 5 0 _ A D D R E S S , P W R _ M G M T _ 1 , 0 x00 ) ;
290 // D i s a b l e I2C m a s t e r
291 w r i t e B y t e ( M P U 9 2 5 0 _ A D D R E S S , I 2 C _ M S T _ C T R L , 0 x00 ) ;
292 // D i s a b l e F I F O and I2C m a s t e r m o d e s
293 w r i t e B y t e ( M P U 9 2 5 0 _ A D D R E S S , U S E R _ C T R L , 0 x00 ) ;
294 // R e s e t F I F O and DMP
295 w r i t e B y t e ( M P U 9 2 5 0 _ A D D R E S S , U S E R _ C T R L , 0 x0C ) ;
296 d e l a y ( 1 5 ) ;
297
298 // C o n f i g u r e M P U 6 0 5 0 g y r o and a c c e l e r o m e t e r for b i a s c a l c u l a t i o n
299 // Set low - p a s s f i l t e r to 188 Hz
300 w r i t e B y t e ( M P U 9 2 5 0 _ A D D R E S S , CONFIG , 0 x01 ) ;
301 // Set s a m p l e r a t e to 1 kHz
302 w r i t e B y t e ( M P U 9 2 5 0 _ A D D R E S S , S M P L R T _ D I V , 0 x00 ) ;
303 // Set g y r o full - s c a l e to 250 d e g r e e s per second , m a x i m u m s e n s i t i v i t y
304 w r i t e B y t e ( M P U 9 2 5 0 _ A D D R E S S , G Y R O _ C O N F I G , 0 x00 ) ;
305 // Set a c c e l e r o m e t e r full - s c a l e to 2 g , m a x i m u m s e n s i t i v i t y
306 w r i t e B y t e ( M P U 9 2 5 0 _ A D D R E S S , A C C E L _ C O N F I G , 0 x00 ) ;
307
308 u i n t 1 6 _ t g y r o s e n s i t i v i t y = 1 3 1 ; // = 131 LSB / d e g r e e s / sec
309 u i n t 1 6 _ t a c c e l s e n s i t i v i t y = 1 6 3 8 4 ; // = 1 6 3 8 4 LSB / g
310
311 // C o n f i g u r e F I F O to c a p t u r e a c c e l e r o m e t e r and g y r o d a t a for b i a s c a l c u l a t i o n
312 w r i t e B y t e ( M P U 9 2 5 0 _ A D D R E S S , U S E R _ C T R L , 0 x40 ) ; // E n a b l e F I F O
313 // E n a b l e g y r o and a c c e l e r o m e t e r s e n s o r s for F I F O ( max s i z e 512 b y t e s in
314 // MPU - 9 1 5 0 )
315 w r i t e B y t e ( M P U 9 2 5 0 _ A D D R E S S , FIFO_EN , 0 x78 ) ;
316 d e l a y ( 4 0 ) ; // a c c u m u l a t e 40 s a m p l e s in 40 m i l l i s e c o n d s = 480 b y t e s
317
318 // At end of s a m p l e a c c u m u l a t i o n , t u r n off F I F O s e n s o r r e a d
319 // D i s a b l e g y r o and a c c e l e r o m e t e r s e n s o r s for F I F O
320 w r i t e B y t e ( M P U 9 2 5 0 _ A D D R E S S , FIFO_EN , 0 x00 ) ;
321 // R e a d F I F O s a m p l e c o u n t
322 r e a d B y t e s ( M P U 9 2 5 0 _ A D D R E S S , F I F O _ C O U N T H , 2 , & d a t a [ 0 ] ) ;
323 f i f o _ c o u n t = (( u i n t 1 6 _ t ) d a t a [0] < < 8) | d a t a [ 1 ] ;
324 // How m a n y s e t s of f u l l g y r o and a c c e l e r o m e t e r d a t a for a v e r a g i n g
325 p a c k e t _ c o u n t = f i f o _ c o u n t / 12;
326
327 for ( ii = 0; ii < p a c k e t _ c o u n t ; ii ++)
328 {
329 i n t 1 6 _ t a c c e l _ t e m p [3] = {0 , 0 , 0} , g y r o _ t e m p [3] = {0 , 0 , 0};
331 r e a d B y t e s ( M P U 9 2 5 0 _ A D D R E S S , F I F O _ R _ W , 12 , & d a t a [ 0 ] ) ;
332 // F o r m s i g n e d 16 - bit i n t e g e r for e a c h s a m p l e in F I F O
333 a c c e l _ t e m p [0] = ( i n t 1 6 _ t ) ((( i n t 1 6 _ t ) d a t a [0] < < 8) | d a t a [1]
) ;
334 a c c e l _ t e m p [1] = ( i n t 1 6 _ t ) ((( i n t 1 6 _ t ) d a t a [2] < < 8) | d a t a [3]
) ;
335 a c c e l _ t e m p [2] = ( i n t 1 6 _ t ) ((( i n t 1 6 _ t ) d a t a [4] < < 8) | d a t a [5]
) ;
336 g y r o _ t e m p [0] = ( i n t 1 6 _ t ) ((( i n t 1 6 _ t ) d a t a [6] < < 8) | d a t a [7]
) ;
337 g y r o _ t e m p [1] = ( i n t 1 6 _ t ) ((( i n t 1 6 _ t ) d a t a [8] < < 8) | d a t a [9]
) ;
338 g y r o _ t e m p [2] = ( i n t 1 6 _ t ) ((( i n t 1 6 _ t ) d a t a [ 1 0 ] < < 8) | d a t a [ 1 1 ] ) ;
339
340 // Sum i n d i v i d u a l s i g n e d 16 - bit b i a s e s to get a c c u m u l a t e d s i g n e d 32 - bit
341 // b i a s e s .
342 a c c e l _ b i a s [0] += ( i n t 3 2 _ t ) a c c e l _ t e m p [ 0 ] ;
343 a c c e l _ b i a s [1] += ( i n t 3 2 _ t ) a c c e l _ t e m p [ 1 ] ;
344 a c c e l _ b i a s [2] += ( i n t 3 2 _ t ) a c c e l _ t e m p [ 2 ] ;
345 g y r o _ b i a s [0] += ( i n t 3 2 _ t ) g y r o _ t e m p [ 0 ] ;
346 g y r o _ b i a s [1] += ( i n t 3 2 _ t ) g y r o _ t e m p [ 1 ] ;
347 g y r o _ b i a s [2] += ( i n t 3 2 _ t ) g y r o _ t e m p [ 2 ] ;
348 }
349 // Sum i n d i v i d u a l s i g n e d 16 - bit b i a s e s to get a c c u m u l a t e d s i g n e d 32 - bit b i a s e s
350 a c c e l _ b i a s [0] /= ( i n t 3 2 _ t ) p a c k e t _ c o u n t ;
351 a c c e l _ b i a s [1] /= ( i n t 3 2 _ t ) p a c k e t _ c o u n t ;
352 a c c e l _ b i a s [2] /= ( i n t 3 2 _ t ) p a c k e t _ c o u n t ;
353 g y r o _ b i a s [0] /= ( i n t 3 2 _ t ) p a c k e t _ c o u n t ;
354 g y r o _ b i a s [1] /= ( i n t 3 2 _ t ) p a c k e t _ c o u n t ;
355 g y r o _ b i a s [2] /= ( i n t 3 2 _ t ) p a c k e t _ c o u n t ;
356
357 // Sum i n d i v i d u a l s i g n e d 16 - bit b i a s e s to get a c c u m u l a t e d s i g n e d 32 - bit b i a s e s
358 if ( a c c e l _ b i a s [2] > 0 L ) {
359 a c c e l _ b i a s [2] -= ( i n t 3 2 _ t ) a c c e l s e n s i t i v i t y ;
360 } e l s e {
361 a c c e l _ b i a s [2] += ( i n t 3 2 _ t ) a c c e l s e n s i t i v i t y ;
362 }
363
364 // C o n s t r u c t the g y r o b i a s e s for p u s h to the h a r d w a r e g y r o b i a s r e g i s t e r s ,
365 // w h i c h are r e s e t to z e r o u p o n d e v i c e s t a r t u p .
366 // D i v i d e by 4 to get 3 2 . 9 LSB per deg / s to c o n f o r m to e x p e c t e d b i a s i n p u t
367 // f o r m a t .
368 d a t a [0] = ( - g y r o _ b i a s [0] / 4 > > 8) & 0 xFF ;
369 // B i a s e s are a d d i t i v e , so c h a n g e s i g n on c a l c u l a t e d a v e r a g e g y r o b i a s e s
370 d a t a [1] = ( - g y r o _ b i a s [0] / 4) & 0 xFF ;
371 d a t a [2] = ( - g y r o _ b i a s [1] / 4 > > 8) & 0 xFF ;
372 d a t a [3] = ( - g y r o _ b i a s [1] / 4) & 0 xFF ;
373 d a t a [4] = ( - g y r o _ b i a s [2] / 4 > > 8) & 0 xFF ;
374 d a t a [5] = ( - g y r o _ b i a s [2] / 4) & 0 xFF ;
375
376 // P u s h g y r o b i a s e s to h a r d w a r e r e g i s t e r s
377 w r i t e B y t e ( M P U 9 2 5 0 _ A D D R E S S , X G _ O F F S E T _ H , d a t a [ 0 ] ) ;
378 w r i t e B y t e ( M P U 9 2 5 0 _ A D D R E S S , X G _ O F F S E T _ L , d a t a [ 1 ] ) ;
379 w r i t e B y t e ( M P U 9 2 5 0 _ A D D R E S S , Y G _ O F F S E T _ H , d a t a [ 2 ] ) ;
380 w r i t e B y t e ( M P U 9 2 5 0 _ A D D R E S S , Y G _ O F F S E T _ L , d a t a [ 3 ] ) ;
381 w r i t e B y t e ( M P U 9 2 5 0 _ A D D R E S S , Z G _ O F F S E T _ H , d a t a [ 4 ] ) ;
382 w r i t e B y t e ( M P U 9 2 5 0 _ A D D R E S S , Z G _ O F F S E T _ L , d a t a [ 5 ] ) ;
383
384 // O u t p u t s c a l e d g y r o b i a s e s for d i s p l a y in the m a i n p r o g r a m
385 g y r o B i a s [0] = (f l o a t) g y r o _ b i a s [0] / (f l o a t) g y r o s e n s i t i v i t y ;
386 g y r o B i a s [1] = (f l o a t) g y r o _ b i a s [1] / (f l o a t) g y r o s e n s i t i v i t y ;
387 g y r o B i a s [2] = (f l o a t) g y r o _ b i a s [2] / (f l o a t) g y r o s e n s i t i v i t y ;
388
389 // C o n s t r u c t the a c c e l e r o m e t e r b i a s e s for p u s h to the h a r d w a r e a c c e l e r o m e t e r
390 // b i a s r e g i s t e r s . T h e s e r e g i s t e r s c o n t a i n f a c t o r y t r i m v a l u e s w h i c h m u s t be
391 // a d d e d to the c a l c u l a t e d a c c e l e r o m e t e r b i a s e s ; on b o o t up t h e s e r e g i s t e r s
392 // w i l l h o l d non - z e r o v a l u e s . In a d d i t i o n , bit 0 of the l o w e r b y t e m u s t be
393 // p r e s e r v e d s i n c e it is u s e d for t e m p e r a t u r e c o m p e n s a t i o n
394 // A c c e l e r o m e t e r b i a s r e g i s t e r s e x p e c t b i a s i n p u t as 2 0 4 8 LSB per g , so t h a t
395 // the a c c e l e r o m e t e r b i a s e s c a l c u l a t e d a b o v e m u s t be d i v i d e d by 8.
396
397 // A p l a c e to h o l d the f a c t o r y a c c e l e r o m e t e r t r i m b i a s e s
398 i n t 3 2 _ t a c c e l _ b i a s _ r e g [3] = {0 , 0 , 0};
399 // R e a d f a c t o r y a c c e l e r o m e t e r t r i m v a l u e s
400 r e a d B y t e s ( M P U 9 2 5 0 _ A D D R E S S , X A _ O F F S E T _ H , 2 , & d a t a [ 0 ] ) ;
401 a c c e l _ b i a s _ r e g [0] = ( i n t 3 2 _ t ) ((( i n t 1 6 _ t ) d a t a [0] < < 8) | d a t a [ 1 ] ) ;
402 r e a d B y t e s ( M P U 9 2 5 0 _ A D D R E S S , Y A _ O F F S E T _ H , 2 , & d a t a [ 0 ] ) ;
403 a c c e l _ b i a s _ r e g [1] = ( i n t 3 2 _ t ) ((( i n t 1 6 _ t ) d a t a [0] < < 8) | d a t a [ 1 ] ) ;
404 r e a d B y t e s ( M P U 9 2 5 0 _ A D D R E S S , Z A _ O F F S E T _ H , 2 , & d a t a [ 0 ] ) ;
405 a c c e l _ b i a s _ r e g [2] = ( i n t 3 2 _ t ) ((( i n t 1 6 _ t ) d a t a [0] < < 8) | d a t a [ 1 ] ) ;
406
407 // D e f i n e m a s k for t e m p e r a t u r e c o m p e n s a t i o n bit 0 of l o w e r b y t e of
408 // a c c e l e r o m e t e r b i a s r e g i s t e r s
409 u i n t 3 2 _ t m a s k = 1 uL ;
410 // D e f i n e a r r a y to h o l d m a s k bit for e a c h a c c e l e r o m e t e r b i a s a x i s
411 u i n t 8 _ t m a s k _ b i t [3] = {0 , 0 , 0};
412
413 for ( ii = 0; ii < 3; ii ++) {
414 // If t e m p e r a t u r e c o m p e n s a t i o n bit is set , r e c o r d t h a t f a c t in m a s k _ b i t
415 if (( a c c e l _ b i a s _ r e g [ ii ] & m a s k ) ) {
416 m a s k _ b i t [ ii ] = 0 x01 ;
417 }
418 }
419
420 // C o n s t r u c t t o t a l a c c e l e r o m e t e r bias , i n c l u d i n g c a l c u l a t e d a v e r a g e
421 // a c c e l e r o m e t e r b i a s f r o m a b o v e
422 // S u b t r a c t c a l c u l a t e d a v e r a g e d a c c e l e r o m e t e r b i a s s c a l e d to 2 0 4 8 LSB / g
423 // (16 g f u l l s c a l e )
424 a c c e l _ b i a s _ r e g [0] -= ( a c c e l _ b i a s [0] / 8) ;
425 a c c e l _ b i a s _ r e g [1] -= ( a c c e l _ b i a s [1] / 8) ;
426 a c c e l _ b i a s _ r e g [2] -= ( a c c e l _ b i a s [2] / 8) ;
427
428 d a t a [0] = ( a c c e l _ b i a s _ r e g [0] > > 8) & 0 xFF ;
429 d a t a [1] = ( a c c e l _ b i a s _ r e g [ 0 ] ) & 0 xFF ;
430 // p r e s e r v e t e m p e r a t u r e c o m p e n s a t i o n bit w h e n w r i t i n g b a c k to a c c e l e r o m e t e r
431 // b i a s r e g i s t e r s
432 d a t a [1] = d a t a [1] | m a s k _ b i t [ 0 ] ;
433 d a t a [2] = ( a c c e l _ b i a s _ r e g [1] > > 8) & 0 xFF ;
434 d a t a [3] = ( a c c e l _ b i a s _ r e g [ 1 ] ) & 0 xFF ;
435 // P r e s e r v e t e m p e r a t u r e c o m p e n s a t i o n bit w h e n w r i t i n g b a c k to a c c e l e r o m e t e r
436 // b i a s r e g i s t e r s
437 d a t a [3] = d a t a [3] | m a s k _ b i t [ 1 ] ;
438 d a t a [4] = ( a c c e l _ b i a s _ r e g [2] > > 8) & 0 xFF ;
439 d a t a [5] = ( a c c e l _ b i a s _ r e g [ 2 ] ) & 0 xFF ;
440 // P r e s e r v e t e m p e r a t u r e c o m p e n s a t i o n bit w h e n w r i t i n g b a c k to a c c e l e r o m e t e r
441 // b i a s r e g i s t e r s
442 d a t a [5] = d a t a [5] | m a s k _ b i t [ 2 ] ;
443
444 // A p p a r e n t l y t h i s is not w o r k i n g for the a c c e l e r a t i o n b i a s e s in the MPU - 9 2 5 0
445 // Are we h a n d l i n g the t e m p e r a t u r e c o r r e c t i o n bit p r o p e r l y ?
446 // P u s h a c c e l e r o m e t e r b i a s e s to h a r d w a r e r e g i s t e r s
447 w r i t e B y t e ( M P U 9 2 5 0 _ A D D R E S S , X A _ O F F S E T _ H , d a t a [ 0 ] ) ;
448 w r i t e B y t e ( M P U 9 2 5 0 _ A D D R E S S , X A _ O F F S E T _ L , d a t a [ 1 ] ) ;
449 w r i t e B y t e ( M P U 9 2 5 0 _ A D D R E S S , Y A _ O F F S E T _ H , d a t a [ 2 ] ) ;
450 w r i t e B y t e ( M P U 9 2 5 0 _ A D D R E S S , Y A _ O F F S E T _ L , d a t a [ 3 ] ) ;
451 w r i t e B y t e ( M P U 9 2 5 0 _ A D D R E S S , Z A _ O F F S E T _ H , d a t a [ 4 ] ) ;
452 w r i t e B y t e ( M P U 9 2 5 0 _ A D D R E S S , Z A _ O F F S E T _ L , d a t a [ 5 ] ) ;
453
454 // O u t p u t s c a l e d a c c e l e r o m e t e r b i a s e s for d i s p l a y in the m a i n p r o g r a m
455 a c c e l B i a s [0] = (f l o a t) a c c e l _ b i a s [0] / (f l o a t) a c c e l s e n s i t i v i t y ;
456 a c c e l B i a s [1] = (f l o a t) a c c e l _ b i a s [1] / (f l o a t) a c c e l s e n s i t i v i t y ;
457 a c c e l B i a s [2] = (f l o a t) a c c e l _ b i a s [2] / (f l o a t) a c c e l s e n s i t i v i t y ;
459 460
461 // A c c e l e r o m e t e r and g y r o s c o p e s e l f t e s t ; c h e c k c a l i b r a t i o n wrt f a c t o r y s e t t i n g s
462 // S h o u l d r e t u r n p e r c e n t d e v i a t i o n f r o m f a c t o r y t r i m values , +/ -14 or l e s s
463 // d e v i a t i o n is a p a s s .
464 v o i d M P U 9 2 5 0 :: M P U 9 2 5 0 S e l f T e s t (f l o a t * d e s t i n a t i o n ) {
465 u i n t 8 _ t r a w D a t a [6] = {0 , 0 , 0 , 0 , 0 , 0};
466 u i n t 8 _ t s e l f T e s t [ 6 ] ;
467 i n t 3 2 _ t g A v g [3] = {0} , a A v g [3] = {0} , a S T A v g [3] = {0} , g S T A v g [3] = { 0 } ;
468 f l o a t f a c t o r y T r i m [ 6 ] ;
469 u i n t 8 _ t FS = 0;
470
471 // Set g y r o s a m p l e r a t e to 1 kHz
472 w r i t e B y t e ( M P U 9 2 5 0 _ A D D R E S S , S M P L R T _ D I V , 0 x00 ) ;
473 // Set g y r o s a m p l e r a t e to 1 kHz and D L P F to 92 Hz
474 w r i t e B y t e ( M P U 9 2 5 0 _ A D D R E S S , CONFIG , 0 x02 ) ;
475 // Set f u l l s c a l e r a n g e for the g y r o to 250 dps
476 w r i t e B y t e ( M P U 9 2 5 0 _ A D D R E S S , G Y R O _ C O N F I G , 1 < < FS ) ;
477 // Set a c c e l e r o m e t e r r a t e to 1 kHz and b a n d w i d t h to 92 Hz
478 w r i t e B y t e ( M P U 9 2 5 0 _ A D D R E S S , A C C E L _ C O N F I G 2 , 0 x02 ) ;
479 // Set f u l l s c a l e r a n g e for the a c c e l e r o m e t e r to 2 g
480 w r i t e B y t e ( M P U 9 2 5 0 _ A D D R E S S , A C C E L _ C O N F I G , 1 < < FS ) ;
481
482 // Get a v e r a g e c u r r e n t v a l u e s of g y r o and a c c l e r o m e t e r
483 for (int ii = 0; ii < 2 0 0 ; ii ++) {
484 // S e r i a l . p r i n t (" BHW :: ii = ") ;
485 // S e r i a l . p r i n t l n ( ii ) ;
486 // R e a d the six raw d a t a r e g i s t e r s i n t o d a t a a r r a y
487 r e a d B y t e s ( M P U 9 2 5 0 _ A D D R E S S , A C C E L _ X O U T _ H , 6 , & r a w D a t a [ 0 ] ) ;
488 // T u r n the MSB and LSB i n t o a s i g n e d 16 - bit v a l u e
489 a A v g [0] += ( i n t 1 6 _ t ) ((( i n t 1 6 _ t ) r a w D a t a [0] < < 8) | r a w D a t a [ 1 ] )
;
490 a A v g [1] += ( i n t 1 6 _ t ) ((( i n t 1 6 _ t ) r a w D a t a [2] < < 8) | r a w D a t a [ 3 ] )
;
491 a A v g [2] += ( i n t 1 6 _ t ) ((( i n t 1 6 _ t ) r a w D a t a [4] < < 8) | r a w D a t a [ 5 ] )
;
492
493 // R e a d the six raw d a t a r e g i s t e r s s e q u e n t i a l l y i n t o d a t a a r r a y
494 r e a d B y t e s ( M P U 9 2 5 0 _ A D D R E S S , G Y R O _ X O U T _ H , 6 , & r a w D a t a [ 0 ] ) ;
495 // T u r n the MSB and LSB i n t o a s i g n e d 16 - bit v a l u e
496 g A v g [0] += ( i n t 1 6 _ t ) ((( i n t 1 6 _ t ) r a w D a t a [0] < < 8) | r a w D a t a [ 1 ] )
;
497 g A v g [1] += ( i n t 1 6 _ t ) ((( i n t 1 6 _ t ) r a w D a t a [2] < < 8) | r a w D a t a [ 3 ] )
;
498 g A v g [2] += ( i n t 1 6 _ t ) ((( i n t 1 6 _ t ) r a w D a t a [4] < < 8) | r a w D a t a [ 5 ] )
;
499 }
500
501 // Get a v e r a g e of 200 v a l u e s and s t o r e as a v e r a g e c u r r e n t r e a d i n g s
502 for (int ii = 0; ii < 3; ii ++) {
503 a A v g [ ii ] /= 2 0 0 ;
504 g A v g [ ii ] /= 2 0 0 ;
505 }
506
507 // C o n f i g u r e the a c c e l e r o m e t e r for self - t e s t
508 // E n a b l e s e l f t e s t on all t h r e e a x e s and set a c c e l e r o m e t e r r a n g e to +/ - 2 g
509 w r i t e B y t e ( M P U 9 2 5 0 _ A D D R E S S , A C C E L _ C O N F I G , 0 xE0 ) ;
510 // E n a b l e s e l f t e s t on all t h r e e a x e s and set g y r o r a n g e to +/ -250 d e g r e e s / s
511 w r i t e B y t e ( M P U 9 2 5 0 _ A D D R E S S , G Y R O _ C O N F I G , 0 xE0 ) ;
512 d e l a y ( 2 5 ) ; // D e l a y a w h i l e to let the d e v i c e s t a b i l i z e
513
514 // Get a v e r a g e self - t e s t v a l u e s of g y r o and a c c l e r o m e t e r
515 for (int ii = 0; ii < 2 0 0 ; ii ++) {
516 // R e a d the six raw d a t a r e g i s t e r s i n t o d a t a a r r a y
517 r e a d B y t e s ( M P U 9 2 5 0 _ A D D R E S S , A C C E L _ X O U T _ H , 6 , & r a w D a t a [ 0 ] ) ;
518 // T u r n the MSB and LSB i n t o a s i g n e d 16 - bit v a l u e
519 a S T A v g [0] += ( i n t 1 6 _ t ) ((( i n t 1 6 _ t ) r a w D a t a [0] < < 8) | r a w D a t a [ 1 ] ) ;
520 a S T A v g [1] += ( i n t 1 6 _ t ) ((( i n t 1 6 _ t ) r a w D a t a [2] < < 8) | r a w D a t a [ 3 ] ) ;
521 a S T A v g [2] += ( i n t 1 6 _ t ) ((( i n t 1 6 _ t ) r a w D a t a [4] < < 8) | r a w D a t a [ 5 ] ) ;
523 // R e a d the six raw d a t a r e g i s t e r s s e q u e n t i a l l y i n t o d a t a a r r a y
524 r e a d B y t e s ( M P U 9 2 5 0 _ A D D R E S S , G Y R O _ X O U T _ H , 6 , & r a w D a t a [ 0 ] ) ;
525 // T u r n the MSB and LSB i n t o a s i g n e d 16 - bit v a l u e
526 g S T A v g [0] += ( i n t 1 6 _ t ) ((( i n t 1 6 _ t ) r a w D a t a [0] < < 8) | r a w D a t a [ 1 ] ) ;
527 g S T A v g [1] += ( i n t 1 6 _ t ) ((( i n t 1 6 _ t ) r a w D a t a [2] < < 8) | r a w D a t a [ 3 ] ) ;
528 g S T A v g [2] += ( i n t 1 6 _ t ) ((( i n t 1 6 _ t ) r a w D a t a [4] < < 8) | r a w D a t a [ 5 ] ) ;
529 }
530
531 // Get a v e r a g e of 200 v a l u e s and s t o r e as a v e r a g e self - t e s t r e a d i n g s
532 for (int ii = 0; ii < 3; ii ++) {
533 a S T A v g [ ii ] /= 2 0 0 ;
534 g S T A v g [ ii ] /= 2 0 0 ;
535 }
536
537 // C o n f i g u r e the g y r o and a c c e l e r o m e t e r for n o r m a l o p e r a t i o n
538 w r i t e B y t e ( M P U 9 2 5 0 _ A D D R E S S , A C C E L _ C O N F I G , 0 x00 ) ;
539 w r i t e B y t e ( M P U 9 2 5 0 _ A D D R E S S , G Y R O _ C O N F I G , 0 x00 ) ;
540 d e l a y ( 2 5 ) ; // D e l a y a w h i l e to let the d e v i c e s t a b i l i z e
541
542 // R e t r i e v e a c c e l e r o m e t e r and g y r o f a c t o r y Self - T e s t C o d e f r o m U S R _ R e g
543 // X - a x i s a c c e l self - t e s t r e s u l t s
544 s e l f T e s t [0] = r e a d B y t e ( M P U 9 2 5 0 _ A D D R E S S , S E L F _ T E S T _ X _ A C C E L ) ;
545 // Y - a x i s a c c e l self - t e s t r e s u l t s
546 s e l f T e s t [1] = r e a d B y t e ( M P U 9 2 5 0 _ A D D R E S S , S E L F _ T E S T _ Y _ A C C E L ) ;
547 // Z - a x i s a c c e l self - t e s t r e s u l t s
548 s e l f T e s t [2] = r e a d B y t e ( M P U 9 2 5 0 _ A D D R E S S , S E L F _ T E S T _ Z _ A C C E L ) ;
549 // X - a x i s g y r o self - t e s t r e s u l t s
550 s e l f T e s t [3] = r e a d B y t e ( M P U 9 2 5 0 _ A D D R E S S , S E L F _ T E S T _ X _ G Y R O ) ;
551 // Y - a x i s g y r o self - t e s t r e s u l t s
552 s e l f T e s t [4] = r e a d B y t e ( M P U 9 2 5 0 _ A D D R E S S , S E L F _ T E S T _ Y _ G Y R O ) ;
553 // Z - a x i s g y r o self - t e s t r e s u l t s
554 s e l f T e s t [5] = r e a d B y t e ( M P U 9 2 5 0 _ A D D R E S S , S E L F _ T E S T _ Z _ G Y R O ) ;
555
556 // R e t r i e v e f a c t o r y self - t e s t v a l u e f r o m self - t e s t c o d e r e a d s
557 // FT [ Xa ] f a c t o r y t r i m c a l c u l a t i o n
558 f a c t o r y T r i m [0] = (f l o a t) ( 2 6 2 0 / 1 < < FS ) * ( pow ( 1 . 0 1 , ((f l o a t) s e l f T e s t [0] - 1 . 0 ) ) ) ;
559 // FT [ Ya ] f a c t o r y t r i m c a l c u l a t i o n
560 f a c t o r y T r i m [1] = (f l o a t) ( 2 6 2 0 / 1 < < FS ) * ( pow ( 1 . 0 1 , ((f l o a t) s e l f T e s t [1] - 1 . 0 ) ) ) ;
561 // FT [ Za ] f a c t o r y t r i m c a l c u l a t i o n
562 f a c t o r y T r i m [2] = (f l o a t) ( 2 6 2 0 / 1 < < FS ) * ( pow ( 1 . 0 1 , ((f l o a t) s e l f T e s t [2] - 1 . 0 ) ) ) ;
563 // FT [ Xg ] f a c t o r y t r i m c a l c u l a t i o n
564 f a c t o r y T r i m [3] = (f l o a t) ( 2 6 2 0 / 1 < < FS ) * ( pow ( 1 . 0 1 , ((f l o a t) s e l f T e s t [3] - 1 . 0 ) ) ) ;
565 // FT [ Yg ] f a c t o r y t r i m c a l c u l a t i o n
566 f a c t o r y T r i m [4] = (f l o a t) ( 2 6 2 0 / 1 < < FS ) * ( pow ( 1 . 0 1 , ((f l o a t) s e l f T e s t [4] - 1 . 0 ) ) ) ;
567 // FT [ Zg ] f a c t o r y t r i m c a l c u l a t i o n
568 f a c t o r y T r i m [5] = (f l o a t) ( 2 6 2 0 / 1 < < FS ) * ( pow ( 1 . 0 1 , ((f l o a t) s e l f T e s t [5] - 1 . 0 ) ) ) ;
569
570 // R e p o r t r e s u l t s as a r a t i o of ( STR - FT ) / FT ; the c h a n g e f r o m F a c t o r y T r i m
571 // of the Self - T e s t R e s p o n s e
572 // To get percent , m u s t m u l t i p l y by 100
573 for (int i = 0; i < 3; i ++)
574 {
575 // R e p o r t p e r c e n t d i f f e r e n c e s
576 d e s t i n a t i o n [ i ] = 1 0 0 . 0 * ((f l o a t) ( a S T A v g [ i ] - a A v g [ i ]) ) / f a c t o r y T r i m [ i ]
577 - 1 0 0 . ;
578 // R e p o r t p e r c e n t d i f f e r e n c e s
579 d e s t i n a t i o n [ i + 3] = 1 0 0 . 0 * ((f l o a t) ( g S T A v g [ i ] - g A v g [ i ]) ) / f a c t o r y T r i m [ i + 3]
580 - 1 0 0 . ;
581 }
582 }
583
584 // F u n c t i o n w h i c h a c c u m u l a t e s m a g n e t o m e t e r d a t a a f t e r d e v i c e i n i t i a l i z a t i o n .
585 // It c a l c u l a t e s the b i a s and s c a l e in the x , y , and z a x e s .
586 v o i d M P U 9 2 5 0 :: m a g C a l M P U 9 2 5 0 (f l o a t * b i a s _ d e s t , f l o a t * s c a l e _ d e s t ) {
587 u i n t 1 6 _ t ii = 0 , s a m p l e _ c o u n t = 0;
588 i n t 3 2 _ t m a g _ b i a s [3] = {0 , 0 , 0} ,
589 m a g _ s c a l e [3] = {0 , 0 , 0};
590 i n t 1 6 _ t m a g _ m a x [3] = {0 x8000 , 0 x8000 , 0 x 8 0 0 0 } ,
591 m a g _ m i n [3] = {0 x7FFF , 0 x7FFF , 0 x 7 F F F } ,
592 m a g _ t e m p [3] = {0 , 0 , 0};
593
594 // M a k e s u r e r e s o l u t i o n has b e e n c a l c u l a t e d
595 g e t M r e s () ;
596
597 // S e r i a l . p r i n t l n ( F (" Mag C a l i b r a t i o n : W a v e d e v i c e in a f i g u r e 8 u n t i l d o n e !") ) ;
598 // S e r i a l . p r i n t l n ( F (" 4 s e c o n d s to get r e a d y f o l l o w e d by 15 s e c o n d s of s a m p l i n g ) ") ) ;
599 d e l a y ( 4 0 0 0 ) ;
600
601 // s h o o t for ~ f i f t e e n s e c o n d s of mag d a t a
602 // at 8 Hz ODR , new mag d a t a is a v a i l a b l e e v e r y 125 ms
603 if ( M m o d e == M _ 8 H Z ) {
604 s a m p l e _ c o u n t = 1 2 8 ;
605 }
606 // at 100 Hz ODR , new mag d a t a is a v a i l a b l e e v e r y 10 ms
607 if ( M m o d e == M _ 1 0 0 H Z ) {
608 s a m p l e _ c o u n t = 1 5 0 0 ;
609 }
610
611 for ( ii = 0; ii < s a m p l e _ c o u n t ; ii ++) {
612 r e a d M a g D a t a ( m a g _ t e m p ) ; // R e a d the mag d a t a
613 for (int jj = 0; jj < 3; jj ++) {
614 if ( m a g _ t e m p [ jj ] > m a g _ m a x [ jj ]) {
615 m a g _ m a x [ jj ] = m a g _ t e m p [ jj ];
616 }
617 if ( m a g _ t e m p [ jj ] < m a g _ m i n [ jj ]) {
618 m a g _ m i n [ jj ] = m a g _ t e m p [ jj ];
619 }
620 }
621 if ( M m o d e == M _ 8 H Z ) {
622 d e l a y ( 1 3 5 ) ; // At 8 Hz ODR , new mag d a t a is a v a i l a b l e e v e r y 125 ms
623 }
624 if ( M m o d e == M _ 1 0 0 H Z ) {
625 d e l a y ( 1 2 ) ; // At 100 Hz ODR , new mag d a t a is a v a i l a b l e e v e r y 10 ms
626 }
627 }
628
629 // S e r i a l . p r i n t l n (" mag x min / max :") ; S e r i a l . p r i n t l n ( m a g _ m a x [ 0 ] )
; S e r i a l . p r i n t l n ( m a g _ m i n [ 0 ] ) ;
630 // S e r i a l . p r i n t l n (" mag y min / max :") ; S e r i a l . p r i n t l n ( m a g _ m a x [ 1 ] )
; S e r i a l . p r i n t l n ( m a g _ m i n [ 1 ] ) ;
631 // S e r i a l . p r i n t l n (" mag z min / max :") ; S e r i a l . p r i n t l n ( m a g _ m a x [ 2 ] )
; S e r i a l . p r i n t l n ( m a g _ m i n [ 2 ] ) ;
632
633 // Get h a r d i r o n c o r r e c t i o n
634 // Get ’ a v e r a g e ’ x mag b i a s in c o u n t s
635 m a g _ b i a s [0] = ( m a g _ m a x [0] + m a g _ m i n [ 0 ] ) / 2;
636 // Get ’ a v e r a g e ’ y mag b i a s in c o u n t s
637 m a g _ b i a s [1] = ( m a g _ m a x [1] + m a g _ m i n [ 1 ] ) / 2;
638 // Get ’ a v e r a g e ’ z mag b i a s in c o u n t s
639 m a g _ b i a s [2] = ( m a g _ m a x [2] + m a g _ m i n [ 2 ] ) / 2;
640
641 // S a v e mag b i a s e s in G for m a i n p r o g r a m
642 b i a s _ d e s t [0] = (f l o a t) m a g _ b i a s [0] * m R e s * f a c t o r y M a g C a l i b r a t i o n [ 0 ] ;
643 b i a s _ d e s t [1] = (f l o a t) m a g _ b i a s [1] * m R e s * f a c t o r y M a g C a l i b r a t i o n [ 1 ] ;
644 b i a s _ d e s t [2] = (f l o a t) m a g _ b i a s [2] * m R e s * f a c t o r y M a g C a l i b r a t i o n [ 2 ] ;
645
646 // Get s o f t i r o n c o r r e c t i o n e s t i m a t e
647 // Get a v e r a g e x a x i s max c h o r d l e n g t h in c o u n t s
648 m a g _ s c a l e [0] = ( m a g _ m a x [0] - m a g _ m i n [ 0 ] ) / 2;
649 // Get a v e r a g e y a x i s max c h o r d l e n g t h in c o u n t s
650 m a g _ s c a l e [1] = ( m a g _ m a x [1] - m a g _ m i n [ 1 ] ) / 2;
651 // Get a v e r a g e z a x i s max c h o r d l e n g t h in c o u n t s
652 m a g _ s c a l e [2] = ( m a g _ m a x [2] - m a g _ m i n [ 2 ] ) / 2;
654 f l o a t a v g _ r a d = m a g _ s c a l e [0] + m a g _ s c a l e [1] + m a g _ s c a l e [ 2 ] ;
655 a v g _ r a d /= 3 . 0 ;
656
657 s c a l e _ d e s t [0] = a v g _ r a d / ((f l o a t) m a g _ s c a l e [ 0 ] ) ;
658 s c a l e _ d e s t [1] = a v g _ r a d / ((f l o a t) m a g _ s c a l e [ 1 ] ) ;
659 s c a l e _ d e s t [2] = a v g _ r a d / ((f l o a t) m a g _ s c a l e [ 2 ] ) ;
660
661 // S e r i a l . p r i n t l n ( F (" Mag C a l i b r a t i o n d o n e !") ) ;
662 }
663 // W i r e . h r e a d and w r i t e p r o t o c o l s
664 u i n t 8 _ t M P U 9 2 5 0 :: w r i t e B y t e ( u i n t 8 _ t d e v i c e A d d r e s s , u i n t 8 _ t r e g i s t e r A d d r e s s , u i n t 8 _ t d a t a ) {
665 if ( _ c s P i n != N O T _ S P I ) {
666 r e t u r n w r i t e B y t e S P I ( r e g i s t e r A d d r e s s , d a t a ) ;
667 } e l s e {
668 r e t u r n w r i t e B y t e W i r e ( d e v i c e A d d r e s s , r e g i s t e r A d d r e s s , d a t a ) ;
669 }
670 }
671
672 u i n t 8 _ t M P U 9 2 5 0 :: w r i t e B y t e S P I ( u i n t 8 _ t r e g i s t e r A d d r e s s , u i n t 8 _ t w r i t e D a t a ) {
673 u i n t 8 _ t r e t u r n V a l ;
674
675 SPI . b e g i n T r a n s a c t i o n ( S P I S e t t i n g s ( S P I _ D A T A _ R A T E , M S B F I R S T , S P I _ M O D E ) ) ;
676 s e l e c t () ;
677
678 SPI . t r a n s f e r ( r e g i s t e r A d d r e s s ) ;
679 r e t u r n V a l = SPI . t r a n s f e r ( w r i t e D a t a ) ;
680
681 d e s e l e c t () ;
682 SPI . e n d T r a n s a c t i o n () ;
683 # i f d e f S E R I A L _ D E B U G
684 S e r i a l . p r i n t (" M P U 9 2 5 0 :: w r i t e B y t e S P I s l a v e r e t u r n e d : 0 x ") ;
685 S e r i a l . p r i n t l n ( r e t u r n V a l , HEX ) ;
686 # e n d i f
687 r e t u r n r e t u r n V a l ;
688 }
689
690 u i n t 8 _ t M P U 9 2 5 0 :: w r i t e B y t e W i r e ( u i n t 8 _ t d e v i c e A d d r e s s , u i n t 8 _ t r e g i s t e r A d d r e s s , u i n t 8 _ t d a t a ) {
691 W i r e . b e g i n T r a n s m i s s i o n ( d e v i c e A d d r e s s ) ; // I n i t i a l i z e the Tx b u f f e r
692 W i r e . w r i t e ( r e g i s t e r A d d r e s s ) ; // Put s l a v e r e g i s t e r a d d r e s s in Tx b u f f e r
693 W i r e . w r i t e ( d a t a ) ; // Put d a t a in Tx b u f f e r
694 W i r e . e n d T r a n s m i s s i o n () ; // S e n d the Tx b u f f e r
695 // T O D O : Fix t h i s to r e t u r n s o m e t h i n g m e a n i n g f u l
696 r e t u r n N U L L ;
697 }
698
699 // R e a d a b y t e f r o m g i v e n r e g i s t e r on d e v i c e . C a l l s n e c e s s a r y SPI or I2C
700 // i m p l e m e n t a t i o n . T h i s was c o n f i g u r e d in the c o n s t r u c t o r .
701 u i n t 8 _ t M P U 9 2 5 0 :: r e a d B y t e ( u i n t 8 _ t d e v i c e A d d r e s s , u i n t 8 _ t r e g i s t e r A d d r e s s ) {
702 if ( _ c s P i n != N O T _ S P I ) {
703 r e t u r n r e a d B y t e S P I ( r e g i s t e r A d d r e s s ) ;
704 } e l s e {
705 r e t u r n r e a d B y t e W i r e ( d e v i c e A d d r e s s , r e g i s t e r A d d r e s s ) ;
706 }
707 }
708
709 // R e a d a b y t e f r o m the g i v e n r e g i s t e r a d d r e s s f r o m d e v i c e u s i n g I2C
710 u i n t 8 _ t M P U 9 2 5 0 :: r e a d B y t e W i r e ( u i n t 8 _ t d e v i c e A d d r e s s , u i n t 8 _ t r e g i s t e r A d d r e s s ) {
711 u i n t 8 _ t d a t a ; // ‘ data ‘ w i l l s t o r e the r e g i s t e r d a t a
712
713 // I n i t i a l i z e the Tx b u f f e r
714 W i r e . b e g i n T r a n s m i s s i o n ( d e v i c e A d d r e s s ) ;
715 // Put s l a v e r e g i s t e r a d d r e s s in Tx b u f f e r
716 W i r e . w r i t e ( r e g i s t e r A d d r e s s ) ;
717 // S e n d the Tx buffer , but s e n d a r e s t a r t to k e e p c o n n e c t i o n a l i v e
718 W i r e . e n d T r a n s m i s s i o n (f a l s e) ;
719 // R e a d one b y t e f r o m s l a v e r e g i s t e r a d d r e s s
720 W i r e . r e q u e s t F r o m ( d e v i c e A d d r e s s , ( u i n t 8 _ t ) 1) ;
722 d a t a = W i r e . r e a d () ;
723 // R e t u r n d a t a r e a d f r o m s l a v e r e g i s t e r
724 r e t u r n d a t a ;
725 }
726
727 // R e a d a b y t e f r o m the g i v e n r e g i s t e r a d d r e s s u s i n g SPI
728 u i n t 8 _ t M P U 9 2 5 0 :: r e a d B y t e S P I ( u i n t 8 _ t r e g i s t e r A d d r e s s ) {
729 r e t u r n w r i t e B y t e S P I ( r e g i s t e r A d d r e s s | R E A D _ F L A G , 0 xFF /* 0 xFF is a r b i t r a r y */) ;
730 }
731
732 // R e a d 1 or m o r e b y t e s f r o m g i v e n r e g i s t e r and d e v i c e u s i n g I2C
733 u i n t 8 _ t M P U 9 2 5 0 :: r e a d B y t e s W i r e ( u i n t 8 _ t d e v i c e A d d r e s s , u i n t 8 _ t r e g i s t e r A d d r e s s , u i n t 8 _ t count , u i n t 8 _ t * d e s t ) {
734 // I n i t i a l i z e the Tx b u f f e r
735 W i r e . b e g i n T r a n s m i s s i o n ( d e v i c e A d d r e s s ) ;
736 // Put s l a v e r e g i s t e r a d d r e s s in Tx b u f f e r
737 W i r e . w r i t e ( r e g i s t e r A d d r e s s ) ;
738 // S e n d the Tx buffer , but s e n d a r e s t a r t to k e e p c o n n e c t i o n a l i v e
739 W i r e . e n d T r a n s m i s s i o n (f a l s e) ;
740
741 u i n t 8 _ t i = 0;
742 // R e a d b y t e s f r o m s l a v e r e g i s t e r a d d r e s s
743 W i r e . r e q u e s t F r o m ( d e v i c e A d d r e s s , c o u n t ) ;
744 w h i l e ( W i r e . a v a i l a b l e () ) {
745 // Put r e a d r e s u l t s in the Rx b u f f e r
746 d e s t [ i ++] = W i r e . r e a d () ;
747 }
748
749 r e t u r n i ; // R e t u r n n u m b e r of b y t e s w r i t t e n
750 }
751
752 // S e l e c t s l a v e IC by a s s e r t i n g CS pin
753 v o i d M P U 9 2 5 0 :: s e l e c t () {
754 d i g i t a l W r i t e ( _csPin , LOW ) ;
755 }
756
757 // S e l e c t s l a v e IC by d e a s s e r t i n g CS pin
758 v o i d M P U 9 2 5 0 :: d e s e l e c t () {
759 d i g i t a l W r i t e ( _csPin , H I G H ) ;
760 }
761
762 u i n t 8 _ t M P U 9 2 5 0 :: r e a d B y t e s S P I ( u i n t 8 _ t r e g i s t e r A d d r e s s , u i n t 8 _ t count , u i n t 8 _ t * d e s t ) {
763 SPI . b e g i n T r a n s a c t i o n ( S P I S e t t i n g s ( S P I _ D A T A _ R A T E , M S B F I R S T , S P I _ M O D E ) ) ;
764 s e l e c t () ;
765
766 SPI . t r a n s f e r ( r e g i s t e r A d d r e s s | R E A D _ F L A G ) ;
767
768 u i n t 8 _ t i ;
769
770 for ( i = 0; i < c o u n t ; i ++) {
771 d e s t [ i ] = SPI . t r a n s f e r (0 x00 ) ;
772 # i f d e f S E R I A L _ D E B U G
773 S e r i a l . p r i n t (" r e a d B y t e s S P I :: R e a d b y t e : 0 x ") ;
774 S e r i a l . p r i n t l n ( d e s t [ i ] , HEX ) ;
775 # e n d i f
776 }
777
778 SPI . e n d T r a n s a c t i o n () ;
779 d e s e l e c t () ;
780 d e l a y M i c r o s e c o n d s ( 5 0 ) ;
781 r e t u r n i ; // R e t u r n n u m b e r of b y t e s w r i t t e n
782 783
784 /* # i f d e f S E R I A L _ D E B U G
785 S e r i a l . p r i n t (" M P U 9 2 5 0 :: w r i t e B y t e S P I s l a v e r e t u r n e d : 0 x ") ;
786 S e r i a l . p r i n t l n ( r e t u r n V a l , HEX ) ;
787 # e n d i f
788 r e t u r n r e t u r n V a l ; */
789 /*
790 // Set s l a v e a d d r e s s of A K 8 9 6 3 and set A K 8 9 6 3 for r e a d
791 w r i t e B y t e S P I ( I 2 C _ S L V 0 _ A D D R , A K 8 9 6 3 _ A D D R E S S | R E A D _ F L A G ) ;
792
793 S e r i a l . p r i n t ("\ n B H W :: I 2 C _ S L V 0 _ A D D R set to : 0 x ") ;
794 S e r i a l . p r i n t l n ( r e a d B y t e ( M P U 9 2 5 0 _ A D D R E S S , I 2 C _ S L V 0 _ A D D R ) , HEX )
;
796 // Set a d d r e s s to s t a r t r e a d f r o m
797 w r i t e B y t e S P I ( I 2 C _ S L V 0 _ R E G , r e g i s t e r A d d r e s s ) ;
798 // R e a d b y t e s f r o m m a g n e t o m e t e r
799 //
800 S e r i a l . p r i n t ("\ n B H W :: I 2 C _ S L V 0 _ C T R L g e t s 0 x ") ;
801 S e r i a l . p r i n t l n ( R E A D _ F L A G | count , HEX ) ;
802
803 // R e a d c o u n t b y t e s f r o m r e g i s t e r A d d r e s s via I 2 C _ S L V 0
804 S e r i a l . p r i n t (" BHW :: r e a d B y t e s S P I : r e t u r n v a l u e t e s t : ") ;
805 S e r i a l . p r i n t l n ( w r i t e B y t e S P I ( I 2 C _ S L V 0 _ C T R L , R E A D _ F L A G | c o u n t ) ) ;
806 */
807 }
808
809 u i n t 8 _ t M P U 9 2 5 0 :: r e a d B y t e s ( u i n t 8 _ t d e v i c e A d d r e s s , u i n t 8 _ t r e g i s t e r A d d r e s s , u i n t 8 _ t count , u i n t 8 _ t * d e s t ) {
810 if ( _ c s P i n == N O T _ S P I ) // R e a d via I2C {
811 r e t u r n r e a d B y t e s W i r e ( d e v i c e A d d r e s s , r e g i s t e r A d d r e s s , count , d e s t ) ;
812 } e l s e // R e a d u s i n g SPI {
813 r e t u r n r e a d B y t e s S P I ( r e g i s t e r A d d r e s s , count , d e s t ) ;
814 }
815 }
816
817 b o o l M P U 9 2 5 0 :: m a g I n i t () {
818 // R e s e t r e g i s t e r s to d e f a u l t s , bit a u t o c l e a r s
819 w r i t e B y t e S P I (0 x6B , 0 x80 ) ;
820 // A u t o s e l e c t the b e s t a v a i l a b l e c l o c k s o u r c e
821 w r i t e B y t e S P I (0 x6B , 0 x01 ) ;
822 // E n a b l e X , Y , & Z a x e s of a c c e l and g y r o
823 w r i t e B y t e S P I (0 x6C , 0 x00 ) ;
824 // C o n f i g d i s a b l e F S Y N C pin , set g y r o / t e m p b a n d w i d t h to 1 8 4 / 1 8 8 Hz
825 w r i t e B y t e S P I (0 x1A , 0 x01 ) ;
826 // S e l f t e s t s off , g y r o set to +/ -2000 dps FS
827 w r i t e B y t e S P I (0 x1B , 0 x18 ) ;
828 // S e l f t e s t off , a c c e l set to +/ - 8 g FS
829 w r i t e B y t e S P I (0 x1C , 0 x08 ) ;
830 // B y p a s s D L P F and set a c c e l b a n d w i d t h to 184 Hz
831 w r i t e B y t e S P I (0 x1D , 0 x09 ) ;
832 // C o n f i g u r e INT pin ( a c t i v e h i g h / push - p u l l / l a t c h u n t i l r e a d )
833 w r i t e B y t e S P I (0 x37 , 0 x30 ) ;
834 // E n a b l e I2C m a s t e r m o d e
835 // T O D O Why not do t h i s 11 -100 ms a f t e r p o w e r up ?
836 w r i t e B y t e S P I (0 x6A , 0 x20 ) ;
837 // D i s a b l e multi - m a s t e r and set I2C m a s t e r c l o c k to 400 kHz
838 // h t t p s :// d e v e l o p e r . m b e d . org / u s e r s / k y l o n g m u / c o d e / M P U 9 2 5 0 _ S P I / c a l l s s a y s
839 // e n a b l e d multi - m a s t e r ... T O D O F i n d out why
840 w r i t e B y t e S P I (0 x24 , 0 x0D ) ;
841 // Set to w r i t e to s l a v e a d d r e s s 0 x0C
842 w r i t e B y t e S P I (0 x25 , 0 x0C ) ;
843 // P o i n t s a v e 0 r e g i s t e r at A K 8 9 6 3 ’ s c o n t r o l 2 ( s o f t r e s e t ) r e g i s t e r
844 w r i t e B y t e S P I (0 x26 , 0 x0B ) ;
845 // S e n d 0 x01 to A K 8 9 6 3 via s l a v e 0 to t r i g g e r a s o f t r e s t a r t
846 w r i t e B y t e S P I (0 x63 , 0 x01 ) ;
847 // E n a b l e s i m p l e 1 - b y t e I2C r e a d s f r o m s l a v e 0
848 w r i t e B y t e S P I (0 x27 , 0 x81 ) ;
849 // P o i n t s a v e 0 r e g i s t e r at A K 8 9 6 3 ’ s c o n t r o l 1 ( m o d e ) r e g i s t e r
850 w r i t e B y t e S P I (0 x26 , 0 x0A ) ;
851 // 16 - bit c o n t i n u o u s m e a s u r e m e n t m o d e 1
852 w r i t e B y t e S P I (0 x63 , 0 x12 ) ;
853 // E n a b l e s i m p l e 1 - b y t e I2C r e a d s f r o m s l a v e 0
854 w r i t e B y t e S P I (0 x27 , 0 x81 ) ;
855
856 // T O D O : R e m o v e t h i s c o d e
857 u i n t 8 _ t ret = a k 8 9 6 3 W h o A m I _ S P I () ;
858 # i f d e f S E R I A L _ D E B U G
859 S e r i a l . p r i n t (" M P U 9 2 5 0 :: m a g I n i t to r e t u r n ") ;
860 S e r i a l . p r i n t l n (( ret == 0 x48 ) ? " t r u e " : " f a l s e ") ;
861 # e n d i f
862 r e t u r n ret == 0 x48 ;
863 }
864
865 // W r i t e a n u l l b y t e w / o CS a s s e r t i o n to get SPI h a r d w a r e to i d l e h i g h ( m o d e 3)
866 v o i d M P U 9 2 5 0 :: k i c k H a r d w a r e () {
867 SPI . b e g i n T r a n s a c t i o n ( S P I S e t t i n g s ( S P I _ D A T A _ R A T E , M S B F I R S T , S P I _ M O D E ) ) ;
868 SPI . t r a n s f e r (0 x00 ) ; // S e n d n u l l b y t e
869 SPI . e n d T r a n s a c t i o n () ;
870 }
871
872 b o o l M P U 9 2 5 0 :: b e g i n () {
873 k i c k H a r d w a r e () ;
874 r e t u r n m a g I n i t () ;
875 }
876
877 // R e a d the W H O A M I ( WIA ) r e g i s t e r of the A K 8 9 6 3
878 // T O D O : T h i s m e t h o d has s i d e e f f e c t s
879 u i n t 8 _ t M P U 9 2 5 0 :: a k 8 9 6 3 W h o A m I _ S P I () {
880 u i n t 8 _ t r e s p o n s e , o l d S l a v e A d d r e s s , o l d S l a v e R e g i s t e r , o l d S l a v e C o n f i g ;
881 // S a v e s t a t e
882 o l d S l a v e A d d r e s s = r e a d B y t e S P I ( I 2 C _ S L V 0 _ A D D R ) ;
883 o l d S l a v e R e g i s t e r = r e a d B y t e S P I ( I 2 C _ S L V 0 _ R E G ) ;
884 o l d S l a v e C o n f i g = r e a d B y t e S P I ( I 2 C _ S L V 0 _ C T R L ) ;
885 # i f d e f S E R I A L _ D E B U G
886 S e r i a l . p r i n t (" Old s l a v e a d d r e s s : 0 x ") ;
887 S e r i a l . p r i n t l n ( o l d S l a v e A d d r e s s , HEX ) ;
888 S e r i a l . p r i n t (" Old s l a v e r e g i s t e r : 0 x ") ;
889 S e r i a l . p r i n t l n ( o l d S l a v e R e g i s t e r , HEX ) ;
890 S e r i a l . p r i n t (" Old s l a v e c o n f i g : 0 x ") ;
891 S e r i a l . p r i n t l n ( o l d S l a v e C o n f i g , HEX ) ;
892 # e n d i f
893
894 // Set the I2C s l a v e a d d r e s of A K 8 9 6 3 and set for r e a d
895 r e s p o n s e = w r i t e B y t e S P I ( I 2 C _ S L V 0 _ A D D R , A K 8 9 6 3 _ A D D R E S S | R E A D _ F L A G ) ;
896 // I2C s l a v e 0 r e g i s t e r a d d r e s s f r o m w h e r e to b e g i n d a t a t r a n s f e r
897 r e s p o n s e = w r i t e B y t e S P I ( I 2 C _ S L V 0 _ R E G , 0 x00 ) ;
898 // E n a b l e 1 - b y t e r e a d s on s l a v e 0
899 r e s p o n s e = w r i t e B y t e S P I ( I 2 C _ S L V 0 _ C T R L , 0 x81 ) ;
900 d e l a y M i c r o s e c o n d s (1) ;
901 // R e a d WIA r e g i s t e r
902 r e s p o n s e = w r i t e B y t e S P I ( W H O _ A M _ I _ A K 8 9 6 3 | R E A D _ F L A G , 0 x00 ) ;
903
904 // R e s t o r e s t a t e
905 w r i t e B y t e S P I ( I 2 C _ S L V 0 _ A D D R , o l d S l a v e A d d r e s s ) ;
906 w r i t e B y t e S P I ( I 2 C _ S L V 0 _ R E G , o l d S l a v e R e g i s t e r ) ;
907 w r i t e B y t e S P I ( I 2 C _ S L V 0 _ C T R L , o l d S l a v e C o n f i g ) ;
908
909 r e t u r n r e s p o n s e ;
910 }
911
912 b o o l M P U 9 2 5 0 :: w r i t e C a l i b r a t i o n (c o n s t v o i d * d a t a ) {
913 c o n s t u i n t 8 _ t * p = (c o n s t u i n t 8 _ t *) d a t a ;
914 u i n t 1 6 _ t crc ;
915 u i n t 8 _ t i ;
916
917 if ( p [0] != 117 || p [1] != 84) r e t u r n f a l s e;
918 crc = 0 x F F F F ;
919 for ( i = 0; i < M P U 9 2 5 0 _ C A L _ S I Z E ; i ++) {
920 crc = _ c r c 1 6 _ u p d a t e ( crc , p [ i ]) ;
921 }
922 if ( crc != 0) r e t u r n f a l s e;
923 for ( i = 0; i < M P U 9 2 5 0 _ C A L _ S I Z E ; i ++) {
924 E E P R O M . w r i t e ( M P U 9 2 5 0 _ C A L _ E E A D D R + i , p [ i ]) ;
925 }
926 for ( i = 0; i < M P U 9 2 5 0 _ C A L _ S I Z E ; i ++) {
927 if ( E E P R O M . r e a d ( M P U 9 2 5 0 _ C A L _ E E A D D R + i ) != p [ i ]) r e t u r n f a l s e
;
928 }
929 m e m c p y ( cal , ((c o n s t u i n t 8 _ t *) d a t a ) + 2 , s i z e o f( cal ) ) ;
930 r e t u r n t r u e;
931 }
932 v o i d M P U 9 2 5 0 :: g e t C a l i b r a t i o n (f l o a t * offsets , f l o a t * s o f t i r o n = NULL , f l o a t * f i e l d s t r e n g t h = N U L L ) {
933 if ( o f f s e t s != N U L L ) {
934 m e m c p y ( offsets , cal , 36) ;
935 }
936 if ( s o f t i r o n != N U L L ) {
937 * s o f t i r o n ++ = cal [ 1 0 ] ;
938 * s o f t i r o n ++ = cal [ 1 3 ] ;
939 * s o f t i r o n ++ = cal [ 1 4 ] ;
941 * s o f t i r o n ++ = cal [ 1 1 ] ;
942 * s o f t i r o n ++ = cal [ 1 5 ] ;
943 * s o f t i r o n ++ = cal [ 1 4 ] ;
944 * s o f t i r o n ++ = cal [ 1 5 ] ;
945 * s o f t i r o n ++ = cal [ 1 2 ] ;
946 }
947 if ( f i e l d s t r e n g t h != N U L L ) * f i e l d s t r e n g t h = cal [ 9 ] ;
948 }