|
38
|
1 /**
|
|
|
2 ******************************************************************************
|
|
|
3 * @file compass.c
|
|
|
4 * @author heinrichs weikamp gmbh
|
|
|
5 * @date 27-March-2014
|
|
|
6 * @version V0.2.0
|
|
|
7 * @since 21-April-2016
|
|
|
8 * @brief for Honeywell Compass and ST LSM303D
|
|
|
9 *
|
|
|
10 @verbatim
|
|
|
11 ==============================================================================
|
|
|
12 ##### How to use #####
|
|
|
13 ==============================================================================
|
|
|
14 V0.1.0 09-March-2016
|
|
|
15 V0.2.0 21-April-2016 Orientation fixed for LSM303D,
|
|
|
16 roll and pitch added to calibration output,
|
|
|
17 orientation double checked with datasheets and layout
|
|
|
18 as well as with value output during calibration
|
|
|
19 V0.2.1 19-May-2016 New date rate config and full-scale selection
|
|
|
20
|
|
|
21 @endverbatim
|
|
|
22 ******************************************************************************
|
|
|
23 * @attention
|
|
|
24 *
|
|
|
25 * <h2><center>© COPYRIGHT(c) 2016 heinrichs weikamp</center></h2>
|
|
|
26 *
|
|
|
27 ******************************************************************************
|
|
|
28 */
|
|
|
29
|
|
|
30 #include <math.h>
|
|
|
31 #include <string.h>
|
|
|
32
|
|
|
33 #include "compass.h"
|
|
|
34 #include "compass_LSM303D.h"
|
|
|
35 #include "compass_LSM303DLHC.h"
|
|
|
36
|
|
|
37 #include "i2c.h"
|
|
|
38 #include "RTE_FlashAccess.h" // to store compass_calib_data
|
|
|
39
|
|
|
40 #include "stm32f4xx_hal.h"
|
|
|
41
|
|
|
42 #define MODE_LSM303DLHC
|
|
|
43 #define TEST_IF_HMC5883L
|
|
|
44 //#define COMPASS_DEACTIVATE
|
|
|
45
|
|
|
46 /// split byte to bits
|
|
|
47 typedef struct{
|
|
|
48 uint8_t bit0:1; ///< split byte to bits
|
|
|
49 uint8_t bit1:1; ///< split byte to bits
|
|
|
50 uint8_t bit2:1; ///< split byte to bits
|
|
|
51 uint8_t bit3:1; ///< split byte to bits
|
|
|
52 uint8_t bit4:1; ///< split byte to bits
|
|
|
53 uint8_t bit5:1; ///< split byte to bits
|
|
|
54 uint8_t bit6:1; ///< split byte to bits
|
|
|
55 uint8_t bit7:1; ///< split byte to bits
|
|
|
56 } ubit8_t;
|
|
|
57
|
|
|
58
|
|
|
59 /// split byte to bits
|
|
|
60 typedef union{
|
|
|
61 ubit8_t ub; ///< split byte to bits
|
|
|
62 uint8_t uw; ///< split byte to bits
|
|
|
63 } bit8_Type;
|
|
|
64
|
|
|
65
|
|
|
66 /// split word to 2 bytes
|
|
|
67 typedef struct{
|
|
|
68 uint8_t low; ///< split word to 2 bytes
|
|
|
69 uint8_t hi; ///< split word to 2 bytes
|
|
|
70 } two_byte;
|
|
|
71
|
|
|
72
|
|
|
73 /// split word to 2 bytes
|
|
|
74 typedef union{
|
|
|
75 two_byte Byte; ///< split word to 2 bytes
|
|
|
76 uint16_t Word; ///< split word to 2 bytes
|
|
|
77 } tword;
|
|
|
78
|
|
|
79
|
|
|
80 /// split signed word to 2 bytes
|
|
|
81 typedef union{
|
|
|
82 two_byte Byte; ///< split signed word to 2 bytes
|
|
|
83 int16_t Word; ///< split signed word to 2 bytes
|
|
|
84 } signed_tword;
|
|
|
85
|
|
|
86
|
|
|
87 /// split full32 to 2 words
|
|
|
88 typedef struct{
|
|
|
89 uint16_t low16; ///< split word to 2 bytes
|
|
|
90 uint16_t hi16; ///< split word to 2 bytes
|
|
|
91 } two_word;
|
|
|
92
|
|
|
93 typedef union{
|
|
|
94 two_word Word16; ///< split word to 2 bytes
|
|
|
95 uint32_t Full32; ///< split word to 2 bytes
|
|
|
96 } tfull32;
|
|
|
97
|
|
|
98
|
|
|
99 /// crazy compass calibration stuff
|
|
|
100 typedef struct
|
|
|
101 {
|
|
|
102 unsigned short int compass_N;
|
|
|
103 float Su, Sv, Sw;
|
|
|
104 float Suu, Svv, Sww, Suv, Suw, Svw;
|
|
|
105 float Suuu, Svvv, Swww;
|
|
|
106 float Suuv, Suuw, Svvu, Svvw, Swwu, Swwv;
|
|
|
107 } SCompassCalib;
|
|
|
108
|
|
|
109
|
|
|
110 #define Q_PI (18000)
|
|
|
111 #define Q_PIO2 (9000)
|
|
|
112
|
|
|
113 #define HMC5883L (1) ///< id used with hardwareCompass
|
|
|
114 #define LSM303D (2) ///< id used with hardwareCompass
|
|
|
115 #define LSM303DLHC (3) ///< id used with hardwareCompass
|
|
|
116 #define COMPASS_NOT_RECOGNIZED (4) ///< id used with hardwareCompass
|
|
|
117
|
|
|
118
|
|
|
119 //////////////////////////////////////////////////////////////////////////////
|
|
|
120 // fifth order of polynomial approximation of atan(), giving 0.05 deg max error
|
|
|
121 //
|
|
|
122 #define K1 (5701) // Needs K1/2**16
|
|
|
123 #define K2 (1645) // Needs K2/2**48 WAS NEGATIV
|
|
|
124 #define K3 ( 446) // Needs K3/2**80
|
|
|
125
|
|
|
126 const float PI = 3.14159265; ///< pi, used in compass_calc()
|
|
|
127
|
|
|
128 typedef short int Int16;
|
|
|
129 typedef signed char Int8;
|
|
|
130 typedef Int16 Angle;
|
|
|
131
|
|
|
132
|
|
|
133 /// The (filtered) components of the magnetometer sensor
|
|
|
134 int16_t compass_DX_f; ///< output from sensor
|
|
|
135 int16_t compass_DY_f; ///< output from sensor
|
|
|
136 int16_t compass_DZ_f; ///< output from sensor
|
|
|
137
|
|
|
138
|
|
|
139 /// Found soft-iron calibration values, deduced from already filtered values
|
|
|
140 int16_t compass_CX_f; ///< calibration value
|
|
|
141 int16_t compass_CY_f; ///< calibration value
|
|
|
142 int16_t compass_CZ_f; ///< calibration value
|
|
|
143
|
|
|
144
|
|
|
145 /// The (filtered) components of the accelerometer sensor
|
|
|
146 int16_t accel_DX_f; ///< output from sensor
|
|
|
147 int16_t accel_DY_f; ///< output from sensor
|
|
|
148 int16_t accel_DZ_f; ///< output from sensor
|
|
|
149
|
|
|
150
|
|
|
151 /// The compass result values
|
|
|
152 float compass_heading; ///< the final result calculated in compass_calc()
|
|
|
153 float compass_roll; ///< the final result calculated in compass_calc()
|
|
|
154 float compass_pitch; ///< the final result calculated in compass_calc()
|
|
|
155
|
|
|
156
|
|
|
157 uint8_t compass_gain; ///< 7 on start, can be reduced during calibration
|
|
|
158
|
|
|
159 uint8_t hardwareCompass = 0; ///< either HMC5883L or LSM303D or not defined yet ( = 0 )
|
|
|
160
|
|
|
161 /// LSM303D variables
|
|
|
162 uint8_t magDataBuffer[6]; ///< here raw data from LSM303D is stored, can be local
|
|
|
163 uint8_t accDataBuffer[6]; ///< here raw data from LSM303D is stored, can be local
|
|
|
164
|
|
|
165 //uint16_t velMag = 0;
|
|
|
166 //uint16_t velAcc = 0;
|
|
|
167
|
|
|
168 //uint16_t magODR[] = {31,62,125,250,500,1000,2000};
|
|
|
169 //uint16_t accODR[] = {0,31,62,125,250,500,1000,2000,4000,8000,16000};
|
|
|
170 //uint8_t fastest = 10; //no sensor is the fastest
|
|
|
171 //uint8_t datas1 = 0;
|
|
|
172 //uint8_t zoffFlag = 0;
|
|
|
173 //uint8_t sendFlag = 0;
|
|
|
174
|
|
|
175
|
|
|
176 // all by pixhawk code:
|
|
|
177
|
|
|
178 // struct accel_scale _accel_scale;
|
|
|
179 unsigned _accel_range_m_s2;
|
|
|
180 float _accel_range_scale;
|
|
|
181 unsigned _accel_samplerate;
|
|
|
182 unsigned _accel_onchip_filter_bandwith;
|
|
|
183
|
|
|
184 // struct mag_scale _mag_scale;
|
|
|
185 unsigned _mag_range_ga;
|
|
|
186 float _mag_range_scale;
|
|
|
187 unsigned _mag_samplerate;
|
|
|
188
|
|
|
189 // default scale factors
|
|
|
190 float _accel_scale_x_offset = 0.0f;
|
|
|
191 float _accel_scale_x_scale = 1.0f;
|
|
|
192 float _accel_scale_y_offset = 0.0f;
|
|
|
193 float _accel_scale_y_scale = 1.0f;
|
|
|
194 float _accel_scale_z_offset = 0.0f;
|
|
|
195 float _accel_scale_z_scale = 1.0f;
|
|
|
196
|
|
|
197 float _mag_scale_x_offset = 0.0f;
|
|
|
198 float _mag_scale_x_scale = 1.0f;
|
|
|
199 float _mag_scale_y_offset = 0.0f;
|
|
|
200 float _mag_scale_y_scale = 1.0f;
|
|
|
201 float _mag_scale_z_offset = 0.0f;
|
|
|
202 float _mag_scale_z_scale = 1.0f;
|
|
|
203
|
|
|
204
|
|
|
205 /* External function prototypes ----------------------------------------------*/
|
|
|
206
|
|
|
207 extern void copyCompassDataDuringCalibration(int16_t dx, int16_t dy, int16_t dz);
|
|
|
208
|
|
|
209 /* Private function prototypes -----------------------------------------------*/
|
|
|
210
|
|
|
211 void compass_reset_calibration(SCompassCalib *g);
|
|
|
212 void compass_add_calibration(SCompassCalib *g);
|
|
|
213 void compass_solve_calibration(SCompassCalib *g);
|
|
|
214
|
|
|
215 void compass_init_HMC5883L(uint8_t fast, uint8_t gain);
|
|
|
216 void compass_sleep_HMC5883L(void);
|
|
|
217 void compass_read_HMC5883L(void);
|
|
|
218
|
|
|
219 void accelerator_init_MMA8452Q(void);
|
|
|
220 void accelerator_sleep_MMA8452Q(void);
|
|
|
221 void acceleration_read_MMA8452Q(void);
|
|
|
222
|
|
|
223 void compass_init_LSM303D(uint8_t fast, uint8_t gain);
|
|
|
224 void compass_sleep_LSM303D(void);
|
|
|
225 void compass_read_LSM303D(void);
|
|
|
226 void acceleration_read_LSM303D(void);
|
|
|
227
|
|
|
228 void compass_init_LSM303DLHC(uint8_t fast, uint8_t gain);
|
|
|
229 void compass_sleep_LSM303DLHC(void);
|
|
|
230 void compass_read_LSM303DLHC(void);
|
|
|
231 void acceleration_read_LSM303DLHC(void);
|
|
|
232
|
|
|
233
|
|
|
234 int LSM303D_accel_set_onchip_lowpass_filter_bandwidth(unsigned bandwidth);
|
|
|
235 int compass_calib_common(void);
|
|
|
236
|
|
|
237 void compass_calc_roll_pitch_only(void);
|
|
|
238
|
|
|
239 void rotate_mag_3f(float *x, float *y, float *z);
|
|
|
240 void rotate_accel_3f(float *x, float *y, float *z);
|
|
|
241
|
|
|
242
|
|
|
243 /* Exported functions --------------------------------------------------------*/
|
|
|
244
|
|
|
245
|
|
|
246 // ===============================================================================
|
|
|
247 // compass_init
|
|
|
248 /// @brief This might be called several times with different gain values during calibration
|
|
|
249 /// On first call it figures out which hardware is integrated
|
|
|
250 ///
|
|
|
251 /// @param gain: 7 is max gain, compass_calib() might reduce it
|
|
|
252 // ===============================================================================
|
|
|
253
|
|
|
254 uint8_t testCompassTypeDebug = 0xFF;
|
|
|
255
|
|
|
256 void compass_init(uint8_t fast, uint8_t gain)
|
|
|
257 {
|
|
|
258 // quick off
|
|
|
259 #ifdef COMPASS_DEACTIVATE
|
|
|
260 hardwareCompass = COMPASS_NOT_RECOGNIZED;
|
|
|
261 #endif
|
|
|
262
|
|
|
263 // don't call again with fast, gain in calib mode etc.
|
|
|
264 // if unknown
|
|
|
265 if(hardwareCompass == COMPASS_NOT_RECOGNIZED)
|
|
|
266 {
|
|
|
267 return;
|
|
|
268 }
|
|
|
269
|
|
|
270 // old code but without else
|
|
|
271 if(hardwareCompass == 0)
|
|
|
272 {
|
|
|
273 uint8_t data = WHO_AM_I;
|
|
|
274 I2C_Master_Transmit( DEVICE_COMPASS_303D, &data, 1);
|
|
|
275 I2C_Master_Receive( DEVICE_COMPASS_303D, &data, 1);
|
|
|
276 if(data == WHOIAM_VALUE)
|
|
|
277 hardwareCompass = LSM303D;
|
|
|
278 // else
|
|
|
279 // hardwareCompass = HMC5883L;
|
|
|
280 }
|
|
|
281
|
|
|
282
|
|
|
283 // könnte Probleme mit altem Chip machen
|
|
|
284 // beim 303D führt dieser Code dazu, dass WHOIAM_VALUE nicht geschickt wird!!!
|
|
|
285
|
|
|
286 #ifdef MODE_LSM303DLHC
|
|
|
287 HAL_StatusTypeDef resultOfOperation = HAL_TIMEOUT;
|
|
|
288
|
|
|
289 if(hardwareCompass == 0)
|
|
|
290 {
|
|
|
291 uint8_t data = DLHC_CTRL_REG1_A;
|
|
|
292 resultOfOperation = I2C_Master_Transmit( DEVICE_ACCELARATOR_303DLHC, &data, 1);
|
|
|
293 if(resultOfOperation == HAL_OK)
|
|
|
294 {
|
|
|
295 I2C_Master_Receive( DEVICE_ACCELARATOR_303DLHC, &data, 1);
|
|
|
296 testCompassTypeDebug = data;
|
|
|
297 if((data & 0x0f) == 0x07)
|
|
|
298 {
|
|
|
299 hardwareCompass = LSM303DLHC;
|
|
|
300 }
|
|
|
301 }
|
|
|
302 else
|
|
|
303 {
|
|
|
304 testCompassTypeDebug = 0xEE;
|
|
|
305 }
|
|
|
306 }
|
|
|
307
|
|
|
308 #endif
|
|
|
309 /*
|
|
|
310 if(hardwareCompass == 0)
|
|
|
311 {
|
|
|
312 uint8_t data = WHO_AM_I;
|
|
|
313 I2C_Master_Transmit( DEVICE_COMPASS_303D, &data, 1);
|
|
|
314 I2C_Master_Receive( DEVICE_COMPASS_303D, &data, 1);
|
|
|
315 if(data == WHOIAM_VALUE)
|
|
|
316 hardwareCompass = LSM303D;
|
|
|
317 else
|
|
|
318 hardwareCompass = HMC5883L;
|
|
|
319 }
|
|
|
320 */
|
|
|
321 // was in else before !
|
|
|
322 if(hardwareCompass == 0)
|
|
|
323 hardwareCompass = HMC5883L;
|
|
|
324
|
|
|
325 #ifdef TEST_IF_HMC5883L
|
|
|
326 HAL_StatusTypeDef resultOfOperationHMC_MMA = HAL_TIMEOUT;
|
|
|
327
|
|
|
328 if(hardwareCompass == HMC5883L)
|
|
|
329 {
|
|
|
330 uint8_t data = 0x2A; // CTRL_REG1 of DEVICE_ACCELARATOR_MMA8452Q
|
|
|
331 resultOfOperationHMC_MMA = I2C_Master_Transmit( DEVICE_ACCELARATOR_MMA8452Q, &data, 1);
|
|
|
332 if(resultOfOperationHMC_MMA == HAL_OK)
|
|
|
333 {
|
|
|
334 hardwareCompass = HMC5883L; // all fine, keep it
|
|
|
335 }
|
|
|
336 else
|
|
|
337 {
|
|
|
338 hardwareCompass = COMPASS_NOT_RECOGNIZED;
|
|
|
339 testCompassTypeDebug = 0xEC;
|
|
|
340 }
|
|
|
341 }
|
|
|
342 #endif
|
|
|
343
|
|
|
344
|
|
|
345 if(hardwareCompass == LSM303DLHC)
|
|
|
346 {
|
|
|
347 compass_init_LSM303DLHC(fast, gain);
|
|
|
348 }
|
|
|
349 else
|
|
|
350 if(hardwareCompass == LSM303D)
|
|
|
351 {
|
|
|
352 compass_init_LSM303D(fast, gain);
|
|
|
353 }
|
|
|
354 else
|
|
|
355 if(hardwareCompass == HMC5883L)
|
|
|
356 {
|
|
|
357 compass_init_HMC5883L(fast, gain);
|
|
|
358 }
|
|
|
359
|
|
|
360 tfull32 dataBlock[4];
|
|
|
361 if(BFA_readLastDataBlock((uint32_t *)dataBlock) == BFA_OK)
|
|
|
362 {
|
|
|
363 compass_CX_f = dataBlock[0].Word16.low16;
|
|
|
364 compass_CY_f = dataBlock[0].Word16.hi16;
|
|
|
365 compass_CZ_f = dataBlock[1].Word16.low16;
|
|
|
366 }
|
|
|
367
|
|
|
368 }
|
|
|
369
|
|
|
370
|
|
|
371 // ===============================================================================
|
|
|
372 // compass_calib
|
|
|
373 /// @brief with onchip_lowpass_filter configuration for accelerometer of LSM303D
|
|
|
374 // ===============================================================================
|
|
|
375 int compass_calib(void)
|
|
|
376 {
|
|
|
377 if(hardwareCompass == LSM303DLHC)
|
|
|
378 {
|
|
|
379 return compass_calib_common(); // 170821 zur Zeit kein lowpass filtering gefunden, nur high pass auf dem Register ohne Erklärung
|
|
|
380 }
|
|
|
381 else
|
|
|
382 if(hardwareCompass == LSM303D)
|
|
|
383 {
|
|
|
384 LSM303D_accel_set_onchip_lowpass_filter_bandwidth(773);
|
|
|
385 int out = compass_calib_common();
|
|
|
386 LSM303D_accel_set_onchip_lowpass_filter_bandwidth(LSM303D_ACCEL_DEFAULT_ONCHIP_FILTER_FREQ);
|
|
|
387 return out;
|
|
|
388 }
|
|
|
389 else
|
|
|
390 if(hardwareCompass == HMC5883L)
|
|
|
391 {
|
|
|
392 return compass_calib_common();
|
|
|
393 }
|
|
|
394 else
|
|
|
395 {
|
|
|
396 return 0; // standard answer of compass_calib_common();
|
|
|
397 }
|
|
|
398
|
|
|
399
|
|
|
400 }
|
|
|
401
|
|
|
402
|
|
|
403 // ===============================================================================
|
|
|
404 // compass_sleep
|
|
|
405 /// @brief low power mode
|
|
|
406 // ===============================================================================
|
|
|
407 void compass_sleep(void)
|
|
|
408 {
|
|
|
409 if(hardwareCompass == LSM303DLHC)
|
|
|
410 {
|
|
|
411 compass_sleep_LSM303DLHC();
|
|
|
412 }
|
|
|
413 else
|
|
|
414 if(hardwareCompass == LSM303D)
|
|
|
415 {
|
|
|
416 compass_sleep_LSM303D();
|
|
|
417 }
|
|
|
418 else
|
|
|
419 if(hardwareCompass == HMC5883L)
|
|
|
420 {
|
|
|
421 compass_sleep_HMC5883L();
|
|
|
422 }
|
|
|
423 }
|
|
|
424
|
|
|
425
|
|
|
426 // ===============================================================================
|
|
|
427 // compass_read
|
|
|
428 /// @brief reads magnetometer and accelerometer for LSM303D,
|
|
|
429 /// otherwise magnetometer only
|
|
|
430 // ===============================================================================
|
|
|
431 void compass_read(void)
|
|
|
432 {
|
|
|
433 if(hardwareCompass == LSM303DLHC)
|
|
|
434 {
|
|
|
435 compass_read_LSM303DLHC();
|
|
|
436 }
|
|
|
437 else
|
|
|
438 if(hardwareCompass == LSM303D)
|
|
|
439 {
|
|
|
440 compass_read_LSM303D();
|
|
|
441 }
|
|
|
442 else
|
|
|
443 if(hardwareCompass == HMC5883L)
|
|
|
444 {
|
|
|
445 compass_read_HMC5883L();
|
|
|
446 }
|
|
|
447 }
|
|
|
448
|
|
|
449
|
|
|
450 // ===============================================================================
|
|
|
451 // accelerator_init
|
|
|
452 /// @brief empty for for LSM303D
|
|
|
453 // ===============================================================================
|
|
|
454 void accelerator_init(void)
|
|
|
455 {
|
|
|
456 // if((hardwareCompass != LSM303D) && (hardwareCompass != LSM303DLHC))
|
|
|
457 if(hardwareCompass == HMC5883L)
|
|
|
458 accelerator_init_MMA8452Q();
|
|
|
459 }
|
|
|
460
|
|
|
461
|
|
|
462 // ===============================================================================
|
|
|
463 // accelerator_sleep
|
|
|
464 /// @brief empty for for LSM303D
|
|
|
465 // ===============================================================================
|
|
|
466 void accelerator_sleep(void)
|
|
|
467 {
|
|
|
468 // if((hardwareCompass != LSM303D) && (hardwareCompass != LSM303DLHC))
|
|
|
469 if(hardwareCompass == HMC5883L)
|
|
|
470 accelerator_sleep_MMA8452Q();
|
|
|
471 }
|
|
|
472
|
|
|
473
|
|
|
474 // ===============================================================================
|
|
|
475 // acceleration_read
|
|
|
476 /// @brief empty for for LSM303D
|
|
|
477 // ===============================================================================
|
|
|
478 void acceleration_read(void)
|
|
|
479 {
|
|
|
480 if(hardwareCompass == LSM303DLHC)
|
|
|
481 {
|
|
|
482 acceleration_read_LSM303DLHC();
|
|
|
483 }
|
|
|
484 else
|
|
|
485 if(hardwareCompass == LSM303D)
|
|
|
486 {
|
|
|
487 acceleration_read_LSM303D();
|
|
|
488 }
|
|
|
489 else
|
|
|
490 if(hardwareCompass == HMC5883L)
|
|
|
491 {
|
|
|
492 acceleration_read_MMA8452Q();
|
|
|
493 }
|
|
|
494 }
|
|
|
495
|
|
|
496
|
|
|
497 /* Private functions ---------------------------------------------------------*/
|
|
|
498
|
|
|
499 // ===============================================================================
|
|
|
500 // LSM303D_read_reg
|
|
|
501 /// @brief tiny helpers by pixhawk
|
|
|
502 // ===============================================================================
|
|
|
503 uint8_t LSM303D_read_reg(uint8_t addr)
|
|
|
504 {
|
|
|
505 uint8_t data;
|
|
|
506
|
|
|
507 I2C_Master_Transmit( DEVICE_COMPASS_303D, &addr, 1);
|
|
|
508 I2C_Master_Receive( DEVICE_COMPASS_303D, &data, 1);
|
|
|
509 return data;
|
|
|
510 }
|
|
|
511
|
|
|
512
|
|
|
513 // ===============================================================================
|
|
|
514 // LSM303D_write_reg
|
|
|
515 /// @brief tiny helpers by pixhawk
|
|
|
516 // ===============================================================================
|
|
|
517 void LSM303D_write_reg(uint8_t addr, uint8_t value)
|
|
|
518 {
|
|
|
519 uint8_t data[2];
|
|
|
520
|
|
|
521 /* enable accel*/
|
|
|
522 data[0] = addr;
|
|
|
523 data[1] = value;
|
|
|
524 I2C_Master_Transmit( DEVICE_COMPASS_303D, data, 2);
|
|
|
525 }
|
|
|
526
|
|
|
527
|
|
|
528 // ===============================================================================
|
|
|
529 // LSM303D_write_checked_reg
|
|
|
530 /// @brief tiny helpers by pixhawk. This runs unchecked at the moment.
|
|
|
531 // ===============================================================================
|
|
|
532 void LSM303D_write_checked_reg(uint8_t addr, uint8_t value)
|
|
|
533 {
|
|
|
534 LSM303D_write_reg(addr, value);
|
|
|
535 }
|
|
|
536
|
|
|
537
|
|
|
538 // ===============================================================================
|
|
|
539 // LSM303D_modify_reg
|
|
|
540 /// @brief tiny helpers by pixhawk
|
|
|
541 // ===============================================================================
|
|
|
542 void LSM303D_modify_reg(unsigned reg, uint8_t clearbits, uint8_t setbits)
|
|
|
543 {
|
|
|
544 uint8_t val;
|
|
|
545
|
|
|
546 val = LSM303D_read_reg(reg);
|
|
|
547 val &= ~clearbits;
|
|
|
548 val |= setbits;
|
|
|
549 LSM303D_write_checked_reg(reg, val);
|
|
|
550 }
|
|
|
551
|
|
|
552
|
|
|
553
|
|
|
554 // ===============================================================================
|
|
|
555 // LSM303DLHC_accelerator_read_req
|
|
|
556 /// @brief
|
|
|
557 // ===============================================================================
|
|
|
558 uint8_t LSM303DLHC_accelerator_read_req(uint8_t addr)
|
|
|
559 {
|
|
|
560 uint8_t data;
|
|
|
561
|
|
|
562 I2C_Master_Transmit( DEVICE_ACCELARATOR_303DLHC, &addr, 1);
|
|
|
563 I2C_Master_Receive( DEVICE_ACCELARATOR_303DLHC, &data, 1);
|
|
|
564 return data;
|
|
|
565 }
|
|
|
566
|
|
|
567
|
|
|
568 // ===============================================================================
|
|
|
569 // LSM303DLHC_accelerator_write_req
|
|
|
570 /// @brief
|
|
|
571 // ===============================================================================
|
|
|
572 void LSM303DLHC_accelerator_write_req(uint8_t addr, uint8_t value)
|
|
|
573 {
|
|
|
574 uint8_t data[2];
|
|
|
575
|
|
|
576 /* enable accel*/
|
|
|
577 data[0] = addr;
|
|
|
578 data[1] = value;
|
|
|
579 I2C_Master_Transmit( DEVICE_ACCELARATOR_303DLHC, data, 2);
|
|
|
580 }
|
|
|
581
|
|
|
582 /*
|
|
|
583 // ===============================================================================
|
|
|
584 // LSM303D_accel_set_range
|
|
|
585 /// @brief tiny helpers by pixhawk
|
|
|
586 // ===============================================================================
|
|
|
587 int LSM303D_accel_set_range(unsigned max_g)
|
|
|
588 {
|
|
|
589 uint8_t setbits = 0;
|
|
|
590 uint8_t clearbits = REG2_FULL_SCALE_BITS_A;
|
|
|
591 float new_scale_g_digit = 0.0f;
|
|
|
592
|
|
|
593 if (max_g == 0) {
|
|
|
594 max_g = 16;
|
|
|
595 }
|
|
|
596
|
|
|
597 if (max_g <= 2) {
|
|
|
598 _accel_range_m_s2 = 2.0f * LSM303D_ONE_G;
|
|
|
599 setbits |= REG2_FULL_SCALE_2G_A;
|
|
|
600 new_scale_g_digit = 0.061e-3f;
|
|
|
601
|
|
|
602 } else if (max_g <= 4) {
|
|
|
603 _accel_range_m_s2 = 4.0f * LSM303D_ONE_G;
|
|
|
604 setbits |= REG2_FULL_SCALE_4G_A;
|
|
|
605 new_scale_g_digit = 0.122e-3f;
|
|
|
606
|
|
|
607 } else if (max_g <= 6) {
|
|
|
608 _accel_range_m_s2 = 6.0f * LSM303D_ONE_G;
|
|
|
609 setbits |= REG2_FULL_SCALE_6G_A;
|
|
|
610 new_scale_g_digit = 0.183e-3f;
|
|
|
611
|
|
|
612 } else if (max_g <= 8) {
|
|
|
613 _accel_range_m_s2 = 8.0f * LSM303D_ONE_G;
|
|
|
614 setbits |= REG2_FULL_SCALE_8G_A;
|
|
|
615 new_scale_g_digit = 0.244e-3f;
|
|
|
616
|
|
|
617 } else if (max_g <= 16) {
|
|
|
618 _accel_range_m_s2 = 16.0f * LSM303D_ONE_G;
|
|
|
619 setbits |= REG2_FULL_SCALE_16G_A;
|
|
|
620 new_scale_g_digit = 0.732e-3f;
|
|
|
621
|
|
|
622 } else {
|
|
|
623 return -1;
|
|
|
624 }
|
|
|
625
|
|
|
626 _accel_range_scale = new_scale_g_digit * LSM303D_ONE_G;
|
|
|
627
|
|
|
628
|
|
|
629 LSM303D_modify_reg(ADDR_CTRL_REG2, clearbits, setbits);
|
|
|
630
|
|
|
631 return 0;
|
|
|
632 }
|
|
|
633 */
|
|
|
634 /*
|
|
|
635 // ===============================================================================
|
|
|
636 // LSM303D_mag_set_range
|
|
|
637 /// @brief tiny helpers by pixhawk
|
|
|
638 // ===============================================================================
|
|
|
639 int LSM303D_mag_set_range(unsigned max_ga)
|
|
|
640 {
|
|
|
641 uint8_t setbits = 0;
|
|
|
642 uint8_t clearbits = REG6_FULL_SCALE_BITS_M;
|
|
|
643 float new_scale_ga_digit = 0.0f;
|
|
|
644
|
|
|
645 if (max_ga == 0) {
|
|
|
646 max_ga = 12;
|
|
|
647 }
|
|
|
648
|
|
|
649 if (max_ga <= 2) {
|
|
|
650 _mag_range_ga = 2;
|
|
|
651 setbits |= REG6_FULL_SCALE_2GA_M;
|
|
|
652 new_scale_ga_digit = 0.080e-3f;
|
|
|
653
|
|
|
654 } else if (max_ga <= 4) {
|
|
|
655 _mag_range_ga = 4;
|
|
|
656 setbits |= REG6_FULL_SCALE_4GA_M;
|
|
|
657 new_scale_ga_digit = 0.160e-3f;
|
|
|
658
|
|
|
659 } else if (max_ga <= 8) {
|
|
|
660 _mag_range_ga = 8;
|
|
|
661 setbits |= REG6_FULL_SCALE_8GA_M;
|
|
|
662 new_scale_ga_digit = 0.320e-3f;
|
|
|
663
|
|
|
664 } else if (max_ga <= 12) {
|
|
|
665 _mag_range_ga = 12;
|
|
|
666 setbits |= REG6_FULL_SCALE_12GA_M;
|
|
|
667 new_scale_ga_digit = 0.479e-3f;
|
|
|
668
|
|
|
669 } else {
|
|
|
670 return -1;
|
|
|
671 }
|
|
|
672
|
|
|
673 _mag_range_scale = new_scale_ga_digit;
|
|
|
674
|
|
|
675 LSM303D_modify_reg(ADDR_CTRL_REG6, clearbits, setbits);
|
|
|
676
|
|
|
677 return 0;
|
|
|
678 }
|
|
|
679 */
|
|
|
680
|
|
|
681 // ===============================================================================
|
|
|
682 // LSM303D_accel_set_onchip_lowpass_filter_bandwidth
|
|
|
683 /// @brief tiny helpers by pixhawk
|
|
|
684 // ===============================================================================
|
|
|
685 int LSM303D_accel_set_onchip_lowpass_filter_bandwidth(unsigned bandwidth)
|
|
|
686 {
|
|
|
687 uint8_t setbits = 0;
|
|
|
688 uint8_t clearbits = REG2_ANTIALIAS_FILTER_BW_BITS_A;
|
|
|
689
|
|
|
690 if (bandwidth == 0) {
|
|
|
691 bandwidth = 773;
|
|
|
692 }
|
|
|
693
|
|
|
694 if (bandwidth <= 50) {
|
|
|
695 setbits |= REG2_AA_FILTER_BW_50HZ_A;
|
|
|
696 _accel_onchip_filter_bandwith = 50;
|
|
|
697
|
|
|
698 } else if (bandwidth <= 194) {
|
|
|
699 setbits |= REG2_AA_FILTER_BW_194HZ_A;
|
|
|
700 _accel_onchip_filter_bandwith = 194;
|
|
|
701
|
|
|
702 } else if (bandwidth <= 362) {
|
|
|
703 setbits |= REG2_AA_FILTER_BW_362HZ_A;
|
|
|
704 _accel_onchip_filter_bandwith = 362;
|
|
|
705
|
|
|
706 } else if (bandwidth <= 773) {
|
|
|
707 setbits |= REG2_AA_FILTER_BW_773HZ_A;
|
|
|
708 _accel_onchip_filter_bandwith = 773;
|
|
|
709
|
|
|
710 } else {
|
|
|
711 return -1;
|
|
|
712 }
|
|
|
713
|
|
|
714 LSM303D_modify_reg(ADDR_CTRL_REG2, clearbits, setbits);
|
|
|
715
|
|
|
716 return 0;
|
|
|
717 }
|
|
|
718
|
|
|
719
|
|
|
720 // ===============================================================================
|
|
|
721 // LSM303D_accel_set_driver_lowpass_filter
|
|
|
722 /// @brief tiny helpers by pixhawk. This one is not used at the moment!
|
|
|
723 // ===============================================================================
|
|
|
724 int LSM303D_accel_set_driver_lowpass_filter(float samplerate, float bandwidth)
|
|
|
725 {
|
|
|
726 /*
|
|
|
727 _accel_filter_x_set_cutoff_frequency(samplerate, bandwidth);
|
|
|
728 _accel_filter_y_set_cutoff_frequency(samplerate, bandwidth);
|
|
|
729 _accel_filter_z_set_cutoff_frequency(samplerate, bandwidth);
|
|
|
730 */
|
|
|
731 return 0;
|
|
|
732 }
|
|
|
733
|
|
|
734 /* unused 170821
|
|
|
735 // ===============================================================================
|
|
|
736 // LSM303D_accel_set_samplerate
|
|
|
737 /// @brief tiny helpers by pixhawk
|
|
|
738 // ===============================================================================
|
|
|
739 int LSM303D_accel_set_samplerate(unsigned frequency)
|
|
|
740 {
|
|
|
741 uint8_t setbits = 0;
|
|
|
742 uint8_t clearbits = REG1_RATE_BITS_A;
|
|
|
743
|
|
|
744 // if (frequency == 0 || frequency == ACCEL_SAMPLERATE_DEFAULT) {
|
|
|
745 frequency = 1600;
|
|
|
746 // }
|
|
|
747
|
|
|
748 if (frequency <= 3) {
|
|
|
749 setbits |= REG1_RATE_3_125HZ_A;
|
|
|
750 _accel_samplerate = 3;
|
|
|
751
|
|
|
752 } else if (frequency <= 6) {
|
|
|
753 setbits |= REG1_RATE_6_25HZ_A;
|
|
|
754 _accel_samplerate = 6;
|
|
|
755
|
|
|
756 } else if (frequency <= 12) {
|
|
|
757 setbits |= REG1_RATE_12_5HZ_A;
|
|
|
758 _accel_samplerate = 12;
|
|
|
759
|
|
|
760 } else if (frequency <= 25) {
|
|
|
761 setbits |= REG1_RATE_25HZ_A;
|
|
|
762 _accel_samplerate = 25;
|
|
|
763
|
|
|
764 } else if (frequency <= 50) {
|
|
|
765 setbits |= REG1_RATE_50HZ_A;
|
|
|
766 _accel_samplerate = 50;
|
|
|
767
|
|
|
768 } else if (frequency <= 100) {
|
|
|
769 setbits |= REG1_RATE_100HZ_A;
|
|
|
770 _accel_samplerate = 100;
|
|
|
771
|
|
|
772 } else if (frequency <= 200) {
|
|
|
773 setbits |= REG1_RATE_200HZ_A;
|
|
|
774 _accel_samplerate = 200;
|
|
|
775
|
|
|
776 } else if (frequency <= 400) {
|
|
|
777 setbits |= REG1_RATE_400HZ_A;
|
|
|
778 _accel_samplerate = 400;
|
|
|
779
|
|
|
780 } else if (frequency <= 800) {
|
|
|
781 setbits |= REG1_RATE_800HZ_A;
|
|
|
782 _accel_samplerate = 800;
|
|
|
783
|
|
|
784 } else if (frequency <= 1600) {
|
|
|
785 setbits |= REG1_RATE_1600HZ_A;
|
|
|
786 _accel_samplerate = 1600;
|
|
|
787
|
|
|
788 } else {
|
|
|
789 return -1;
|
|
|
790 }
|
|
|
791
|
|
|
792 LSM303D_modify_reg(ADDR_CTRL_REG1, clearbits, setbits);
|
|
|
793 return 0;
|
|
|
794 }
|
|
|
795 // ===============================================================================
|
|
|
796 // LSM303D_mag_set_samplerate
|
|
|
797 /// @brief tiny helpers by pixhawk
|
|
|
798 // ===============================================================================
|
|
|
799 int LSM303D_mag_set_samplerate(unsigned frequency)
|
|
|
800 {
|
|
|
801 uint8_t setbits = 0;
|
|
|
802 uint8_t clearbits = REG5_RATE_BITS_M;
|
|
|
803
|
|
|
804 if (frequency == 0) {
|
|
|
805 frequency = 100;
|
|
|
806 }
|
|
|
807
|
|
|
808 if (frequency <= 3) {
|
|
|
809 setbits |= REG5_RATE_3_125HZ_M;
|
|
|
810 _mag_samplerate = 25;
|
|
|
811
|
|
|
812 } else if (frequency <= 6) {
|
|
|
813 setbits |= REG5_RATE_6_25HZ_M;
|
|
|
814 _mag_samplerate = 25;
|
|
|
815
|
|
|
816 } else if (frequency <= 12) {
|
|
|
817 setbits |= REG5_RATE_12_5HZ_M;
|
|
|
818 _mag_samplerate = 25;
|
|
|
819
|
|
|
820 } else if (frequency <= 25) {
|
|
|
821 setbits |= REG5_RATE_25HZ_M;
|
|
|
822 _mag_samplerate = 25;
|
|
|
823
|
|
|
824 } else if (frequency <= 50) {
|
|
|
825 setbits |= REG5_RATE_50HZ_M;
|
|
|
826 _mag_samplerate = 50;
|
|
|
827
|
|
|
828 } else if (frequency <= 100) {
|
|
|
829 setbits |= REG5_RATE_100HZ_M;
|
|
|
830 _mag_samplerate = 100;
|
|
|
831
|
|
|
832 } else {
|
|
|
833 return -1;
|
|
|
834 }
|
|
|
835
|
|
|
836 LSM303D_modify_reg(ADDR_CTRL_REG5, clearbits, setbits);
|
|
|
837 return 0;
|
|
|
838 }
|
|
|
839 */
|
|
|
840
|
|
|
841
|
|
|
842 // rotate_mag_3f: nicht genutzt aber praktisch; rotate_accel_3f wird benutzt
|
|
|
843 // ===============================================================================
|
|
|
844 // rotate_mag_3f
|
|
|
845 /// @brief swap axis in convient way, by hw
|
|
|
846 /// @param *x raw input is set to *y input
|
|
|
847 /// @param *y raw input is set to -*x input
|
|
|
848 /// @param *z raw is not touched
|
|
|
849 // ===============================================================================
|
|
|
850 void rotate_mag_3f(float *x, float *y, float *z)
|
|
|
851 {
|
|
|
852 return;
|
|
|
853 /*
|
|
|
854 *x = *x; // HMC: *x = -*y
|
|
|
855 *y = *y; // HMC: *y = *x // change 20.04.2016: zuvor *y = -*y
|
|
|
856 *z = *z; // HMC: *z = *z
|
|
|
857 */
|
|
|
858 }
|
|
|
859
|
|
|
860
|
|
|
861 // ===============================================================================
|
|
|
862 // rotate_accel_3f
|
|
|
863 /// @brief swap axis in convient way, by hw, same as MMA8452Q
|
|
|
864 /// @param *x raw input, output is with sign change
|
|
|
865 /// @param *y raw input, output is with sign change
|
|
|
866 /// @param *z raw input, output is with sign change
|
|
|
867 // ===============================================================================
|
|
|
868 void rotate_accel_3f(float *x, float *y, float *z)
|
|
|
869 {
|
|
|
870 *x = -*x;
|
|
|
871 *y = -*y;
|
|
|
872 *z = -*z;
|
|
|
873 /* tested:
|
|
|
874 x = x, y =-y, z=-z: does not work with roll
|
|
|
875 x = x, y =y, z=-z: does not work with pitch
|
|
|
876 x = x, y =y, z=z: does not work at all
|
|
|
877 */
|
|
|
878 }
|
|
|
879
|
|
|
880
|
|
|
881 // ===============================================================================
|
|
|
882 // compass_init_LSM303D by PIXhawk (LSM303D::reset())
|
|
|
883 // https://raw.githubusercontent.com/PX4/Firmware/master/src/drivers/lsm303d/lsm303d.cpp
|
|
|
884 /// @brief The new ST 303D
|
|
|
885 /// This might be called several times with different gain values during calibration
|
|
|
886 /// but gain change is not supported at the moment.
|
|
|
887 ///
|
|
|
888 /// @param gain: 7 is max gain and set with here, compass_calib() might reduce it
|
|
|
889 // ===============================================================================
|
|
|
890
|
|
|
891 //uint8_t testCompassLS303D[11];
|
|
|
892
|
|
|
893 void compass_init_LSM303D(uint8_t fast, uint8_t gain)
|
|
|
894 {
|
|
|
895 // matthias version 160620
|
|
|
896 if(fast == 0)
|
|
|
897 {
|
|
|
898 LSM303D_write_checked_reg(ADDR_CTRL_REG0, 0x00);
|
|
|
899 LSM303D_write_checked_reg(ADDR_CTRL_REG1, 0x3F); // mod 12,5 Hz 3 instead of 6,25 Hz 2
|
|
|
900 LSM303D_write_checked_reg(ADDR_CTRL_REG2, 0xC0);
|
|
|
901 LSM303D_write_checked_reg(ADDR_CTRL_REG3, 0x00);
|
|
|
902 LSM303D_write_checked_reg(ADDR_CTRL_REG4, 0x00);
|
|
|
903 LSM303D_write_checked_reg(ADDR_CTRL_REG5, 0x68); // mod 12,5 Hz 8 instead of 6,25 Hz 4
|
|
|
904 }
|
|
|
905 else
|
|
|
906 {
|
|
|
907 LSM303D_write_checked_reg(ADDR_CTRL_REG0, 0x00);
|
|
|
908 LSM303D_write_checked_reg(ADDR_CTRL_REG1, 0x6F); // 100 Hz
|
|
|
909 LSM303D_write_checked_reg(ADDR_CTRL_REG2, 0xC0);
|
|
|
910 LSM303D_write_checked_reg(ADDR_CTRL_REG3, 0x00);
|
|
|
911 LSM303D_write_checked_reg(ADDR_CTRL_REG4, 0x00);
|
|
|
912 LSM303D_write_checked_reg(ADDR_CTRL_REG5, 0x74); // 100 Hz
|
|
|
913 }
|
|
|
914 LSM303D_write_checked_reg(ADDR_CTRL_REG6, 0x00);
|
|
|
915 LSM303D_write_checked_reg(ADDR_CTRL_REG7, 0x00);
|
|
|
916
|
|
|
917 /*
|
|
|
918 uint8_t data;
|
|
|
919 for(int i=0;i<11;i++)
|
|
|
920 {
|
|
|
921 data = ADDR_INT_THS_L_M + i;
|
|
|
922 I2C_Master_Transmit( DEVICE_COMPASS_303D, &data, 1);
|
|
|
923 I2C_Master_Receive( DEVICE_COMPASS_303D, &testCompassLS303D[i], 1);
|
|
|
924 }
|
|
|
925 */
|
|
|
926
|
|
|
927 return;
|
|
|
928 /*
|
|
|
929 LSM303D_accel_set_range(LSM303D_ACCEL_DEFAULT_RANGE_G); // modifies ADDR_CTRL_REG2
|
|
|
930 LSM303D_accel_set_samplerate(LSM303D_ACCEL_DEFAULT_RATE); // modifies ADDR_CTRL_REG1
|
|
|
931
|
|
|
932 LSM303D_mag_set_range(LSM303D_MAG_DEFAULT_RANGE_GA);
|
|
|
933 LSM303D_mag_set_samplerate(LSM303D_MAG_DEFAULT_RATE);
|
|
|
934 */
|
|
|
935
|
|
|
936 /*
|
|
|
937 // my stuff hw
|
|
|
938 // enable accel
|
|
|
939 LSM303D_write_checked_reg(ADDR_CTRL_REG1,
|
|
|
940 REG1_X_ENABLE_A | REG1_Y_ENABLE_A | REG1_Z_ENABLE_A | REG1_BDU_UPDATE | REG1_RATE_800HZ_A);
|
|
|
941
|
|
|
942 // enable mag
|
|
|
943 LSM303D_write_checked_reg(ADDR_CTRL_REG7, REG7_CONT_MODE_M);
|
|
|
944 LSM303D_write_checked_reg(ADDR_CTRL_REG5, REG5_RES_HIGH_M | REG5_ENABLE_T);
|
|
|
945 LSM303D_write_checked_reg(ADDR_CTRL_REG3, 0x04); // DRDY on ACCEL on INT1
|
|
|
946 LSM303D_write_checked_reg(ADDR_CTRL_REG4, 0x04); // DRDY on MAG on INT2
|
|
|
947
|
|
|
948 LSM303D_accel_set_range(LSM303D_ACCEL_DEFAULT_RANGE_G);
|
|
|
949 LSM303D_accel_set_samplerate(LSM303D_ACCEL_DEFAULT_RATE);
|
|
|
950 LSM303D_accel_set_driver_lowpass_filter((float)LSM303D_ACCEL_DEFAULT_RATE, (float)LSM303D_ACCEL_DEFAULT_DRIVER_FILTER_FREQ);
|
|
|
951 //LSM303D_accel_set_onchip_lowpass_filter_bandwidth(773); // factory setting
|
|
|
952
|
|
|
953 // we setup the anti-alias on-chip filter as 50Hz. We believe
|
|
|
954 // this operates in the analog domain, and is critical for
|
|
|
955 // anti-aliasing. The 2 pole software filter is designed to
|
|
|
956 // operate in conjunction with this on-chip filter
|
|
|
957 if(fast)
|
|
|
958 LSM303D_accel_set_onchip_lowpass_filter_bandwidth(773); // factory setting
|
|
|
959 else
|
|
|
960 LSM303D_accel_set_onchip_lowpass_filter_bandwidth(LSM303D_ACCEL_DEFAULT_ONCHIP_FILTER_FREQ);
|
|
|
961
|
|
|
962
|
|
|
963 LSM303D_mag_set_range(LSM303D_MAG_DEFAULT_RANGE_GA);
|
|
|
964 LSM303D_mag_set_samplerate(LSM303D_MAG_DEFAULT_RATE);
|
|
|
965 */
|
|
|
966 }
|
|
|
967
|
|
|
968
|
|
|
969 // ===============================================================================
|
|
|
970 // compass_sleep_LSM303D
|
|
|
971 /// @brief The new compass chip, hopefully this works!
|
|
|
972 // ===============================================================================
|
|
|
973 void compass_sleep_LSM303D(void)
|
|
|
974 {
|
|
|
975 LSM303D_write_checked_reg(ADDR_CTRL_REG1, 0x00); // CNTRL1: acceleration sensor Power-down mode
|
|
|
976 LSM303D_write_checked_reg(ADDR_CTRL_REG7, 0x02); // CNTRL7: magnetic sensor Power-down mode
|
|
|
977 }
|
|
|
978
|
|
|
979
|
|
|
980 // ===============================================================================
|
|
|
981 // acceleration_read_LSM303D
|
|
|
982 /// @brief The new LSM303D, code by pixhawk
|
|
|
983 ///
|
|
|
984 /// output is accel_DX_f, accel_DY_f, accel_DZ_f
|
|
|
985 // ===============================================================================
|
|
|
986 void acceleration_read_LSM303D(void)
|
|
|
987 {
|
|
|
988 uint8_t data;
|
|
|
989 float xraw_f, yraw_f, zraw_f;
|
|
|
990 float accel_report_x, accel_report_y, accel_report_z;
|
|
|
991
|
|
|
992 memset(accDataBuffer,0,6);
|
|
|
993
|
|
|
994 accel_DX_f = 0;
|
|
|
995 accel_DY_f = 0;
|
|
|
996 accel_DZ_f = 0;
|
|
|
997
|
|
|
998 for(int i=0;i<6;i++)
|
|
|
999 {
|
|
|
1000 data = ADDR_OUT_X_L_A + i;
|
|
|
1001 I2C_Master_Transmit( DEVICE_COMPASS_303D, &data, 1);
|
|
|
1002 I2C_Master_Receive( DEVICE_COMPASS_303D, &accDataBuffer[i], 1);
|
|
|
1003 }
|
|
|
1004
|
|
|
1005 xraw_f = ((float)( (int16_t)((accDataBuffer[1] << 8) | (accDataBuffer[0]))));
|
|
|
1006 yraw_f = ((float)( (int16_t)((accDataBuffer[3] << 8) | (accDataBuffer[2]))));
|
|
|
1007 zraw_f = ((float)( (int16_t)((accDataBuffer[5] << 8) | (accDataBuffer[4]))));
|
|
|
1008
|
|
|
1009 rotate_accel_3f(&xraw_f, &yraw_f, &zraw_f);
|
|
|
1010
|
|
|
1011 // mh
|
|
|
1012 accel_report_x = xraw_f;
|
|
|
1013 accel_report_y = yraw_f;
|
|
|
1014 accel_report_z = zraw_f;
|
|
|
1015
|
|
|
1016 // my stuff
|
|
|
1017 /*
|
|
|
1018 accel_report_x = ((xraw_f * _accel_range_scale) - _accel_scale_x_offset) * _accel_scale_x_scale;
|
|
|
1019 accel_report_y = ((yraw_f * _accel_range_scale) - _accel_scale_y_offset) * _accel_scale_y_scale;
|
|
|
1020 accel_report_z = ((zraw_f * _accel_range_scale) - _accel_scale_z_offset) * _accel_scale_z_scale;
|
|
|
1021 */
|
|
|
1022 accel_DX_f = ((int16_t)(accel_report_x));
|
|
|
1023 accel_DY_f = ((int16_t)(accel_report_y));
|
|
|
1024 accel_DZ_f = ((int16_t)(accel_report_z));
|
|
|
1025 }
|
|
|
1026 /* special code after accel_report_z = ...
|
|
|
1027 * prior to output
|
|
|
1028 // we have logs where the accelerometers get stuck at a fixed
|
|
|
1029 // large value. We want to detect this and mark the sensor as
|
|
|
1030 // being faulty
|
|
|
1031
|
|
|
1032 if (fabsf(_last_accel[0] - x_in_new) < 0.001f &&
|
|
|
1033 fabsf(_last_accel[1] - y_in_new) < 0.001f &&
|
|
|
1034 fabsf(_last_accel[2] - z_in_new) < 0.001f &&
|
|
|
1035 fabsf(x_in_new) > 20 &&
|
|
|
1036 fabsf(y_in_new) > 20 &&
|
|
|
1037 fabsf(z_in_new) > 20) {
|
|
|
1038 _constant_accel_count += 1;
|
|
|
1039
|
|
|
1040 } else {
|
|
|
1041 _constant_accel_count = 0;
|
|
|
1042 }
|
|
|
1043
|
|
|
1044 if (_constant_accel_count > 100) {
|
|
|
1045 // we've had 100 constant accel readings with large
|
|
|
1046 // values. The sensor is almost certainly dead. We
|
|
|
1047 // will raise the error_count so that the top level
|
|
|
1048 // flight code will know to avoid this sensor, but
|
|
|
1049 // we'll still give the data so that it can be logged
|
|
|
1050 // and viewed
|
|
|
1051 perf_count(_bad_values);
|
|
|
1052 _constant_accel_count = 0;
|
|
|
1053 }
|
|
|
1054
|
|
|
1055 _last_accel[0] = x_in_new;
|
|
|
1056 _last_accel[1] = y_in_new;
|
|
|
1057 _last_accel[2] = z_in_new;
|
|
|
1058
|
|
|
1059 accel_report.x = _accel_filter_x.apply(x_in_new);
|
|
|
1060 accel_report.y = _accel_filter_y.apply(y_in_new);
|
|
|
1061 accel_report.z = _accel_filter_z.apply(z_in_new);
|
|
|
1062
|
|
|
1063 math::Vector<3> aval(x_in_new, y_in_new, z_in_new);
|
|
|
1064 math::Vector<3> aval_integrated;
|
|
|
1065
|
|
|
1066 bool accel_notify = _accel_int.put(accel_report.timestamp, aval, aval_integrated, accel_report.integral_dt);
|
|
|
1067 accel_report.x_integral = aval_integrated(0);
|
|
|
1068 accel_report.y_integral = aval_integrated(1);
|
|
|
1069 accel_report.z_integral = aval_integrated(2);
|
|
|
1070 */
|
|
|
1071
|
|
|
1072
|
|
|
1073 // ===============================================================================
|
|
|
1074 // compass_read_LSM303D
|
|
|
1075 /// @brief The new LSM303D, code by pixhawk
|
|
|
1076 ///
|
|
|
1077 /// output is compass_DX_f, compass_DY_f, compass_DZ_f
|
|
|
1078 // ===============================================================================
|
|
|
1079 void compass_read_LSM303D(void)
|
|
|
1080 {
|
|
|
1081 uint8_t data;
|
|
|
1082 // float xraw_f, yraw_f, zraw_f;
|
|
|
1083 // float mag_report_x, mag_report_y, mag_report_z;
|
|
|
1084
|
|
|
1085 memset(magDataBuffer,0,6);
|
|
|
1086
|
|
|
1087 compass_DX_f = 0;
|
|
|
1088 compass_DY_f = 0;
|
|
|
1089 compass_DZ_f = 0;
|
|
|
1090
|
|
|
1091 for(int i=0;i<6;i++)
|
|
|
1092 {
|
|
|
1093 data = ADDR_OUT_X_L_M + i;
|
|
|
1094 I2C_Master_Transmit( DEVICE_COMPASS_303D, &data, 1);
|
|
|
1095 I2C_Master_Receive( DEVICE_COMPASS_303D, &magDataBuffer[i], 1);
|
|
|
1096 }
|
|
|
1097
|
|
|
1098 // mh 160620 flip x and y if flip display
|
|
|
1099 compass_DX_f = (((int16_t)((magDataBuffer[1] << 8) | (magDataBuffer[0]))));
|
|
|
1100 compass_DY_f = (((int16_t)((magDataBuffer[3] << 8) | (magDataBuffer[2]))));
|
|
|
1101 compass_DZ_f = (((int16_t)((magDataBuffer[5] << 8) | (magDataBuffer[4]))));
|
|
|
1102 // no rotation
|
|
|
1103 return;
|
|
|
1104 /*
|
|
|
1105 // my stuff
|
|
|
1106 compass_DX_f = (((int16_t)((magDataBuffer[1] << 8) | (magDataBuffer[0]))) / 10) - 200;
|
|
|
1107 compass_DY_f = (((int16_t)((magDataBuffer[3] << 8) | (magDataBuffer[2]))) / 10) - 200;
|
|
|
1108 compass_DZ_f = (((int16_t)((magDataBuffer[5] << 8) | (magDataBuffer[4]))) / 10) - 200;
|
|
|
1109 */
|
|
|
1110 // old
|
|
|
1111 /*
|
|
|
1112 xraw_f = ((float)( (int16_t)((magDataBuffer[1] << 8) | (magDataBuffer[0]))));
|
|
|
1113 yraw_f = ((float)( (int16_t)((magDataBuffer[3] << 8) | (magDataBuffer[2]))));
|
|
|
1114 zraw_f = ((float)( (int16_t)((magDataBuffer[5] << 8) | (magDataBuffer[4]))));
|
|
|
1115
|
|
|
1116 rotate_mag_3f(&xraw_f, &yraw_f, &zraw_f);
|
|
|
1117
|
|
|
1118 compass_DX_f = (int16_t)((xraw_f * 0.1f) - 200.0f);
|
|
|
1119 compass_DY_f = (int16_t)((yraw_f * 0.1f) - 200.0f);
|
|
|
1120 compass_DZ_f = (int16_t)((zraw_f * 0.1f) - 200.0f);
|
|
|
1121 */
|
|
|
1122 /*
|
|
|
1123 mag_report_x = ((xraw_f * _mag_range_scale) - _mag_scale_x_offset) * _mag_scale_x_scale;
|
|
|
1124 mag_report_y = ((yraw_f * _mag_range_scale) - _mag_scale_y_offset) * _mag_scale_y_scale;
|
|
|
1125 mag_report_z = ((zraw_f * _mag_range_scale) - _mag_scale_z_offset) * _mag_scale_z_scale;
|
|
|
1126
|
|
|
1127 compass_DX_f = (int16_t)(mag_report_x * 1000.0f); // 1000.0 is just a wild guess by hw
|
|
|
1128 compass_DY_f = (int16_t)(mag_report_y * 1000.0f);
|
|
|
1129 compass_DZ_f = (int16_t)(mag_report_z * 1000.0f);
|
|
|
1130 */
|
|
|
1131 }
|
|
|
1132
|
|
|
1133
|
|
|
1134 // ===============================================================================
|
|
|
1135 // compass_init_LSM303DLHC
|
|
|
1136 /// @brief The new ST 303DLHC 2017/2018
|
|
|
1137 /// This might be called several times with different gain values during calibration
|
|
|
1138 /// but gain change is not supported at the moment.
|
|
|
1139 /// parts from KOMPASS LSM303DLH-compass-app-note.pdf
|
|
|
1140 ///
|
|
|
1141 /// @param gain:
|
|
|
1142 /// @param fast:
|
|
|
1143 // ===============================================================================
|
|
|
1144
|
|
|
1145
|
|
|
1146
|
|
|
1147 void compass_init_LSM303DLHC(uint8_t fast, uint8_t gain)
|
|
|
1148 {
|
|
|
1149 // acceleration
|
|
|
1150 // todo : BDU an (wie 303D) und high res, beides in REG4
|
|
|
1151 //LSM303D_write_checked_reg(DLHC_CTRL_REG2_A,0x00); // 0x00 default, hier könnte filter sein 0x8?80, cutoff freq. not beschrieben
|
|
|
1152
|
|
|
1153 if(fast == 0)
|
|
|
1154 {
|
|
|
1155 LSM303DLHC_accelerator_write_req(DLHC_CTRL_REG1_A, 0x27); // 10 hz
|
|
|
1156 }
|
|
|
1157 else
|
|
|
1158 {
|
|
|
1159 LSM303DLHC_accelerator_write_req(DLHC_CTRL_REG1_A, 0x57); // 100 hz
|
|
|
1160 }
|
|
|
1161 // LSM303D_write_checked_reg(DLHC_CTRL_REG4_A, 0x88); // 0x88: BDU + HighRes, BDU ist doof!
|
|
|
1162 LSM303D_write_checked_reg(DLHC_CTRL_REG4_A, 0x00); // 0x00 little-endian, ist's immer
|
|
|
1163 // LSM303D_write_checked_reg(DLHC_CTRL_REG4_A, 0x08); // 0x08: HighRes
|
|
|
1164 //LSM303D_write_checked_reg(DLHC_CTRL_REG4_A, 0x80); //
|
|
|
1165
|
|
|
1166
|
|
|
1167 // compass
|
|
|
1168 LSM303D_write_checked_reg(DLHC_CRA_REG_M,0x10); // 15 Hz
|
|
|
1169
|
|
|
1170 if(fast == 0)
|
|
|
1171 {
|
|
|
1172 LSM303D_write_checked_reg(DLHC_CRA_REG_M,0x10); // 15 Hz
|
|
|
1173 }
|
|
|
1174 else
|
|
|
1175 {
|
|
|
1176 LSM303D_write_checked_reg(DLHC_CRA_REG_M,0x18); // 75 Hz
|
|
|
1177 }
|
|
|
1178 LSM303D_write_checked_reg(DLHC_CRB_REG_M,0x20); // 0x60: 2.5 Gauss ,0x40: +/-1.9 Gauss,0x20: +/-1.3 Gauss
|
|
|
1179 LSM303D_write_checked_reg(DLHC_MR_REG_M,0x00); //continuous conversation
|
|
|
1180
|
|
|
1181
|
|
|
1182
|
|
|
1183 return;
|
|
|
1184
|
|
|
1185
|
|
|
1186 // LSM303D_write_checked_reg(,);
|
|
|
1187 // LSM303D_write_checked_reg(DLHC_CTRL_REG1_A, 0x27); // 0x27 = acc. normal mode with ODR 50Hz - passt nicht mit datenblatt!!
|
|
|
1188 // LSM303D_write_checked_reg(DLHC_CTRL_REG4_A, 0x40); // 0x40 = full scale range ±2 gauss in continuous data update mode and change the little-endian to a big-endian structure.
|
|
|
1189
|
|
|
1190 if(fast == 0)
|
|
|
1191 {
|
|
|
1192 LSM303DLHC_accelerator_write_req(DLHC_CTRL_REG1_A, 0x27); // 0x27 = acc. normal mode, all axes, with ODR 10HZ laut LSM303DLHC, page 25/42
|
|
|
1193 //
|
|
|
1194 //LSM303D_write_checked_reg(DLHC_CTRL_REG2_A,0x00); // 0x00 default, hier könnte filter sein 0x8?80, cutoff freq. not beschrieben
|
|
|
1195 //LSM303D_write_checked_reg(DLHC_CTRL_REG3_A,0x00); // 0x00 default
|
|
|
1196 //
|
|
|
1197 LSM303DLHC_accelerator_write_req(DLHC_CTRL_REG4_A, 0x00); // 0x00 = ich glaube little-endian ist gut
|
|
|
1198 // LSM303D_write_checked_reg(DLHC_CTRL_REG4_A, 0x40); // 0x00 = ich glaube little-endian ist gut
|
|
|
1199 //
|
|
|
1200 //LSM303D_write_checked_reg(DLHC_CTRL_REG5_A,0x00); // 0x00 default
|
|
|
1201 //LSM303D_write_checked_reg(DLHC_CTRL_REG6_A,0x00); // 0x00 default
|
|
|
1202 // magnetic sensor
|
|
|
1203 LSM303D_write_checked_reg(DLHC_CRA_REG_M,0x10); // 15 Hz
|
|
|
1204 }
|
|
|
1205 else
|
|
|
1206 {
|
|
|
1207 LSM303DLHC_accelerator_write_req(DLHC_CTRL_REG1_A, 0x57); // 0x57 = acc. normal mode, all axes, with ODR 100HZ, LSM303DLHC, page 25/42
|
|
|
1208 //
|
|
|
1209 //LSM303D_write_checked_reg(DLHC_CTRL_REG2_A,0x00); // 0x00 default, hier könnte filter sein 0x8?80, cutoff freq. not beschrieben
|
|
|
1210 //LSM303D_write_checked_reg(DLHC_CTRL_REG3_A,0x00); // 0x00 default
|
|
|
1211 //
|
|
|
1212 LSM303DLHC_accelerator_write_req(DLHC_CTRL_REG4_A, 0x00); // 0x00 = ich glaube little-endian ist gut
|
|
|
1213 // LSM303D_write_checked_reg(DLHC_CTRL_REG4_A, 0x40); // 0x00 = ich glaube little-endian ist gut
|
|
|
1214 //
|
|
|
1215 //LSM303D_write_checked_reg(DLHC_CTRL_REG5_A,0x00); // 0x00 default
|
|
|
1216 //LSM303D_write_checked_reg(DLHC_CTRL_REG6_A,0x00); // 0x00 default
|
|
|
1217 // magnetic sensor
|
|
|
1218 LSM303D_write_checked_reg(DLHC_CRA_REG_M,0x18); // 75 Hz
|
|
|
1219 }
|
|
|
1220 LSM303D_write_checked_reg(DLHC_CRB_REG_M,0x02); // +/-1.9 Gauss
|
|
|
1221 LSM303D_write_checked_reg(DLHC_MR_REG_M,0x00); //continuous conversation
|
|
|
1222
|
|
|
1223
|
|
|
1224 /*
|
|
|
1225 // matthias version 160620
|
|
|
1226 if(fast == 0)
|
|
|
1227 {
|
|
|
1228 LSM303D_write_checked_reg(ADDR_CTRL_REG0, 0x00);
|
|
|
1229 LSM303D_write_checked_reg(ADDR_CTRL_REG1, 0x3F); // mod 12,5 Hz 3 instead of 6,25 Hz 2
|
|
|
1230 LSM303D_write_checked_reg(ADDR_CTRL_REG2, 0xC0); // anti alias 50 Hz (minimum)
|
|
|
1231 LSM303D_write_checked_reg(ADDR_CTRL_REG3, 0x00);
|
|
|
1232 LSM303D_write_checked_reg(ADDR_CTRL_REG4, 0x00);
|
|
|
1233 LSM303D_write_checked_reg(ADDR_CTRL_REG5, 0x68); // mod 12,5 Hz 8 instead of 6,25 Hz 4
|
|
|
1234 }
|
|
|
1235 else
|
|
|
1236 {
|
|
|
1237 LSM303D_write_checked_reg(ADDR_CTRL_REG0, 0x00);
|
|
|
1238 LSM303D_write_checked_reg(ADDR_CTRL_REG1, 0x6F); // 100 Hz
|
|
|
1239 LSM303D_write_checked_reg(ADDR_CTRL_REG2, 0xC0);
|
|
|
1240 LSM303D_write_checked_reg(ADDR_CTRL_REG3, 0x00);
|
|
|
1241 LSM303D_write_checked_reg(ADDR_CTRL_REG4, 0x00);
|
|
|
1242 LSM303D_write_checked_reg(ADDR_CTRL_REG5, 0x74); // 100 Hz
|
|
|
1243 }
|
|
|
1244 LSM303D_write_checked_reg(ADDR_CTRL_REG6, 0x00);
|
|
|
1245 LSM303D_write_checked_reg(ADDR_CTRL_REG7, 0x00);
|
|
|
1246 */
|
|
|
1247 return;
|
|
|
1248 }
|
|
|
1249
|
|
|
1250 // ===============================================================================
|
|
|
1251 // compass_sleep_LSM303DLHC
|
|
|
1252 /// @brief The new 2017/2018 compass chip.
|
|
|
1253 // ===============================================================================
|
|
|
1254 void compass_sleep_LSM303DLHC(void)
|
|
|
1255 {
|
|
|
1256 LSM303DLHC_accelerator_write_req(DLHC_CTRL_REG1_A, 0x07); // CTRL_REG1_A: linear acceleration Power-down mode
|
|
|
1257 LSM303D_write_checked_reg(DLHC_MR_REG_M, 0x02); // MR_REG_M: magnetic sensor Power-down mode
|
|
|
1258 }
|
|
|
1259
|
|
|
1260
|
|
|
1261 // ===============================================================================
|
|
|
1262 // compass_read_LSM303DLHC
|
|
|
1263 /// @brief The new 2017/2018 compass chip.
|
|
|
1264 // ===============================================================================
|
|
|
1265 void compass_read_LSM303DLHC(void)
|
|
|
1266 {
|
|
|
1267 uint8_t data;
|
|
|
1268
|
|
|
1269 memset(magDataBuffer,0,6);
|
|
|
1270
|
|
|
1271 compass_DX_f = 0;
|
|
|
1272 compass_DY_f = 0;
|
|
|
1273 compass_DZ_f = 0;
|
|
|
1274
|
|
|
1275 for(int i=0;i<6;i++)
|
|
|
1276 {
|
|
|
1277 data = DLHC_OUT_X_L_M + i;
|
|
|
1278 I2C_Master_Transmit( DEVICE_COMPASS_303D, &data, 1);
|
|
|
1279 I2C_Master_Receive( DEVICE_COMPASS_303D, &magDataBuffer[i], 1);
|
|
|
1280 }
|
|
|
1281
|
|
|
1282 // 303DLHC new order
|
|
|
1283 compass_DX_f = (((int16_t)((magDataBuffer[0] << 8) | (magDataBuffer[1]))));
|
|
|
1284 compass_DZ_f = (((int16_t)((magDataBuffer[2] << 8) | (magDataBuffer[3]))));
|
|
|
1285 compass_DY_f = (((int16_t)((magDataBuffer[4] << 8) | (magDataBuffer[5]))));
|
|
|
1286
|
|
|
1287 // no rotation, otherwise see compass_read_LSM303D()
|
|
|
1288 return;
|
|
|
1289 }
|
|
|
1290
|
|
|
1291
|
|
|
1292 // ===============================================================================
|
|
|
1293 // acceleration_read_LSM303DLHC
|
|
|
1294 /// @brief The new 2017/2018 compass chip.
|
|
|
1295 // ===============================================================================
|
|
|
1296 void acceleration_read_LSM303DLHC(void)
|
|
|
1297 {
|
|
|
1298 uint8_t data;
|
|
|
1299 float xraw_f, yraw_f, zraw_f;
|
|
|
1300 float accel_report_x, accel_report_y, accel_report_z;
|
|
|
1301
|
|
|
1302 memset(accDataBuffer,0,6);
|
|
|
1303
|
|
|
1304 accel_DX_f = 0;
|
|
|
1305 accel_DY_f = 0;
|
|
|
1306 accel_DZ_f = 0;
|
|
|
1307
|
|
|
1308 for(int i=0;i<6;i++)
|
|
|
1309 {
|
|
|
1310 data = DLHC_OUT_X_L_A + i;
|
|
|
1311 I2C_Master_Transmit( DEVICE_ACCELARATOR_303DLHC, &data, 1);
|
|
|
1312 I2C_Master_Receive( DEVICE_ACCELARATOR_303DLHC, &accDataBuffer[i], 1);
|
|
|
1313 }
|
|
|
1314
|
|
|
1315 xraw_f = ((float)( (int16_t)((accDataBuffer[1] << 8) | (accDataBuffer[0]))));
|
|
|
1316 yraw_f = ((float)( (int16_t)((accDataBuffer[3] << 8) | (accDataBuffer[2]))));
|
|
|
1317 zraw_f = ((float)( (int16_t)((accDataBuffer[5] << 8) | (accDataBuffer[4]))));
|
|
|
1318
|
|
|
1319 rotate_accel_3f(&xraw_f, &yraw_f, &zraw_f);
|
|
|
1320
|
|
|
1321 // mh für 303D
|
|
|
1322 accel_report_x = xraw_f;
|
|
|
1323 accel_report_y = yraw_f;
|
|
|
1324 accel_report_z = zraw_f;
|
|
|
1325
|
|
|
1326 accel_DX_f = ((int16_t)(accel_report_x));
|
|
|
1327 accel_DY_f = ((int16_t)(accel_report_y));
|
|
|
1328 accel_DZ_f = ((int16_t)(accel_report_z));
|
|
|
1329 }
|
|
|
1330
|
|
|
1331
|
|
|
1332 // --------------------------------------------------------------------------------
|
|
|
1333 // ----------EARLIER COMPONENTS ---------------------------------------------------
|
|
|
1334 // --------------------------------------------------------------------------------
|
|
|
1335
|
|
|
1336 // ===============================================================================
|
|
|
1337 // compass_init_HMC5883L
|
|
|
1338 /// @brief The horrible Honeywell compass chip
|
|
|
1339 /// This might be called several times during calibration
|
|
|
1340 ///
|
|
|
1341 /// @param fast: 1 is fast mode, 0 is normal mode
|
|
|
1342 /// @param gain: 7 is max gain and set with here, compass_calib() might reduce it
|
|
|
1343 // ===============================================================================
|
|
|
1344 void compass_init_HMC5883L(uint8_t fast, uint8_t gain)
|
|
|
1345 {
|
|
|
1346 uint8_t write_buffer[4];
|
|
|
1347
|
|
|
1348 compass_gain = gain;
|
|
|
1349 uint16_t length = 0;
|
|
|
1350 write_buffer[0] = 0x00; // 00 = config Register A
|
|
|
1351
|
|
|
1352 if( fast )
|
|
|
1353 write_buffer[1] = 0x38; // 0b 0011 1000; // ConfigA: 75Hz, 2 Samples averaged
|
|
|
1354 else
|
|
|
1355 write_buffer[1] = 0x68; // 0b 0110 1000; // ConfigA: 3Hz, 8 Samples averaged
|
|
|
1356
|
|
|
1357 switch(gain)
|
|
|
1358 {
|
|
|
1359 case 7:
|
|
|
1360 write_buffer[2] = 0xE0; //0b 1110 0000; // ConfigB: gain
|
|
|
1361 break;
|
|
|
1362 case 6:
|
|
|
1363 write_buffer[2] = 0xC0; //0b 1100 0000; // ConfigB: gain
|
|
|
1364 break;
|
|
|
1365 case 5:
|
|
|
1366 write_buffer[2] = 0xA0; //0b 1010 0000; // ConfigB: gain
|
|
|
1367 break;
|
|
|
1368 case 4:
|
|
|
1369 write_buffer[2] = 0x80; //0b 1000 0000; // ConfigB: gain
|
|
|
1370 break;
|
|
|
1371 case 3:
|
|
|
1372 write_buffer[2] = 0x60; //0b 0110 0000; // ConfigB: gain
|
|
|
1373 break;
|
|
|
1374 case 2:
|
|
|
1375 write_buffer[2] = 0x40; //0b 01000 0000; // ConfigB: gain
|
|
|
1376 break;
|
|
|
1377 case 1:
|
|
|
1378 write_buffer[2] = 0x20; //0b 00100 0000; // ConfigB: gain
|
|
|
1379 break;
|
|
|
1380 case 0:
|
|
|
1381 write_buffer[2] = 0x00; //0b 00000 0000; // ConfigB: gain
|
|
|
1382 break;
|
|
|
1383 }
|
|
|
1384 write_buffer[3] = 0x00; // Mode: continuous mode
|
|
|
1385 length = 4;
|
|
|
1386 //hmc_twi_write(0);
|
|
|
1387 I2C_Master_Transmit( DEVICE_COMPASS_HMC5883L, write_buffer, length);
|
|
|
1388 }
|
|
|
1389
|
|
|
1390
|
|
|
1391
|
|
|
1392 // ===============================================================================
|
|
|
1393 // compass_sleep_HMC5883L
|
|
|
1394 /// @brief Power-down mode for Honeywell compass chip
|
|
|
1395 // ===============================================================================
|
|
|
1396 void compass_sleep_HMC5883L(void)
|
|
|
1397 {
|
|
|
1398 uint8_t write_buffer[4];
|
|
|
1399 uint16_t length = 0;
|
|
|
1400
|
|
|
1401 write_buffer[0] = 0x00; // 00 = config Register A
|
|
|
1402 write_buffer[1] = 0x68; // 0b 0110 1000; // ConfigA
|
|
|
1403 write_buffer[2] = 0x20; // 0b 0010 0000; // ConfigB
|
|
|
1404 write_buffer[3] = 0x02; // 0b 0000 0010; // Idle Mode
|
|
|
1405 length = 4;
|
|
|
1406 I2C_Master_Transmit( DEVICE_COMPASS_HMC5883L, write_buffer, length);
|
|
|
1407 }
|
|
|
1408
|
|
|
1409
|
|
|
1410 // ===============================================================================
|
|
|
1411 // accelerator_init_MMA8452Q
|
|
|
1412 /// @brief Power-down mode for acceleration chip used in combination with Honeywell compass
|
|
|
1413 // ===============================================================================
|
|
|
1414 void accelerator_init_MMA8452Q(void)
|
|
|
1415 {
|
|
|
1416 uint8_t write_buffer[4];
|
|
|
1417 uint16_t length = 0;
|
|
|
1418 //HAL_Delay(1);
|
|
|
1419 //return;
|
|
|
1420 write_buffer[0] = 0x0E; // XYZ_DATA_CFG
|
|
|
1421 write_buffer[1] = 0x00;//0b00000000; // High pass Filter=0 , +/- 2g range
|
|
|
1422 length = 2;
|
|
|
1423 I2C_Master_Transmit( DEVICE_ACCELARATOR_MMA8452Q, write_buffer, length);
|
|
|
1424 //HAL_Delay(1);
|
|
|
1425 write_buffer[0] = 0x2A; // CTRL_REG1
|
|
|
1426 write_buffer[1] = 0x34; //0b00110100; // CTRL_REG1: 160ms data rate, St.By Mode, reduced noise mode
|
|
|
1427 write_buffer[2] = 0x02; //0b00000010; // CTRL_REG2: High Res in Active mode
|
|
|
1428 length = 3;
|
|
|
1429 I2C_Master_Transmit( DEVICE_ACCELARATOR_MMA8452Q, write_buffer, length);
|
|
|
1430
|
|
|
1431 //HAL_Delay(1);
|
|
|
1432 //hw_delay_us(100);
|
|
|
1433 write_buffer[0] = 0x2A; // CTRL_REG1
|
|
|
1434 write_buffer[1] = 0x35; //0b00110101; // CTRL_REG1: ... Active Mode
|
|
|
1435 length = 2;
|
|
|
1436 I2C_Master_Transmit( DEVICE_ACCELARATOR_MMA8452Q, write_buffer, length);
|
|
|
1437 /*
|
|
|
1438 HAL_Delay(6);
|
|
|
1439 compass_read();
|
|
|
1440 HAL_Delay(1);
|
|
|
1441 acceleration_read();
|
|
|
1442
|
|
|
1443 compass_calc();
|
|
|
1444 */
|
|
|
1445 }
|
|
|
1446
|
|
|
1447
|
|
|
1448 // ===============================================================================
|
|
|
1449 // accelerator_sleep_MMA8452Q
|
|
|
1450 /// @brief compass_sleep_HMC5883L
|
|
|
1451 // ===============================================================================
|
|
|
1452 void accelerator_sleep_MMA8452Q(void)
|
|
|
1453 {
|
|
|
1454 uint16_t length = 0;
|
|
|
1455 uint8_t write_buffer[4];
|
|
|
1456
|
|
|
1457 write_buffer[0] = 0x2A; // CTRL_REG1
|
|
|
1458 write_buffer[1] = 0x00; //0b00000000; // CTRL_REG1: Standby Mode
|
|
|
1459 length = 2;
|
|
|
1460 I2C_Master_Transmit( DEVICE_ACCELARATOR_MMA8452Q, write_buffer, length);
|
|
|
1461 }
|
|
|
1462
|
|
|
1463
|
|
|
1464 // ===============================================================================
|
|
|
1465 // compass_read_HMC5883L
|
|
|
1466 /// @brief The new ST 303D - get ALL data and store in static variables
|
|
|
1467 ///
|
|
|
1468 /// output is compass_DX_f, compass_DY_f, compass_DZ_f
|
|
|
1469 // ===============================================================================
|
|
|
1470 void compass_read_HMC5883L(void)
|
|
|
1471 {
|
|
|
1472 uint8_t buffer[20];
|
|
|
1473 compass_DX_f = 0;
|
|
|
1474 compass_DY_f = 0;
|
|
|
1475 compass_DZ_f = 0;
|
|
|
1476 uint8_t length = 0;
|
|
|
1477 uint8_t read_buffer[6];
|
|
|
1478 signed_tword data;
|
|
|
1479 for(int i = 0; i<6;i++)
|
|
|
1480 read_buffer[i] = 0;
|
|
|
1481 buffer[0] = 0x03; // 03 = Data Output X MSB Register
|
|
|
1482 length = 1;
|
|
|
1483 I2C_Master_Transmit( DEVICE_COMPASS_HMC5883L, buffer, length);
|
|
|
1484 length = 6;
|
|
|
1485 I2C_Master_Receive( DEVICE_COMPASS_HMC5883L, read_buffer, length);
|
|
|
1486
|
|
|
1487
|
|
|
1488 data.Byte.hi = read_buffer[0];
|
|
|
1489 data.Byte.low = read_buffer[1];
|
|
|
1490 //Y = Z
|
|
|
1491 compass_DY_f = - data.Word;
|
|
|
1492
|
|
|
1493 data.Byte.hi = read_buffer[2];
|
|
|
1494 data.Byte.low = read_buffer[3];
|
|
|
1495 compass_DZ_f = data.Word;
|
|
|
1496
|
|
|
1497 data.Byte.hi = read_buffer[4];
|
|
|
1498 data.Byte.low = read_buffer[5];
|
|
|
1499 //X = -Y
|
|
|
1500 compass_DX_f = data.Word;
|
|
|
1501 }
|
|
|
1502
|
|
|
1503
|
|
|
1504 // ===============================================================================
|
|
|
1505 // acceleration_read_MMA8452Q
|
|
|
1506 /// @brief The old MMA8452Q used with the Honeywell compass
|
|
|
1507 /// get the acceleration data and store in static variables
|
|
|
1508 ///
|
|
|
1509 /// output is accel_DX_f, accel_DY_f, accel_DZ_f
|
|
|
1510 // ===============================================================================
|
|
|
1511 void acceleration_read_MMA8452Q(void)
|
|
|
1512 {
|
|
|
1513 uint8_t buffer[20];
|
|
|
1514 accel_DX_f = 0;
|
|
|
1515 accel_DY_f = 0;
|
|
|
1516 accel_DZ_f = 0;
|
|
|
1517 uint8_t length = 0;
|
|
|
1518 // bit8_Type status ;
|
|
|
1519 uint8_t read_buffer[7];
|
|
|
1520 signed_tword data;
|
|
|
1521 for(int i = 0; i<6;i++)
|
|
|
1522 read_buffer[i] = 0;
|
|
|
1523 buffer[0] = 0x00; // 03 = Data Output X MSB Register
|
|
|
1524 length = 1;
|
|
|
1525 I2C_Master_Transmit( DEVICE_ACCELARATOR_MMA8452Q, buffer, length);
|
|
|
1526 length = 7;
|
|
|
1527 I2C_Master_Receive( DEVICE_ACCELARATOR_MMA8452Q, read_buffer, length);
|
|
|
1528
|
|
|
1529 // status.uw = read_buffer[0];
|
|
|
1530 data.Byte.hi = read_buffer[1];
|
|
|
1531 data.Byte.low = read_buffer[2];
|
|
|
1532 accel_DX_f =data.Word/16;
|
|
|
1533 data.Byte.hi = read_buffer[3];
|
|
|
1534 data.Byte.low = read_buffer[4];
|
|
|
1535 accel_DY_f =data.Word/16;
|
|
|
1536 data.Byte.hi = read_buffer[5];
|
|
|
1537 data.Byte.low = read_buffer[6];
|
|
|
1538 accel_DZ_f =data.Word/16;
|
|
|
1539
|
|
|
1540 accel_DX_f *= -1;
|
|
|
1541 accel_DY_f *= -1;
|
|
|
1542 accel_DZ_f *= -1;
|
|
|
1543 }
|
|
|
1544
|
|
|
1545
|
|
|
1546 // ===============================================================================
|
|
|
1547 // compass_calc_roll_pitch_only
|
|
|
1548 /// @brief only the roll and pitch parts of compass_calc()
|
|
|
1549 ///
|
|
|
1550 /// input is accel_DX_f, accel_DY_f, accel_DZ_f
|
|
|
1551 /// output is compass_pitch and compass_roll
|
|
|
1552 // ===============================================================================
|
|
|
1553 void compass_calc_roll_pitch_only(void)
|
|
|
1554 {
|
|
|
1555 float sinPhi, cosPhi;
|
|
|
1556 float Phi, Teta;
|
|
|
1557
|
|
|
1558 //---- Calculate sine and cosine of roll angle Phi -----------------------
|
|
|
1559 Phi= atan2f(accel_DY_f, accel_DZ_f) ;
|
|
|
1560 compass_roll = Phi * 180.0f /PI;
|
|
|
1561 sinPhi = sinf(Phi);
|
|
|
1562 cosPhi = cosf(Phi);
|
|
|
1563
|
|
|
1564 //---- calculate sin and cosine of pitch angle Theta ---------------------
|
|
|
1565 Teta = atanf(-(float)accel_DX_f/(accel_DY_f * sinPhi + accel_DZ_f * cosPhi));
|
|
|
1566 compass_pitch = Teta * 180.0f /PI;
|
|
|
1567 }
|
|
|
1568
|
|
|
1569
|
|
|
1570 // ===============================================================================
|
|
|
1571 // compass_calc
|
|
|
1572 /// @brief all the fancy stuff first implemented in OSTC3
|
|
|
1573 ///
|
|
|
1574 /// input is compass_DX_f, compass_DY_f, compass_DZ_f, accel_DX_f, accel_DY_f, accel_DZ_f
|
|
|
1575 /// and compass_CX_f, compass_CY_f, compass_CZ_f
|
|
|
1576 /// output is compass_heading, compass_pitch and compass_roll
|
|
|
1577 // ===============================================================================
|
|
|
1578 void compass_calc(void)
|
|
|
1579 {
|
|
|
1580 float sinPhi, cosPhi, sinTeta, cosTeta;
|
|
|
1581 float Phi, Teta, Psi;
|
|
|
1582 int16_t iBfx, iBfy;
|
|
|
1583 int16_t iBpx, iBpy, iBpz;
|
|
|
1584
|
|
|
1585 //---- Make hard iron correction -----------------------------------------
|
|
|
1586 // Measured magnetometer orientation, measured ok.
|
|
|
1587 // From matthias drawing: (X,Y,Z) --> (X,Y,Z) : no rotation.
|
|
|
1588 iBpx = compass_DX_f - compass_CX_f; // X
|
|
|
1589 iBpy = compass_DY_f - compass_CY_f; // Y
|
|
|
1590 iBpz = compass_DZ_f - compass_CZ_f; // Z
|
|
|
1591
|
|
|
1592 //---- Calculate sine and cosine of roll angle Phi -----------------------
|
|
|
1593 //sincos(accel_DZ_f, accel_DY_f, &sin, &cos);
|
|
|
1594 Phi= atan2f(accel_DY_f, accel_DZ_f) ;
|
|
|
1595 compass_roll = Phi * 180.0f /PI;
|
|
|
1596 sinPhi = sinf(Phi);
|
|
|
1597 cosPhi = cosf(Phi);
|
|
|
1598
|
|
|
1599 //---- rotate by roll angle (-Phi) ---------------------------------------
|
|
|
1600 iBfy = iBpy * cosPhi - iBpz * sinPhi;
|
|
|
1601 iBpz = iBpy * sinPhi + iBpz * cosPhi;
|
|
|
1602 //Gz = imul(accel_DY_f, sin) + imul(accel_DZ_f, cos);
|
|
|
1603
|
|
|
1604 //---- calculate sin and cosine of pitch angle Theta ---------------------
|
|
|
1605 //sincos(Gz, -accel_DX_f, &sin, &cos); // NOTE: changed sin sign.
|
|
|
1606 // Teta takes into account roll of computer and sends combination of Y and Z :-) understand now hw 160421
|
|
|
1607 Teta = atanf(-(float)accel_DX_f/(accel_DY_f * sinPhi + accel_DZ_f * cosPhi));
|
|
|
1608 compass_pitch = Teta * 180.0f /PI;
|
|
|
1609 sinTeta = sinf(Teta);
|
|
|
1610 cosTeta = cosf(Teta);
|
|
|
1611 /* correct cosine if pitch not in range -90 to 90 degrees */
|
|
|
1612 if( cosTeta < 0 ) cosTeta = -cosTeta;
|
|
|
1613
|
|
|
1614 ///---- de-rotate by pitch angle Theta -----------------------------------
|
|
|
1615 iBfx = iBpx * cosTeta + iBpz * sinTeta;
|
|
|
1616
|
|
|
1617 //---- Detect uncalibrated compass ---------------------------------------
|
|
|
1618 if( !compass_CX_f && !compass_CY_f && !compass_CZ_f )
|
|
|
1619 {
|
|
|
1620 compass_heading = -1;
|
|
|
1621 return;
|
|
|
1622 }
|
|
|
1623
|
|
|
1624 //---- calculate current yaw = e-compass angle Psi -----------------------
|
|
|
1625 // Result in degree (no need of 0.01 deg precision...
|
|
|
1626 Psi = atan2f(-iBfy,iBfx);
|
|
|
1627 compass_heading = Psi * 180.0f /PI;
|
|
|
1628 // Result in 0..360 range:
|
|
|
1629 if( compass_heading < 0 )
|
|
|
1630 compass_heading += 360;
|
|
|
1631 }
|
|
|
1632
|
|
|
1633
|
|
|
1634 /*
|
|
|
1635 // ===============================================================================
|
|
|
1636 // compass_calc_mini_during_calibration
|
|
|
1637 /// @brief all the fancy stuff first implemented in OSTC3
|
|
|
1638 ///
|
|
|
1639 /// input is accel_DX_f, accel_DY_f, accel_DZ_f
|
|
|
1640 /// output is compass_pitch and compass_roll
|
|
|
1641 // ===============================================================================
|
|
|
1642 void compass_calc_mini_during_calibration(void)
|
|
|
1643 {
|
|
|
1644 float sinPhi, cosPhi;
|
|
|
1645 float Phi, Teta;
|
|
|
1646
|
|
|
1647 //---- Calculate sine and cosine of roll angle Phi -----------------------
|
|
|
1648 //sincos(accel_DZ_f, accel_DY_f, &sin, &cos);
|
|
|
1649 Phi= atan2f(accel_DY_f, accel_DZ_f) ;
|
|
|
1650 compass_roll = Phi * 180.0f /PI;
|
|
|
1651 sinPhi = sinf(Phi);
|
|
|
1652 cosPhi = cosf(Phi);
|
|
|
1653
|
|
|
1654 //---- calculate sin and cosine of pitch angle Theta ---------------------
|
|
|
1655 //sincos(Gz, -accel_DX_f, &sin, &cos); // NOTE: changed sin sign.
|
|
|
1656 Teta = atanf(-(float)accel_DX_f/(accel_DY_f * sinPhi + accel_DZ_f * cosPhi));
|
|
|
1657 compass_pitch = Teta * 180.0f /PI;
|
|
|
1658 }
|
|
|
1659 */
|
|
|
1660
|
|
|
1661
|
|
|
1662 // //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
|
|
|
1663 // // - Calibration - ///////////////////////////////////////////////////////////////////////////////////////////////////////
|
|
|
1664 // //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
|
|
|
1665
|
|
|
1666 /* can be lost during sleep as those are reset with compass_reset_calibration() */
|
|
|
1667
|
|
|
1668 // ===============================================================================
|
|
|
1669 // compass_reset_calibration
|
|
|
1670 /// @brief all the fancy stuff first implemented in OSTC3
|
|
|
1671 ///
|
|
|
1672 /// output is struct g and compass_CX_f, compass_CY_f, compass_CZ_f
|
|
|
1673 ///
|
|
|
1674 /// @param g: is a struct with crazy stuff like Suuu, Svvv, Svvu, etc.
|
|
|
1675 /// all is set to zero here
|
|
|
1676 // ===============================================================================
|
|
|
1677 void compass_reset_calibration(SCompassCalib *g)
|
|
|
1678 {
|
|
|
1679 g->compass_N = 0;
|
|
|
1680 g->Su = g->Sv = g->Sw = 0.0;
|
|
|
1681 g->Suu = g->Svv = g->Sww = g->Suv = g->Suw = g->Svw = 0.0;
|
|
|
1682 g->Suuu = g->Svvv = g->Swww = 0.0;
|
|
|
1683 g->Suuv = g->Suuw = g->Svvu = g->Svvw = g->Swwu = g->Swwv = 0.0;
|
|
|
1684 compass_CX_f = compass_CY_f = compass_CZ_f = 0.0;
|
|
|
1685 }
|
|
|
1686
|
|
|
1687
|
|
|
1688 // ===============================================================================
|
|
|
1689 // compass_add_calibration
|
|
|
1690 /// @brief all the fancy stuff first implemented in OSTC3
|
|
|
1691 ///
|
|
|
1692 /// input is compass_DX_f, compass_DY_f, compass_DZ_f
|
|
|
1693 /// and compass_CX_f, compass_CY_f, compass_CZ_f
|
|
|
1694 /// output is struct g
|
|
|
1695 ///
|
|
|
1696 /// @param g: is a struct with crazy stuff like Suuu, Svvv, Svvu, etc.
|
|
|
1697 // ===============================================================================
|
|
|
1698 void compass_add_calibration(SCompassCalib *g)
|
|
|
1699 {
|
|
|
1700 float u, v, w;
|
|
|
1701
|
|
|
1702 u = (compass_DX_f - compass_CX_f) / 32768.0f;
|
|
|
1703 v = (compass_DY_f - compass_CY_f) / 32768.0f;
|
|
|
1704 w = (compass_DZ_f - compass_CZ_f) / 32768.0f;
|
|
|
1705
|
|
|
1706 g->compass_N++;
|
|
|
1707 g->Su += u;
|
|
|
1708 g->Sv += v;
|
|
|
1709 g->Sw += w;
|
|
|
1710 g->Suv += u*v;
|
|
|
1711 g->Suw += u*w;
|
|
|
1712 g->Svw += v*w;
|
|
|
1713 g->Suu += u*u;
|
|
|
1714 g->Suuu += u*u*u;
|
|
|
1715 g->Suuv += v*u*u;
|
|
|
1716 g->Suuw += w*u*u;
|
|
|
1717 g->Svv += v*v;
|
|
|
1718 g->Svvv += v*v*v;
|
|
|
1719 g->Svvu += u*v*v;
|
|
|
1720 g->Svvw += w*v*v;
|
|
|
1721 g->Sww += w*w;
|
|
|
1722 g->Swww += w*w*w;
|
|
|
1723 g->Swwu += u*w*w;
|
|
|
1724 g->Swwv += v*w*w;
|
|
|
1725 }
|
|
|
1726
|
|
|
1727 //////////////////////////////////////////////////////////////////////////////
|
|
|
1728
|
|
|
1729 // ===============================================================================
|
|
|
1730 // compass_solve_calibration
|
|
|
1731 /// @brief all the fancy stuff first implemented in OSTC3
|
|
|
1732 ///
|
|
|
1733 /// input is compass_CX_f, compass_CY_f, compass_CZ_f and g
|
|
|
1734 /// output is struct g
|
|
|
1735 ///
|
|
|
1736 /// @param g: is a struct with crazy stuff like Suuu, Svvv, Svvu, etc.
|
|
|
1737 // ===============================================================================
|
|
|
1738 void compass_solve_calibration(SCompassCalib *g)
|
|
|
1739 {
|
|
|
1740 float yu, yv, yw;
|
|
|
1741 float delta;
|
|
|
1742 float uc, vc, wc;
|
|
|
1743
|
|
|
1744
|
|
|
1745 //---- Normalize partial sums --------------------------------------------
|
|
|
1746 //
|
|
|
1747 // u, v, w should be centered on the mean value um, vm, wm:
|
|
|
1748 // x = u + um, with um = Sx/N
|
|
|
1749 //
|
|
|
1750 // So:
|
|
|
1751 // (u + um)**2 = u**2 + 2u*um + um**2
|
|
|
1752 // Su = 0, um = Sx/N
|
|
|
1753 // Sxx = Suu + 2 um Su + N*(Sx/N)**2 = Suu + Sx**2/N
|
|
|
1754 // Suu = Sxx - Sx**2/N
|
|
|
1755 yu = g->Su/g->compass_N;
|
|
|
1756 yv = g->Sv/g->compass_N;
|
|
|
1757 yw = g->Sw/g->compass_N;
|
|
|
1758
|
|
|
1759 g->Suu -= g->Su*yu;
|
|
|
1760 g->Svv -= g->Sv*yv;
|
|
|
1761 g->Sww -= g->Sw*yw;
|
|
|
1762
|
|
|
1763 // (u + um)(v + vm) = uv + u vm + v um + um vm
|
|
|
1764 // Sxy = Suv + N * um vm
|
|
|
1765 // Suv = Sxy - N * (Sx/N)(Sy/N);
|
|
|
1766 g->Suv -= g->Su*yv;
|
|
|
1767 g->Suw -= g->Su*yw;
|
|
|
1768 g->Svw -= g->Sv*yw;
|
|
|
1769
|
|
|
1770 // (u + um)**3 = u**3 + 3 u**2 um + 3 u um**2 + um**3
|
|
|
1771 // Sxxx = Suuu + 3 um Suu + 3 um**2 Su + N.um**3
|
|
|
1772 // Su = 0, um = Sx/N:
|
|
|
1773 // Suuu = Sxxx - 3 Sx*Suu/N - N.(Sx/N)**3
|
|
|
1774 // = Sxxx - 3 Sx*Suu/N - Sx**3/N**2
|
|
|
1775
|
|
|
1776 // (u + um)**2 (v + vm) = (u**2 + 2 u um + um**2)(v + vm)
|
|
|
1777 // Sxxy = Suuv + vm Suu + 2 um (Suv + vm Su) + um**2 (Sv + N.vm)
|
|
|
1778 //
|
|
|
1779 // Su = 0, Sv = 0, vm = Sy/N:
|
|
|
1780 // Sxxy = Suuv + vm Suu + 2 um Suv + N um**2 vm
|
|
|
1781 //
|
|
|
1782 // Suuv = Sxxy - (Sy/N) Suu - 2 (Sx/N) Suv - (Sx/N)**2 Sy
|
|
|
1783 // = Sxxy - Suu*Sy/N - 2 Suv*Sx/N - Sx*Sx*Sy/N/N
|
|
|
1784 // = Sxxy - (Suu + Sx*Sx/N)*Sy/N - 2 Suv*Sx/N
|
|
|
1785 g->Suuu -= (3*g->Suu + g->Su*yu)*yu;
|
|
|
1786 g->Suuv -= (g->Suu + g->Su*yu)*yv + 2*g->Suv*yu;
|
|
|
1787 g->Suuw -= (g->Suu + g->Su*yu)*yw + 2*g->Suw*yu;
|
|
|
1788
|
|
|
1789 g->Svvu -= (g->Svv + g->Sv*yv)*yu + 2*g->Suv*yv;
|
|
|
1790 g->Svvv -= (3*g->Svv + g->Sv*yv)*yv;
|
|
|
1791 g->Svvw -= (g->Svv + g->Sv*yv)*yw + 2*g->Svw*yv;
|
|
|
1792
|
|
|
1793 g->Swwu -= (g->Sww + g->Sw*yw)*yu + 2*g->Suw*yw;
|
|
|
1794 g->Swwv -= (g->Sww + g->Sw*yw)*yv + 2*g->Svw*yw;
|
|
|
1795 g->Swww -= (3*g->Sww + g->Sw*yw)*yw;
|
|
|
1796
|
|
|
1797 //---- Solve the system --------------------------------------------------
|
|
|
1798 // uc Suu + vc Suv + wc Suw = (Suuu + Svvu + Swwu) / 2
|
|
|
1799 // uc Suv + vc Svv + wc Svw = (Suuv + Svvv + Swwv) / 2
|
|
|
1800 // uc Suw + vc Svw + wc Sww = (Suuw + Svvw + Swww) / 2
|
|
|
1801 // Note this is symetric, with a positiv diagonal, hence
|
|
|
1802 // it always have a uniq solution.
|
|
|
1803 yu = 0.5f * (g->Suuu + g->Svvu + g->Swwu);
|
|
|
1804 yv = 0.5f * (g->Suuv + g->Svvv + g->Swwv);
|
|
|
1805 yw = 0.5f * (g->Suuw + g->Svvw + g->Swww);
|
|
|
1806 delta = g->Suu * (g->Svv * g->Sww - g->Svw * g->Svw)
|
|
|
1807 - g->Suv * (g->Suv * g->Sww - g->Svw * g->Suw)
|
|
|
1808 + g->Suw * (g->Suv * g->Svw - g->Svv * g->Suw);
|
|
|
1809
|
|
|
1810 uc = (yu * (g->Svv * g->Sww - g->Svw * g->Svw)
|
|
|
1811 - yv * (g->Suv * g->Sww - g->Svw * g->Suw)
|
|
|
1812 + yw * (g->Suv * g->Svw - g->Svv * g->Suw) )/delta;
|
|
|
1813 vc = (g->Suu * ( yv * g->Sww - yw * g->Svw)
|
|
|
1814 - g->Suv * ( yu * g->Sww - yw * g->Suw)
|
|
|
1815 + g->Suw * ( yu * g->Svw - yv * g->Suw) )/delta;
|
|
|
1816 wc = (g->Suu * (g->Svv * yw - g->Svw * yv )
|
|
|
1817 - g->Suv * (g->Suv * yw - g->Svw * yu )
|
|
|
1818 + g->Suw * (g->Suv * yv - g->Svv * yu ) )/delta;
|
|
|
1819
|
|
|
1820 // Back to uncentered coordinates:
|
|
|
1821 // xc = um + uc
|
|
|
1822 uc = g->Su/g->compass_N + compass_CX_f/32768.0f + uc;
|
|
|
1823 vc = g->Sv/g->compass_N + compass_CY_f/32768.0f + vc;
|
|
|
1824 wc = g->Sw/g->compass_N + compass_CZ_f/32768.0f + wc;
|
|
|
1825
|
|
|
1826 // Then save the new calibrated center:
|
|
|
1827 compass_CX_f = (short)(32768 * uc);
|
|
|
1828 compass_CY_f = (short)(32768 * vc);
|
|
|
1829 compass_CZ_f = (short)(32768 * wc);
|
|
|
1830 }
|
|
|
1831
|
|
|
1832
|
|
|
1833 // ===============================================================================
|
|
|
1834 // compass_calib
|
|
|
1835 /// @brief the main loop for calibration
|
|
|
1836 /// output is compass_CX_f, compass_CY_f, compass_CZ_f and g
|
|
|
1837 /// 160704 removed -4096 limit for LSM303D
|
|
|
1838 ///
|
|
|
1839 /// @return always 0
|
|
|
1840 // ===============================================================================
|
|
|
1841 int compass_calib_common(void)
|
|
|
1842 {
|
|
|
1843 SCompassCalib g;
|
|
|
1844
|
|
|
1845 // Starts with no calibration at all:
|
|
|
1846 compass_reset_calibration(&g);
|
|
|
1847
|
|
|
1848 int64_t tickstart = 0;
|
|
|
1849 uint32_t ticks = 0;
|
|
|
1850 uint32_t lasttick = 0;
|
|
|
1851 tickstart = HAL_GetTick();
|
|
|
1852 // Eine Minute kalibrieren
|
|
|
1853 while((ticks) < 60 * 1000)
|
|
|
1854 {
|
|
|
1855 compass_read();
|
|
|
1856
|
|
|
1857 acceleration_read();
|
|
|
1858 compass_calc_roll_pitch_only();
|
|
|
1859
|
|
|
1860 if((hardwareCompass == HMC5883L)
|
|
|
1861 &&((compass_DX_f == -4096) ||
|
|
|
1862 (compass_DY_f == -4096) ||
|
|
|
1863 (compass_DZ_f == -4096) ))
|
|
|
1864 {
|
|
|
1865 if(compass_gain == 0)
|
|
|
1866 return -1;
|
|
|
1867 compass_gain--;
|
|
|
1868
|
|
|
1869 compass_init(1, compass_gain);
|
|
|
1870 compass_reset_calibration(&g);
|
|
|
1871 //tickstart = HAL_GetTick();
|
|
|
1872 continue;
|
|
|
1873 }
|
|
|
1874
|
|
|
1875 copyCompassDataDuringCalibration(compass_DX_f,compass_DY_f,compass_DZ_f);
|
|
|
1876 compass_add_calibration(&g);
|
|
|
1877 HAL_Delay(1);
|
|
|
1878 lasttick = HAL_GetTick();
|
|
|
1879 if(lasttick == 0)
|
|
|
1880 {
|
|
|
1881 tickstart = -ticks;
|
|
|
1882 }
|
|
|
1883 ticks = lasttick - tickstart;
|
|
|
1884 }
|
|
|
1885
|
|
|
1886 compass_solve_calibration(&g);
|
|
|
1887
|
|
|
1888 tfull32 dataBlock[4];
|
|
|
1889 dataBlock[0].Word16.low16 = compass_CX_f;
|
|
|
1890 dataBlock[0].Word16.hi16 = compass_CY_f;
|
|
|
1891 dataBlock[1].Word16.low16 = compass_CZ_f;
|
|
|
1892 dataBlock[1].Word16.hi16 = 0xFFFF;
|
|
|
1893 dataBlock[2].Full32 = 0x7FFFFFFF;
|
|
|
1894 dataBlock[3].Full32 = 0x7FFFFFFF;
|
|
|
1895 BFA_writeDataBlock((uint32_t *)dataBlock);
|
|
|
1896
|
|
|
1897 return 0;
|
|
|
1898 }
|
|
|
1899
|
|
|
1900 // //////////////////////////// TEST CODE /////////////////////////////////////
|
|
|
1901
|
|
|
1902
|
|
|
1903
|
|
|
1904 //#include <QtDebug>
|
|
|
1905 //#include <stdio.h>
|
|
|
1906 //#include <math.h>
|
|
|
1907 /*#include <stdlib.h>
|
|
|
1908
|
|
|
1909 short compass_DX_f, compass_DY_f, compass_DZ_f;
|
|
|
1910 short compass_CX_f, compass_CY_f, compass_CZ_f;
|
|
|
1911
|
|
|
1912 inline float uniform(void) {
|
|
|
1913 return (rand() & 0xFFFF) / 65536.0f;
|
|
|
1914 }
|
|
|
1915 inline float sqr(float x) {
|
|
|
1916 return x*x;
|
|
|
1917 }
|
|
|
1918
|
|
|
1919 static const float radius = 0.21f;
|
|
|
1920 static const float cx = 0.79f, cy = -0.46f, cz = 0.24f;
|
|
|
1921 // const float cx = 0, cy = 0, cz = 0;
|
|
|
1922
|
|
|
1923 float check_compass_calib(void)
|
|
|
1924 {
|
|
|
1925
|
|
|
1926 // Starts with no calibration at all:
|
|
|
1927 compass_CX_f = compass_CY_f = compass_CZ_f = 0;
|
|
|
1928
|
|
|
1929 // Try 10 recalibration passes:
|
|
|
1930 for(int p=0; p<10; ++p)
|
|
|
1931 {
|
|
|
1932 compass_reset_calibration();
|
|
|
1933
|
|
|
1934 //---- Generates random points on a sphere -------------------------------
|
|
|
1935 // of radius,center (cx, cy, cz):
|
|
|
1936 for(int i=0; i<100; ++i)
|
|
|
1937 {
|
|
|
1938 float theta = uniform()*360.0f;
|
|
|
1939 float phi = uniform()*180.0f - 90.0f;
|
|
|
1940
|
|
|
1941 float x = cx + radius * cosf(phi)*cosf(theta);
|
|
|
1942 float y = cy + radius * cosf(phi)*sinf(theta);
|
|
|
1943 float z = cz + radius * sinf(phi);
|
|
|
1944
|
|
|
1945 compass_DX_f = (short)(32768 * x);
|
|
|
1946 compass_DY_f = (short)(32768 * y);
|
|
|
1947 compass_DZ_f = (short)(32768 * z);
|
|
|
1948 compass_add_calibration();
|
|
|
1949 }
|
|
|
1950
|
|
|
1951 compass_solve_calibration();
|
|
|
1952 //qDebug() << "Center ="
|
|
|
1953 // << compass_CX_f/32768.0f
|
|
|
1954 // << compass_CY_f/32768.0f
|
|
|
1955 // << compass_CZ_f/32768.0f;
|
|
|
1956
|
|
|
1957 float r2 = sqr(compass_CX_f/32768.0f - cx)
|
|
|
1958 + sqr(compass_CY_f/32768.0f - cy)
|
|
|
1959 + sqr(compass_CZ_f/32768.0f - cz);
|
|
|
1960 if( r2 > 0.01f*0.01f )
|
|
|
1961 return sqrtf(r2);
|
|
|
1962 }
|
|
|
1963 return 0;
|
|
|
1964 }*/
|
|
|
1965
|
|
|
1966
|
|
|
1967
|
|
|
1968 /*
|
|
|
1969 void compass_read_LSM303D_v3(void)
|
|
|
1970 {
|
|
|
1971 uint8_t data;
|
|
|
1972
|
|
|
1973 memset(magDataBuffer,0,6);
|
|
|
1974
|
|
|
1975 compass_DX_f = 0;
|
|
|
1976 compass_DY_f = 0;
|
|
|
1977 compass_DZ_f = 0;
|
|
|
1978
|
|
|
1979 //magnetometer multi read, order xl,xh, yl,yh, zl, zh
|
|
|
1980 data = REG_MAG_DATA_ADDR;
|
|
|
1981 I2C_Master_Transmit( DEVICE_COMPASS_303D, &data, 1);
|
|
|
1982 I2C_Master_Receive( DEVICE_COMPASS_303D, magDataBuffer, 6);
|
|
|
1983
|
|
|
1984 compass_DX_f = ((int16_t)( (int16_t)((magDataBuffer[1] << 8) | (magDataBuffer[0]))));
|
|
|
1985 compass_DY_f = ((int16_t)( (int16_t)((magDataBuffer[3] << 8) | (magDataBuffer[2]))));
|
|
|
1986 compass_DZ_f = ((int16_t)( (int16_t)((magDataBuffer[5] << 8) | (magDataBuffer[4]))));
|
|
|
1987
|
|
|
1988 // compass_DX_f = compass_DX_f * stat->sensitivity_mag;
|
|
|
1989 // compass_DY_f = compass_DY_f * stat->sensitivity_mag;
|
|
|
1990 // compass_DZ_f = compass_DZ_f * stat->sensitivity_mag;
|
|
|
1991 }
|
|
|
1992
|
|
|
1993
|
|
|
1994 // ===============================================================================
|
|
|
1995 // compass_init_LSM303D by STMicroelectronics 2013 V1.0.5 2013/Oct/23
|
|
|
1996 /// @brief The new ST 303D
|
|
|
1997 /// This might be called several times with different gain values during calibration
|
|
|
1998 ///
|
|
|
1999 /// @param gain: 7 is max gain and set with here, compass_calib() might reduce it
|
|
|
2000 // ===============================================================================
|
|
|
2001
|
|
|
2002 void compass_init_LSM303D_v3(uint8_t gain)
|
|
|
2003 {
|
|
|
2004 uint8_t data[10];
|
|
|
2005
|
|
|
2006 // CNTRL1
|
|
|
2007 // 0011 acceleration data rate 0011 = 12.5 Hz (3.125 Hz - 1600 Hz)
|
|
|
2008 // 0xxx block data update off
|
|
|
2009 // x111 enable all three axes
|
|
|
2010
|
|
|
2011 // CNTRL5
|
|
|
2012 // 0xxx xxxx temp sensor off
|
|
|
2013 // x00x xxxx magnetic resolution
|
|
|
2014 // xxx0 1xxx magentic data rate 01 = 6,25 Hz (3.125 Hz - 50 Hz (100 Hz))
|
|
|
2015 // xxxx xx00 latch irq requests off
|
|
|
2016
|
|
|
2017 // CNTRL7
|
|
|
2018 // 00xx high pass filter mode, 00 normal mode
|
|
|
2019 // xx0x filter for acceleration data bypassed
|
|
|
2020 // xxx0 temperature sensor mode only off
|
|
|
2021 // x0xx magnetic data low-power mode off
|
|
|
2022 // xx00 magnetic sensor mode 00 = continous-conversion mode (default 10 power-down)
|
|
|
2023
|
|
|
2024 data[0] = CNTRL0;
|
|
|
2025 data[1] = 0x00;
|
|
|
2026 I2C_Master_Transmit( DEVICE_COMPASS_303D, data, 2);
|
|
|
2027
|
|
|
2028 // acc
|
|
|
2029 data[0] = CNTRL1;
|
|
|
2030 data[1] = 0x00;
|
|
|
2031 data[2] = 0x0F;
|
|
|
2032 data[3] = 0x00;
|
|
|
2033 data[4] = 0x00;
|
|
|
2034 I2C_Master_Transmit( DEVICE_COMPASS_303D, data, 5);
|
|
|
2035
|
|
|
2036 // mag
|
|
|
2037 data[0] = CNTRL3;
|
|
|
2038 data[1] = 0x00;
|
|
|
2039 data[2] = 0x00;
|
|
|
2040 data[3] = 0x18;
|
|
|
2041 data[4] = 0x20;
|
|
|
2042 I2C_Master_Transmit( DEVICE_COMPASS_303D, data, 5);
|
|
|
2043
|
|
|
2044 data[0] = CNTRL7;
|
|
|
2045 data[1] = ((MSMS_MASK & CONTINUOS_CONVERSION) |
|
|
|
2046 ((~MSMS_MASK) & CNTRL7_RESUME_VALUE));
|
|
|
2047 I2C_Master_Transmit( DEVICE_COMPASS_303D, data, 2);
|
|
|
2048
|
|
|
2049 HAL_Delay(100);
|
|
|
2050 }
|
|
|
2051
|
|
|
2052
|
|
|
2053 // ===============================================================================
|
|
|
2054 // compass_init_LSM303D by nordevx for arduion
|
|
|
2055 /// @brief The new ST 303D
|
|
|
2056 /// This might be called several times with different gain values during calibration
|
|
|
2057 ///
|
|
|
2058 /// @param gain: 7 is max gain and set with here, compass_calib() might reduce it
|
|
|
2059 // ===============================================================================
|
|
|
2060 void compass_init_LSM303D_v2(uint8_t gain)
|
|
|
2061 {
|
|
|
2062 uint8_t data[2];
|
|
|
2063
|
|
|
2064 // CNTRL1
|
|
|
2065 // 0011 acceleration data rate 0011 = 12.5 Hz (3.125 Hz - 1600 Hz)
|
|
|
2066 // 0xxx block data update off
|
|
|
2067 // x111 enable all three axes
|
|
|
2068
|
|
|
2069 // CNTRL5
|
|
|
2070 // 0xxx xxxx temp sensor off
|
|
|
2071 // x00x xxxx magnetic resolution
|
|
|
2072 // xxx0 1xxx magentic data rate 01 = 6,25 Hz (3.125 Hz - 50 Hz (100 Hz))
|
|
|
2073 // xxxx xx00 latch irq requests off
|
|
|
2074
|
|
|
2075 // CNTRL7
|
|
|
2076 // 00xx high pass filter mode, 00 normal mode
|
|
|
2077 // xx0x filter for acceleration data bypassed
|
|
|
2078 // xxx0 temperature sensor mode only off
|
|
|
2079 // x0xx magnetic data low-power mode off
|
|
|
2080 // xx00 magnetic sensor mode 00 = continous-conversion mode (default 10 power-down)
|
|
|
2081
|
|
|
2082 data[0] = CNTRL1;
|
|
|
2083 data[1] = 0x37; //0b 0011 0111
|
|
|
2084 I2C_Master_Transmit( DEVICE_COMPASS_303D, data, 2);
|
|
|
2085
|
|
|
2086 data[0] = CNTRL5;
|
|
|
2087 data[1] = 0x08; // 0b 0000 1000
|
|
|
2088 I2C_Master_Transmit( DEVICE_COMPASS_303D, data, 2);
|
|
|
2089
|
|
|
2090 data[0] = CNTRL7;
|
|
|
2091 data[1] = 0x00; // 0b 0000 0000
|
|
|
2092 I2C_Master_Transmit( DEVICE_COMPASS_303D, data, 2);
|
|
|
2093
|
|
|
2094 HAL_Delay(100);
|
|
|
2095 }
|
|
|
2096
|
|
|
2097
|
|
|
2098 // ===============================================================================
|
|
|
2099 // compass_init_LSM303D_v1 by ST lsm303d.c
|
|
|
2100 /// @brief The new ST 303D
|
|
|
2101 /// This might be called several times with different gain values during calibration
|
|
|
2102 ///
|
|
|
2103 /// @param gain: 7 is max gain and set with here, compass_calib() might reduce it
|
|
|
2104 // ===============================================================================
|
|
|
2105 void compass_init_LSM303D_v1(uint8_t gain)
|
|
|
2106 {
|
|
|
2107 uint8_t data;
|
|
|
2108
|
|
|
2109 compass_gain = gain;
|
|
|
2110
|
|
|
2111 memset(magDataBuffer,0,6);
|
|
|
2112 memset(accDataBuffer,0,6);
|
|
|
2113
|
|
|
2114 data = CNTRL5;
|
|
|
2115 I2C_Master_Transmit( DEVICE_COMPASS_303D, &data, 1);
|
|
|
2116 I2C_Master_Receive( DEVICE_COMPASS_303D, &data, 1);
|
|
|
2117 data = (data & 0x1c) >> 2;
|
|
|
2118 velMag = magODR[data];
|
|
|
2119
|
|
|
2120 data = CNTRL1;
|
|
|
2121 I2C_Master_Transmit( DEVICE_COMPASS_303D, &data, 1);
|
|
|
2122 I2C_Master_Receive( DEVICE_COMPASS_303D, &data, 1);
|
|
|
2123 data = (data & 0xf0) >> 4;
|
|
|
2124 velAcc = accODR[data];
|
|
|
2125
|
|
|
2126 data = CNTRL7;
|
|
|
2127 I2C_Master_Transmit( DEVICE_COMPASS_303D, &data, 1);
|
|
|
2128 I2C_Master_Receive( DEVICE_COMPASS_303D, &datas1, 1);
|
|
|
2129 datas1 = (datas1 & 0x02);
|
|
|
2130
|
|
|
2131 //if mag is not pd
|
|
|
2132 //mag is bigger than gyro
|
|
|
2133 if( (velMag < velAcc) || datas1 != 0 ) {
|
|
|
2134 //acc is the biggest
|
|
|
2135 fastest = ACC_IS_FASTEST;
|
|
|
2136 }
|
|
|
2137 else {
|
|
|
2138 //acc is the biggest
|
|
|
2139 fastest = MAG_IS_FASTEST;
|
|
|
2140 }
|
|
|
2141
|
|
|
2142 zoffFlag = 1;
|
|
|
2143
|
|
|
2144 if( fastest == MAG_IS_FASTEST)
|
|
|
2145 {
|
|
|
2146 data = STATUS_REG_M;
|
|
|
2147 I2C_Master_Transmit( DEVICE_COMPASS_303D, &data, 1);
|
|
|
2148 I2C_Master_Receive( DEVICE_COMPASS_303D, &data, 1);
|
|
|
2149
|
|
|
2150 // if(ValBit(data, ZYXMDA)) {
|
|
|
2151 sendFlag = 1;
|
|
|
2152 // }
|
|
|
2153
|
|
|
2154 }
|
|
|
2155 else if(fastest == ACC_IS_FASTEST)
|
|
|
2156 {
|
|
|
2157 data = STATUS_REG_A;
|
|
|
2158 I2C_Master_Transmit( DEVICE_COMPASS_303D, &data, 1);
|
|
|
2159 I2C_Master_Receive( DEVICE_COMPASS_303D, &data, 1);
|
|
|
2160 // if(ValBit(data, DATAREADY_BIT)) {
|
|
|
2161 sendFlag = 1;
|
|
|
2162 // }
|
|
|
2163 }
|
|
|
2164 }
|
|
|
2165
|
|
|
2166 // ===============================================================================
|
|
|
2167 // compass_read_LSM303D
|
|
|
2168 /// @brief The new LSM303D :-)
|
|
|
2169 ///
|
|
|
2170 /// output is compass_DX_f, compass_DY_f, compass_DZ_f, accel_DX_f, accel_DY_f, accel_DZ_f
|
|
|
2171 // ===============================================================================
|
|
|
2172 void compass_read_LSM303D_v2(void)
|
|
|
2173 {
|
|
|
2174 uint8_t data;
|
|
|
2175
|
|
|
2176 memset(magDataBuffer,0,6);
|
|
|
2177 memset(accDataBuffer,0,6);
|
|
|
2178
|
|
|
2179 compass_DX_f = 0;
|
|
|
2180 compass_DY_f = 0;
|
|
|
2181 compass_DZ_f = 0;
|
|
|
2182
|
|
|
2183 accel_DX_f = 0;
|
|
|
2184 accel_DY_f = 0;
|
|
|
2185 accel_DZ_f = 0;
|
|
|
2186
|
|
|
2187 //Accelerometer multi read, order xl,xh, yl,yh, zl, zh
|
|
|
2188 data = REG_ACC_DATA_ADDR;
|
|
|
2189 I2C_Master_Transmit( DEVICE_COMPASS_303D, &data, 1);
|
|
|
2190 I2C_Master_Receive( DEVICE_COMPASS_303D, accDataBuffer, 6);
|
|
|
2191
|
|
|
2192 //magnetometer multi read, order xl,xh, yl,yh, zl, zh
|
|
|
2193 data = OUT_X_L_M;
|
|
|
2194 I2C_Master_Transmit( DEVICE_COMPASS_303D, &data, 1);
|
|
|
2195 I2C_Master_Receive( DEVICE_COMPASS_303D, magDataBuffer, 6);
|
|
|
2196
|
|
|
2197 accel_DX_f = ((int16_t)( (int16_t)((accDataBuffer[1] << 8) | (accDataBuffer[0]))));
|
|
|
2198 accel_DY_f = ((int16_t)( (int16_t)((accDataBuffer[3] << 8) | (accDataBuffer[2]))));
|
|
|
2199 accel_DZ_f = ((int16_t)( (int16_t)((accDataBuffer[5] << 8) | (accDataBuffer[4]))));
|
|
|
2200
|
|
|
2201 // accel_DX_f = accel_DX_f * stat->sensitivity_acc;
|
|
|
2202 // accel_DY_f = accel_DY_f * stat->sensitivity_acc;
|
|
|
2203 // accel_DZ_f = accel_DZ_f * stat->sensitivity_acc;
|
|
|
2204
|
|
|
2205
|
|
|
2206 compass_DX_f = magDataBuffer[1];
|
|
|
2207 compass_DX_f *= 256;
|
|
|
2208 compass_DX_f += magDataBuffer[0];
|
|
|
2209
|
|
|
2210 compass_DY_f = magDataBuffer[3];
|
|
|
2211 compass_DY_f *= 256;
|
|
|
2212 compass_DY_f += magDataBuffer[2];
|
|
|
2213
|
|
|
2214 compass_DY_f = magDataBuffer[5];
|
|
|
2215 compass_DY_f *= 256;
|
|
|
2216 compass_DY_f += magDataBuffer[4];
|
|
|
2217
|
|
|
2218 }
|
|
|
2219
|
|
|
2220
|
|
|
2221 */
|
|
|
2222
|
