Mercurial > public > ostc4

comparison Small_CPU/Src/compass.c @ 38:5f11787b4f42

Find changesets by keywords (author, files, the commit message), revision number or hash, or revset expression.
include in ostc4 repository
author heinrichsweikamp
date Sat, 28 Apr 2018 11:52:34 +0200
parents
children 6a6116d7b5bb
comparison
equal deleted inserted replaced
37:ccc45c0e1ea2 38:5f11787b4f42
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>&copy; 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