Mercurial > public > ostc4
annotate Small_CPU/Src/compass.c @ 175:9da7dd50e2ec max-depth
cleanup: remove unused code
as found while fixing the max depth bug.
Signed-off-by: Jan Mulder <jlmulder@xs4all.nl>
author | Jan Mulder <jlmulder@xs4all.nl> |
---|---|
date | Mon, 11 Mar 2019 19:57:51 +0100 |
parents | 14e4c83a7559 |
children | 9ecc2e60418d |
rev | line source |
---|---|
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; | |
70 | 278 else |
279 hardwareCompass = HMC5883L; | |
38 | 280 } |
281 | |
282 | |
70 | 283 // k�nnte Probleme mit altem Chip machen |
284 // beim 303D f�hrt dieser Code dazu, dass WHOIAM_VALUE nicht geschickt wird!!! | |
38 | 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 | |
70 | 309 |
38 | 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 } | |
70 | 320 |
38 | 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 { | |
70 | 379 return compass_calib_common(); // 170821 zur Zeit kein lowpass filtering gefunden, nur high pass auf dem Register ohne Erkl�rung |
38 | 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 | |
70 | 1151 //LSM303D_write_checked_reg(DLHC_CTRL_REG2_A,0x00); // 0x00 default, hier k�nnte filter sein 0x8?80, cutoff freq. not beschrieben |
38 | 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!! | |
70 | 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. |
38 | 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 // | |
70 | 1194 //LSM303D_write_checked_reg(DLHC_CTRL_REG2_A,0x00); // 0x00 default, hier k�nnte filter sein 0x8?80, cutoff freq. not beschrieben |
38 | 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 // | |
70 | 1209 //LSM303D_write_checked_reg(DLHC_CTRL_REG2_A,0x00); // 0x00 default, hier k�nnte filter sein 0x8?80, cutoff freq. not beschrieben |
38 | 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 | |
70 | 1321 // mh f�r 303D |
38 | 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 acceleration_read(); | |
1857 compass_calc_roll_pitch_only(); | |
1858 | |
1859 if((hardwareCompass == HMC5883L) | |
1860 &&((compass_DX_f == -4096) || | |
1861 (compass_DY_f == -4096) || | |
1862 (compass_DZ_f == -4096) )) | |
1863 { | |
1864 if(compass_gain == 0) | |
1865 return -1; | |
1866 compass_gain--; | |
1867 | |
1868 compass_init(1, compass_gain); | |
1869 compass_reset_calibration(&g); | |
1870 //tickstart = HAL_GetTick(); | |
1871 continue; | |
1872 } | |
1873 | |
1874 copyCompassDataDuringCalibration(compass_DX_f,compass_DY_f,compass_DZ_f); | |
104 | 1875 compass_add_calibration(&g); |
38 | 1876 HAL_Delay(1); |
1877 lasttick = HAL_GetTick(); | |
1878 if(lasttick == 0) | |
1879 { | |
1880 tickstart = -ticks; | |
1881 } | |
104 | 1882 HAL_Delay(1); |
38 | 1883 ticks = lasttick - tickstart; |
147
14e4c83a7559
Forward compass data during calibration mode
Ideenmodellierer
parents:
104
diff
changeset
|
1884 SPI_Evaluate_RX_Data(); |
104 | 1885 } |
38 | 1886 |
1887 compass_solve_calibration(&g); | |
1888 | |
1889 tfull32 dataBlock[4]; | |
1890 dataBlock[0].Word16.low16 = compass_CX_f; | |
1891 dataBlock[0].Word16.hi16 = compass_CY_f; | |
1892 dataBlock[1].Word16.low16 = compass_CZ_f; | |
1893 dataBlock[1].Word16.hi16 = 0xFFFF; | |
1894 dataBlock[2].Full32 = 0x7FFFFFFF; | |
1895 dataBlock[3].Full32 = 0x7FFFFFFF; | |
1896 BFA_writeDataBlock((uint32_t *)dataBlock); | |
1897 | |
1898 return 0; | |
1899 } | |
1900 | |
1901 // //////////////////////////// TEST CODE ///////////////////////////////////// | |
1902 | |
1903 | |
1904 | |
1905 //#include <QtDebug> | |
1906 //#include <stdio.h> | |
1907 //#include <math.h> | |
1908 /*#include <stdlib.h> | |
1909 | |
1910 short compass_DX_f, compass_DY_f, compass_DZ_f; | |
1911 short compass_CX_f, compass_CY_f, compass_CZ_f; | |
1912 | |
1913 inline float uniform(void) { | |
1914 return (rand() & 0xFFFF) / 65536.0f; | |
1915 } | |
1916 inline float sqr(float x) { | |
1917 return x*x; | |
1918 } | |
1919 | |
1920 static const float radius = 0.21f; | |
1921 static const float cx = 0.79f, cy = -0.46f, cz = 0.24f; | |
1922 // const float cx = 0, cy = 0, cz = 0; | |
1923 | |
1924 float check_compass_calib(void) | |
1925 { | |
1926 | |
1927 // Starts with no calibration at all: | |
1928 compass_CX_f = compass_CY_f = compass_CZ_f = 0; | |
1929 | |
1930 // Try 10 recalibration passes: | |
1931 for(int p=0; p<10; ++p) | |
1932 { | |
1933 compass_reset_calibration(); | |
1934 | |
1935 //---- Generates random points on a sphere ------------------------------- | |
1936 // of radius,center (cx, cy, cz): | |
1937 for(int i=0; i<100; ++i) | |
1938 { | |
1939 float theta = uniform()*360.0f; | |
1940 float phi = uniform()*180.0f - 90.0f; | |
1941 | |
1942 float x = cx + radius * cosf(phi)*cosf(theta); | |
1943 float y = cy + radius * cosf(phi)*sinf(theta); | |
1944 float z = cz + radius * sinf(phi); | |
1945 | |
1946 compass_DX_f = (short)(32768 * x); | |
1947 compass_DY_f = (short)(32768 * y); | |
1948 compass_DZ_f = (short)(32768 * z); | |
1949 compass_add_calibration(); | |
1950 } | |
1951 | |
1952 compass_solve_calibration(); | |
1953 //qDebug() << "Center =" | |
1954 // << compass_CX_f/32768.0f | |
1955 // << compass_CY_f/32768.0f | |
1956 // << compass_CZ_f/32768.0f; | |
1957 | |
1958 float r2 = sqr(compass_CX_f/32768.0f - cx) | |
1959 + sqr(compass_CY_f/32768.0f - cy) | |
1960 + sqr(compass_CZ_f/32768.0f - cz); | |
1961 if( r2 > 0.01f*0.01f ) | |
1962 return sqrtf(r2); | |
1963 } | |
1964 return 0; | |
1965 }*/ | |
1966 | |
1967 | |
1968 | |
1969 /* | |
1970 void compass_read_LSM303D_v3(void) | |
1971 { | |
1972 uint8_t data; | |
1973 | |
1974 memset(magDataBuffer,0,6); | |
1975 | |
1976 compass_DX_f = 0; | |
1977 compass_DY_f = 0; | |
1978 compass_DZ_f = 0; | |
1979 | |
1980 //magnetometer multi read, order xl,xh, yl,yh, zl, zh | |
1981 data = REG_MAG_DATA_ADDR; | |
1982 I2C_Master_Transmit( DEVICE_COMPASS_303D, &data, 1); | |
1983 I2C_Master_Receive( DEVICE_COMPASS_303D, magDataBuffer, 6); | |
1984 | |
1985 compass_DX_f = ((int16_t)( (int16_t)((magDataBuffer[1] << 8) | (magDataBuffer[0])))); | |
1986 compass_DY_f = ((int16_t)( (int16_t)((magDataBuffer[3] << 8) | (magDataBuffer[2])))); | |
1987 compass_DZ_f = ((int16_t)( (int16_t)((magDataBuffer[5] << 8) | (magDataBuffer[4])))); | |
1988 | |
1989 // compass_DX_f = compass_DX_f * stat->sensitivity_mag; | |
1990 // compass_DY_f = compass_DY_f * stat->sensitivity_mag; | |
1991 // compass_DZ_f = compass_DZ_f * stat->sensitivity_mag; | |
1992 } | |
1993 | |
1994 | |
1995 // =============================================================================== | |
1996 // compass_init_LSM303D by STMicroelectronics 2013 V1.0.5 2013/Oct/23 | |
1997 /// @brief The new ST 303D | |
1998 /// This might be called several times with different gain values during calibration | |
1999 /// | |
2000 /// @param gain: 7 is max gain and set with here, compass_calib() might reduce it | |
2001 // =============================================================================== | |
2002 | |
2003 void compass_init_LSM303D_v3(uint8_t gain) | |
2004 { | |
2005 uint8_t data[10]; | |
2006 | |
2007 // CNTRL1 | |
2008 // 0011 acceleration data rate 0011 = 12.5 Hz (3.125 Hz - 1600 Hz) | |
2009 // 0xxx block data update off | |
2010 // x111 enable all three axes | |
2011 | |
2012 // CNTRL5 | |
2013 // 0xxx xxxx temp sensor off | |
2014 // x00x xxxx magnetic resolution | |
2015 // xxx0 1xxx magentic data rate 01 = 6,25 Hz (3.125 Hz - 50 Hz (100 Hz)) | |
2016 // xxxx xx00 latch irq requests off | |
2017 | |
2018 // CNTRL7 | |
2019 // 00xx high pass filter mode, 00 normal mode | |
2020 // xx0x filter for acceleration data bypassed | |
2021 // xxx0 temperature sensor mode only off | |
2022 // x0xx magnetic data low-power mode off | |
2023 // xx00 magnetic sensor mode 00 = continous-conversion mode (default 10 power-down) | |
2024 | |
2025 data[0] = CNTRL0; | |
2026 data[1] = 0x00; | |
2027 I2C_Master_Transmit( DEVICE_COMPASS_303D, data, 2); | |
2028 | |
2029 // acc | |
2030 data[0] = CNTRL1; | |
2031 data[1] = 0x00; | |
2032 data[2] = 0x0F; | |
2033 data[3] = 0x00; | |
2034 data[4] = 0x00; | |
2035 I2C_Master_Transmit( DEVICE_COMPASS_303D, data, 5); | |
2036 | |
2037 // mag | |
2038 data[0] = CNTRL3; | |
2039 data[1] = 0x00; | |
2040 data[2] = 0x00; | |
2041 data[3] = 0x18; | |
2042 data[4] = 0x20; | |
2043 I2C_Master_Transmit( DEVICE_COMPASS_303D, data, 5); | |
2044 | |
2045 data[0] = CNTRL7; | |
2046 data[1] = ((MSMS_MASK & CONTINUOS_CONVERSION) | | |
2047 ((~MSMS_MASK) & CNTRL7_RESUME_VALUE)); | |
2048 I2C_Master_Transmit( DEVICE_COMPASS_303D, data, 2); | |
2049 | |
2050 HAL_Delay(100); | |
2051 } | |
2052 | |
2053 | |
2054 // =============================================================================== | |
2055 // compass_init_LSM303D by nordevx for arduion | |
2056 /// @brief The new ST 303D | |
2057 /// This might be called several times with different gain values during calibration | |
2058 /// | |
2059 /// @param gain: 7 is max gain and set with here, compass_calib() might reduce it | |
2060 // =============================================================================== | |
2061 void compass_init_LSM303D_v2(uint8_t gain) | |
2062 { | |
2063 uint8_t data[2]; | |
2064 | |
2065 // CNTRL1 | |
2066 // 0011 acceleration data rate 0011 = 12.5 Hz (3.125 Hz - 1600 Hz) | |
2067 // 0xxx block data update off | |
2068 // x111 enable all three axes | |
2069 | |
2070 // CNTRL5 | |
2071 // 0xxx xxxx temp sensor off | |
2072 // x00x xxxx magnetic resolution | |
2073 // xxx0 1xxx magentic data rate 01 = 6,25 Hz (3.125 Hz - 50 Hz (100 Hz)) | |
2074 // xxxx xx00 latch irq requests off | |
2075 | |
2076 // CNTRL7 | |
2077 // 00xx high pass filter mode, 00 normal mode | |
2078 // xx0x filter for acceleration data bypassed | |
2079 // xxx0 temperature sensor mode only off | |
2080 // x0xx magnetic data low-power mode off | |
2081 // xx00 magnetic sensor mode 00 = continous-conversion mode (default 10 power-down) | |
2082 | |
2083 data[0] = CNTRL1; | |
2084 data[1] = 0x37; //0b 0011 0111 | |
2085 I2C_Master_Transmit( DEVICE_COMPASS_303D, data, 2); | |
2086 | |
2087 data[0] = CNTRL5; | |
2088 data[1] = 0x08; // 0b 0000 1000 | |
2089 I2C_Master_Transmit( DEVICE_COMPASS_303D, data, 2); | |
2090 | |
2091 data[0] = CNTRL7; | |
2092 data[1] = 0x00; // 0b 0000 0000 | |
2093 I2C_Master_Transmit( DEVICE_COMPASS_303D, data, 2); | |
2094 | |
2095 HAL_Delay(100); | |
2096 } | |
2097 | |
2098 | |
2099 // =============================================================================== | |
2100 // compass_init_LSM303D_v1 by ST lsm303d.c | |
2101 /// @brief The new ST 303D | |
2102 /// This might be called several times with different gain values during calibration | |
2103 /// | |
2104 /// @param gain: 7 is max gain and set with here, compass_calib() might reduce it | |
2105 // =============================================================================== | |
2106 void compass_init_LSM303D_v1(uint8_t gain) | |
2107 { | |
2108 uint8_t data; | |
2109 | |
2110 compass_gain = gain; | |
2111 | |
2112 memset(magDataBuffer,0,6); | |
2113 memset(accDataBuffer,0,6); | |
2114 | |
2115 data = CNTRL5; | |
2116 I2C_Master_Transmit( DEVICE_COMPASS_303D, &data, 1); | |
2117 I2C_Master_Receive( DEVICE_COMPASS_303D, &data, 1); | |
2118 data = (data & 0x1c) >> 2; | |
2119 velMag = magODR[data]; | |
2120 | |
2121 data = CNTRL1; | |
2122 I2C_Master_Transmit( DEVICE_COMPASS_303D, &data, 1); | |
2123 I2C_Master_Receive( DEVICE_COMPASS_303D, &data, 1); | |
2124 data = (data & 0xf0) >> 4; | |
2125 velAcc = accODR[data]; | |
2126 | |
2127 data = CNTRL7; | |
2128 I2C_Master_Transmit( DEVICE_COMPASS_303D, &data, 1); | |
2129 I2C_Master_Receive( DEVICE_COMPASS_303D, &datas1, 1); | |
2130 datas1 = (datas1 & 0x02); | |
2131 | |
2132 //if mag is not pd | |
2133 //mag is bigger than gyro | |
2134 if( (velMag < velAcc) || datas1 != 0 ) { | |
2135 //acc is the biggest | |
2136 fastest = ACC_IS_FASTEST; | |
2137 } | |
2138 else { | |
2139 //acc is the biggest | |
2140 fastest = MAG_IS_FASTEST; | |
2141 } | |
2142 | |
2143 zoffFlag = 1; | |
2144 | |
2145 if( fastest == MAG_IS_FASTEST) | |
2146 { | |
2147 data = STATUS_REG_M; | |
2148 I2C_Master_Transmit( DEVICE_COMPASS_303D, &data, 1); | |
2149 I2C_Master_Receive( DEVICE_COMPASS_303D, &data, 1); | |
2150 | |
2151 // if(ValBit(data, ZYXMDA)) { | |
2152 sendFlag = 1; | |
2153 // } | |
2154 | |
2155 } | |
2156 else if(fastest == ACC_IS_FASTEST) | |
2157 { | |
2158 data = STATUS_REG_A; | |
2159 I2C_Master_Transmit( DEVICE_COMPASS_303D, &data, 1); | |
2160 I2C_Master_Receive( DEVICE_COMPASS_303D, &data, 1); | |
2161 // if(ValBit(data, DATAREADY_BIT)) { | |
2162 sendFlag = 1; | |
2163 // } | |
2164 } | |
2165 } | |
2166 | |
2167 // =============================================================================== | |
2168 // compass_read_LSM303D | |
2169 /// @brief The new LSM303D :-) | |
2170 /// | |
2171 /// output is compass_DX_f, compass_DY_f, compass_DZ_f, accel_DX_f, accel_DY_f, accel_DZ_f | |
2172 // =============================================================================== | |
2173 void compass_read_LSM303D_v2(void) | |
2174 { | |
2175 uint8_t data; | |
2176 | |
2177 memset(magDataBuffer,0,6); | |
2178 memset(accDataBuffer,0,6); | |
2179 | |
2180 compass_DX_f = 0; | |
2181 compass_DY_f = 0; | |
2182 compass_DZ_f = 0; | |
2183 | |
2184 accel_DX_f = 0; | |
2185 accel_DY_f = 0; | |
2186 accel_DZ_f = 0; | |
2187 | |
2188 //Accelerometer multi read, order xl,xh, yl,yh, zl, zh | |
2189 data = REG_ACC_DATA_ADDR; | |
2190 I2C_Master_Transmit( DEVICE_COMPASS_303D, &data, 1); | |
2191 I2C_Master_Receive( DEVICE_COMPASS_303D, accDataBuffer, 6); | |
2192 | |
2193 //magnetometer multi read, order xl,xh, yl,yh, zl, zh | |
2194 data = OUT_X_L_M; | |
2195 I2C_Master_Transmit( DEVICE_COMPASS_303D, &data, 1); | |
2196 I2C_Master_Receive( DEVICE_COMPASS_303D, magDataBuffer, 6); | |
2197 | |
2198 accel_DX_f = ((int16_t)( (int16_t)((accDataBuffer[1] << 8) | (accDataBuffer[0])))); | |
2199 accel_DY_f = ((int16_t)( (int16_t)((accDataBuffer[3] << 8) | (accDataBuffer[2])))); | |
2200 accel_DZ_f = ((int16_t)( (int16_t)((accDataBuffer[5] << 8) | (accDataBuffer[4])))); | |
2201 | |
2202 // accel_DX_f = accel_DX_f * stat->sensitivity_acc; | |
2203 // accel_DY_f = accel_DY_f * stat->sensitivity_acc; | |
2204 // accel_DZ_f = accel_DZ_f * stat->sensitivity_acc; | |
2205 | |
2206 | |
2207 compass_DX_f = magDataBuffer[1]; | |
2208 compass_DX_f *= 256; | |
2209 compass_DX_f += magDataBuffer[0]; | |
2210 | |
2211 compass_DY_f = magDataBuffer[3]; | |
2212 compass_DY_f *= 256; | |
2213 compass_DY_f += magDataBuffer[2]; | |
2214 | |
2215 compass_DY_f = magDataBuffer[5]; | |
2216 compass_DY_f *= 256; | |
2217 compass_DY_f += magDataBuffer[4]; | |
2218 | |
2219 } | |
2220 | |
2221 | |
2222 */ | |
2223 |