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
|
|
36 #include "i2c.h"
|
219
|
37 #include "spi.h"
|
38
|
38 #include "RTE_FlashAccess.h" // to store compass_calib_data
|
|
39
|
|
40 #include "stm32f4xx_hal.h"
|
|
41
|
410
|
42 extern uint32_t time_elapsed_ms(uint32_t ticksstart,uint32_t ticksnow);
|
|
43
|
38
|
44 /// split byte to bits
|
|
45 typedef struct{
|
|
46 uint8_t bit0:1; ///< split byte to bits
|
|
47 uint8_t bit1:1; ///< split byte to bits
|
|
48 uint8_t bit2:1; ///< split byte to bits
|
|
49 uint8_t bit3:1; ///< split byte to bits
|
|
50 uint8_t bit4:1; ///< split byte to bits
|
|
51 uint8_t bit5:1; ///< split byte to bits
|
|
52 uint8_t bit6:1; ///< split byte to bits
|
|
53 uint8_t bit7:1; ///< split byte to bits
|
|
54 } ubit8_t;
|
|
55
|
|
56
|
|
57 /// split byte to bits
|
|
58 typedef union{
|
|
59 ubit8_t ub; ///< split byte to bits
|
|
60 uint8_t uw; ///< split byte to bits
|
|
61 } bit8_Type;
|
|
62
|
|
63
|
|
64 /// split word to 2 bytes
|
|
65 typedef struct{
|
|
66 uint8_t low; ///< split word to 2 bytes
|
|
67 uint8_t hi; ///< split word to 2 bytes
|
|
68 } two_byte;
|
|
69
|
|
70
|
|
71 /// split word to 2 bytes
|
|
72 typedef union{
|
|
73 two_byte Byte; ///< split word to 2 bytes
|
|
74 uint16_t Word; ///< split word to 2 bytes
|
|
75 } tword;
|
|
76
|
|
77
|
|
78 /// split signed word to 2 bytes
|
|
79 typedef union{
|
|
80 two_byte Byte; ///< split signed word to 2 bytes
|
|
81 int16_t Word; ///< split signed word to 2 bytes
|
|
82 } signed_tword;
|
|
83
|
|
84
|
|
85 /// split full32 to 2 words
|
|
86 typedef struct{
|
|
87 uint16_t low16; ///< split word to 2 bytes
|
|
88 uint16_t hi16; ///< split word to 2 bytes
|
|
89 } two_word;
|
|
90
|
|
91 typedef union{
|
|
92 two_word Word16; ///< split word to 2 bytes
|
|
93 uint32_t Full32; ///< split word to 2 bytes
|
|
94 } tfull32;
|
|
95
|
|
96
|
|
97 /// crazy compass calibration stuff
|
|
98 typedef struct
|
|
99 {
|
|
100 unsigned short int compass_N;
|
|
101 float Su, Sv, Sw;
|
|
102 float Suu, Svv, Sww, Suv, Suw, Svw;
|
|
103 float Suuu, Svvv, Swww;
|
|
104 float Suuv, Suuw, Svvu, Svvw, Swwu, Swwv;
|
|
105 } SCompassCalib;
|
|
106
|
|
107
|
|
108 #define Q_PI (18000)
|
|
109 #define Q_PIO2 (9000)
|
|
110
|
|
111
|
|
112
|
|
113 //////////////////////////////////////////////////////////////////////////////
|
|
114 // fifth order of polynomial approximation of atan(), giving 0.05 deg max error
|
|
115 //
|
|
116 #define K1 (5701) // Needs K1/2**16
|
|
117 #define K2 (1645) // Needs K2/2**48 WAS NEGATIV
|
|
118 #define K3 ( 446) // Needs K3/2**80
|
|
119
|
|
120 const float PI = 3.14159265; ///< pi, used in compass_calc()
|
|
121
|
|
122 typedef short int Int16;
|
|
123 typedef signed char Int8;
|
|
124 typedef Int16 Angle;
|
|
125
|
|
126
|
|
127 /// The (filtered) components of the magnetometer sensor
|
|
128 int16_t compass_DX_f; ///< output from sensor
|
|
129 int16_t compass_DY_f; ///< output from sensor
|
|
130 int16_t compass_DZ_f; ///< output from sensor
|
|
131
|
|
132
|
|
133 /// Found soft-iron calibration values, deduced from already filtered values
|
|
134 int16_t compass_CX_f; ///< calibration value
|
|
135 int16_t compass_CY_f; ///< calibration value
|
|
136 int16_t compass_CZ_f; ///< calibration value
|
|
137
|
|
138
|
|
139 /// The (filtered) components of the accelerometer sensor
|
|
140 int16_t accel_DX_f; ///< output from sensor
|
|
141 int16_t accel_DY_f; ///< output from sensor
|
|
142 int16_t accel_DZ_f; ///< output from sensor
|
|
143
|
|
144
|
|
145 /// The compass result values
|
|
146 float compass_heading; ///< the final result calculated in compass_calc()
|
|
147 float compass_roll; ///< the final result calculated in compass_calc()
|
|
148 float compass_pitch; ///< the final result calculated in compass_calc()
|
|
149
|
|
150
|
|
151 uint8_t compass_gain; ///< 7 on start, can be reduced during calibration
|
|
152
|
357
|
153 uint8_t hardwareCompass = 0; ///< either HMC5883L (=1) or LSM303D (=2) or LSM303AGR (=3) or not defined yet (=0)
|
38
|
154
|
|
155 /// LSM303D variables
|
|
156 uint8_t magDataBuffer[6]; ///< here raw data from LSM303D is stored, can be local
|
|
157 uint8_t accDataBuffer[6]; ///< here raw data from LSM303D is stored, can be local
|
|
158
|
|
159
|
|
160 // struct accel_scale _accel_scale;
|
|
161 unsigned _accel_range_m_s2;
|
|
162 float _accel_range_scale;
|
|
163 unsigned _accel_samplerate;
|
|
164 unsigned _accel_onchip_filter_bandwith;
|
|
165
|
|
166 // struct mag_scale _mag_scale;
|
|
167 unsigned _mag_range_ga;
|
|
168 float _mag_range_scale;
|
|
169 unsigned _mag_samplerate;
|
|
170
|
|
171 // default scale factors
|
|
172 float _accel_scale_x_offset = 0.0f;
|
|
173 float _accel_scale_x_scale = 1.0f;
|
|
174 float _accel_scale_y_offset = 0.0f;
|
|
175 float _accel_scale_y_scale = 1.0f;
|
|
176 float _accel_scale_z_offset = 0.0f;
|
|
177 float _accel_scale_z_scale = 1.0f;
|
|
178
|
|
179 float _mag_scale_x_offset = 0.0f;
|
|
180 float _mag_scale_x_scale = 1.0f;
|
|
181 float _mag_scale_y_offset = 0.0f;
|
|
182 float _mag_scale_y_scale = 1.0f;
|
|
183 float _mag_scale_z_offset = 0.0f;
|
|
184 float _mag_scale_z_scale = 1.0f;
|
|
185
|
|
186
|
|
187 /* External function prototypes ----------------------------------------------*/
|
|
188
|
|
189 extern void copyCompassDataDuringCalibration(int16_t dx, int16_t dy, int16_t dz);
|
|
190
|
|
191 /* Private function prototypes -----------------------------------------------*/
|
|
192
|
|
193 void compass_reset_calibration(SCompassCalib *g);
|
|
194 void compass_add_calibration(SCompassCalib *g);
|
|
195 void compass_solve_calibration(SCompassCalib *g);
|
|
196
|
|
197 void compass_init_HMC5883L(uint8_t fast, uint8_t gain);
|
|
198 void compass_sleep_HMC5883L(void);
|
|
199 void compass_read_HMC5883L(void);
|
|
200
|
|
201 void accelerator_init_MMA8452Q(void);
|
|
202 void accelerator_sleep_MMA8452Q(void);
|
|
203 void acceleration_read_MMA8452Q(void);
|
|
204
|
|
205 void compass_init_LSM303D(uint8_t fast, uint8_t gain);
|
|
206 void compass_sleep_LSM303D(void);
|
|
207 void compass_read_LSM303D(void);
|
|
208 void acceleration_read_LSM303D(void);
|
|
209
|
357
|
210 void compass_init_LSM303AGR(uint8_t fast, uint8_t gain);
|
|
211 void compass_sleep_LSM303AGR(void);
|
|
212 void compass_read_LSM303AGR(void);
|
|
213 void acceleration_read_LSM303AGR(void);
|
|
214
|
38
|
215 int LSM303D_accel_set_onchip_lowpass_filter_bandwidth(unsigned bandwidth);
|
|
216 int compass_calib_common(void);
|
|
217
|
|
218 void compass_calc_roll_pitch_only(void);
|
|
219
|
|
220 void rotate_mag_3f(float *x, float *y, float *z);
|
|
221 void rotate_accel_3f(float *x, float *y, float *z);
|
|
222
|
|
223
|
|
224 /* Exported functions --------------------------------------------------------*/
|
|
225
|
|
226
|
|
227 // ===============================================================================
|
|
228 // compass_init
|
|
229 /// @brief This might be called several times with different gain values during calibration
|
|
230 /// On first call it figures out which hardware is integrated
|
|
231 ///
|
|
232 /// @param gain: 7 is max gain, compass_calib() might reduce it
|
|
233 // ===============================================================================
|
|
234
|
|
235 void compass_init(uint8_t fast, uint8_t gain)
|
|
236 {
|
|
237
|
|
238 // don't call again with fast, gain in calib mode etc.
|
|
239 // if unknown
|
|
240 if(hardwareCompass == COMPASS_NOT_RECOGNIZED)
|
|
241 {
|
|
242 return;
|
|
243 }
|
|
244
|
|
245 // old code but without else
|
|
246 if(hardwareCompass == 0)
|
|
247 {
|
|
248 uint8_t data = WHO_AM_I;
|
|
249 I2C_Master_Transmit( DEVICE_COMPASS_303D, &data, 1);
|
|
250 I2C_Master_Receive( DEVICE_COMPASS_303D, &data, 1);
|
357
|
251 if(data == WHOIAM_VALUE_LSM303D)
|
|
252 hardwareCompass = compass_generation2; //LSM303D;
|
358
|
253 data = WHO_AM_I;
|
|
254 I2C_Master_Transmit( DEVICE_ACCELARATOR_303AGR, &data, 1);
|
|
255 I2C_Master_Receive( DEVICE_ACCELARATOR_303AGR, &data, 1);
|
357
|
256 if(data == WHOIAM_VALUE_LSM303AGR)
|
|
257 hardwareCompass = compass_generation3; //LSM303AGR;
|
38
|
258 }
|
|
259
|
180
|
260 /* No compass identified => Retry */
|
38
|
261 if(hardwareCompass == 0)
|
|
262 {
|
|
263 uint8_t data = WHO_AM_I;
|
|
264 I2C_Master_Transmit( DEVICE_COMPASS_303D, &data, 1);
|
|
265 I2C_Master_Receive( DEVICE_COMPASS_303D, &data, 1);
|
357
|
266 if(data == WHOIAM_VALUE_LSM303D)
|
|
267 hardwareCompass = compass_generation2; //LSM303D;
|
358
|
268 data = WHO_AM_I;
|
|
269 I2C_Master_Transmit( DEVICE_ACCELARATOR_303AGR, &data, 1);
|
|
270 I2C_Master_Receive( DEVICE_ACCELARATOR_303AGR, &data, 1);
|
357
|
271 if(data == WHOIAM_VALUE_LSM303AGR)
|
|
272 hardwareCompass = compass_generation3; //LSM303AGR;
|
38
|
273 }
|
70
|
274
|
180
|
275 /* Assume that a HMC5883L is equipped by default if detection still failed */
|
38
|
276 if(hardwareCompass == 0)
|
357
|
277 hardwareCompass = compass_generation1; //HMC5883L;
|
38
|
278
|
|
279 HAL_StatusTypeDef resultOfOperationHMC_MMA = HAL_TIMEOUT;
|
|
280
|
358
|
281 // test if both chips of the two-chip solution (gen 1) are present
|
357
|
282 if(hardwareCompass == compass_generation1) // HMC5883L)
|
38
|
283 {
|
358
|
284 HAL_Delay(10);
|
|
285 MX_I2C1_Init();
|
38
|
286 uint8_t data = 0x2A; // CTRL_REG1 of DEVICE_ACCELARATOR_MMA8452Q
|
|
287 resultOfOperationHMC_MMA = I2C_Master_Transmit( DEVICE_ACCELARATOR_MMA8452Q, &data, 1);
|
|
288 if(resultOfOperationHMC_MMA == HAL_OK)
|
|
289 {
|
357
|
290 hardwareCompass = compass_generation1; //HMC5883L; // all fine, keep it
|
38
|
291 }
|
|
292 else
|
|
293 {
|
|
294 hardwareCompass = COMPASS_NOT_RECOGNIZED;
|
|
295 }
|
|
296 }
|
|
297
|
357
|
298 if(hardwareCompass == compass_generation2) //LSM303D)
|
38
|
299 compass_init_LSM303D(fast, gain);
|
357
|
300 if(hardwareCompass == compass_generation3) //LSM303AGR)
|
|
301 compass_init_LSM303AGR(fast, gain);
|
|
302 if(hardwareCompass == compass_generation1) //HMC5883L)
|
38
|
303 compass_init_HMC5883L(fast, gain);
|
|
304
|
357
|
305 tfull32 dataBlock[4];
|
|
306 if(BFA_readLastDataBlock((uint32_t *)dataBlock) == BFA_OK)
|
38
|
307 {
|
|
308 compass_CX_f = dataBlock[0].Word16.low16;
|
|
309 compass_CY_f = dataBlock[0].Word16.hi16;
|
|
310 compass_CZ_f = dataBlock[1].Word16.low16;
|
|
311 }
|
|
312
|
|
313 }
|
|
314
|
|
315
|
|
316 // ===============================================================================
|
|
317 // compass_calib
|
|
318 /// @brief with onchip_lowpass_filter configuration for accelerometer of LSM303D
|
|
319 // ===============================================================================
|
|
320 int compass_calib(void)
|
|
321 {
|
357
|
322 if(hardwareCompass == compass_generation2) //LSM303D)
|
38
|
323 {
|
|
324 LSM303D_accel_set_onchip_lowpass_filter_bandwidth(773);
|
|
325 int out = compass_calib_common();
|
|
326 LSM303D_accel_set_onchip_lowpass_filter_bandwidth(LSM303D_ACCEL_DEFAULT_ONCHIP_FILTER_FREQ);
|
|
327 return out;
|
|
328 }
|
|
329 else
|
357
|
330 if(hardwareCompass == compass_generation1) //HMC5883L)
|
|
331 {
|
|
332 return compass_calib_common();
|
|
333 }
|
|
334 else
|
|
335 if(hardwareCompass == compass_generation3) //LSM303AGR)
|
38
|
336 {
|
|
337 return compass_calib_common();
|
|
338 }
|
|
339 else
|
|
340 {
|
|
341 return 0; // standard answer of compass_calib_common();
|
|
342 }
|
|
343
|
|
344
|
|
345 }
|
|
346
|
|
347
|
|
348 // ===============================================================================
|
|
349 // compass_sleep
|
|
350 /// @brief low power mode
|
|
351 // ===============================================================================
|
|
352 void compass_sleep(void)
|
|
353 {
|
357
|
354 if(hardwareCompass == compass_generation2) //LSM303D)
|
38
|
355 {
|
|
356 compass_sleep_LSM303D();
|
|
357 }
|
|
358 else
|
357
|
359 if(hardwareCompass == compass_generation1) //HMC5883L)
|
38
|
360 {
|
|
361 compass_sleep_HMC5883L();
|
|
362 }
|
|
363 }
|
|
364
|
|
365
|
|
366 // ===============================================================================
|
|
367 // compass_read
|
|
368 /// @brief reads magnetometer and accelerometer for LSM303D,
|
|
369 /// otherwise magnetometer only
|
|
370 // ===============================================================================
|
|
371 void compass_read(void)
|
|
372 {
|
357
|
373 if(hardwareCompass == compass_generation2) //LSM303D)
|
38
|
374 compass_read_LSM303D();
|
357
|
375 if(hardwareCompass == compass_generation1) //HMC5883L)
|
38
|
376 compass_read_HMC5883L();
|
357
|
377 if(hardwareCompass == compass_generation3) //LSM303AGR)
|
|
378 compass_read_LSM303AGR();
|
|
379
|
38
|
380 }
|
|
381
|
|
382
|
|
383 // ===============================================================================
|
|
384 // accelerator_init
|
|
385 /// @brief empty for for LSM303D
|
|
386 // ===============================================================================
|
|
387 void accelerator_init(void)
|
|
388 {
|
357
|
389 if(hardwareCompass == compass_generation1) //HMC5883L)
|
38
|
390 accelerator_init_MMA8452Q();
|
|
391 }
|
|
392
|
|
393
|
|
394 // ===============================================================================
|
|
395 // accelerator_sleep
|
|
396 /// @brief empty for for LSM303D
|
|
397 // ===============================================================================
|
|
398 void accelerator_sleep(void)
|
|
399 {
|
357
|
400 if(hardwareCompass == compass_generation1) //HMC5883L)
|
38
|
401 accelerator_sleep_MMA8452Q();
|
|
402 }
|
|
403
|
|
404
|
|
405 // ===============================================================================
|
|
406 // acceleration_read
|
|
407 /// @brief empty for for LSM303D
|
|
408 // ===============================================================================
|
|
409 void acceleration_read(void)
|
|
410 {
|
357
|
411 if(hardwareCompass == compass_generation2) //LSM303D)
|
38
|
412 acceleration_read_LSM303D();
|
357
|
413 if(hardwareCompass == compass_generation1) //HMC5883L)
|
38
|
414 acceleration_read_MMA8452Q();
|
357
|
415 if(hardwareCompass == compass_generation3) //LSM303AGR)
|
|
416 acceleration_read_LSM303AGR();
|
38
|
417 }
|
|
418
|
|
419
|
|
420 /* Private functions ---------------------------------------------------------*/
|
|
421
|
|
422 // ===============================================================================
|
357
|
423 // LSM303AGR_read_reg
|
|
424 // ===============================================================================
|
|
425 uint8_t LSM303AGR_read_reg(uint8_t addr)
|
|
426 {
|
|
427 uint8_t data;
|
|
428
|
|
429 I2C_Master_Transmit( DEVICE_COMPASS_303AGR, &addr, 1);
|
|
430 I2C_Master_Receive( DEVICE_COMPASS_303AGR, &data, 1);
|
|
431 return data;
|
|
432 }
|
|
433
|
|
434
|
|
435 // ===============================================================================
|
|
436 // LSM303AGR_write_reg
|
|
437 // ===============================================================================
|
|
438 void LSM303AGR_write_reg(uint8_t addr, uint8_t value)
|
|
439 {
|
|
440 uint8_t data[2];
|
|
441
|
|
442 data[0] = addr;
|
|
443 data[1] = value;
|
|
444 I2C_Master_Transmit( DEVICE_COMPASS_303AGR, data, 2);
|
|
445 }
|
|
446
|
|
447 // ===============================================================================
|
|
448 // LSM303AGR_acc_write_reg
|
|
449 // ===============================================================================
|
|
450 void LSM303AGR_acc_write_reg(uint8_t addr, uint8_t value)
|
|
451 {
|
|
452 uint8_t data[2];
|
|
453
|
|
454 data[0] = addr;
|
|
455 data[1] = value;
|
|
456 I2C_Master_Transmit( DEVICE_ACCELARATOR_303AGR, data, 2);
|
|
457 }
|
|
458
|
|
459
|
|
460 // ===============================================================================
|
|
461 // LSM303AGR_write_checked_reg
|
|
462 // ===============================================================================
|
|
463 void LSM303AGR_write_checked_reg(uint8_t addr, uint8_t value)
|
|
464 {
|
|
465 LSM303AGR_write_reg(addr, value);
|
|
466 }
|
|
467
|
|
468 // ===============================================================================
|
|
469 // LSM303AGR_acc_write_checked_reg
|
|
470 // ===============================================================================
|
|
471 void LSM303AGR_acc_write_checked_reg(uint8_t addr, uint8_t value)
|
|
472 {
|
|
473 LSM303AGR_acc_write_reg(addr, value);
|
|
474 }
|
|
475
|
|
476 // ===============================================================================
|
38
|
477 // LSM303D_read_reg
|
|
478 // ===============================================================================
|
|
479 uint8_t LSM303D_read_reg(uint8_t addr)
|
|
480 {
|
|
481 uint8_t data;
|
|
482
|
|
483 I2C_Master_Transmit( DEVICE_COMPASS_303D, &addr, 1);
|
|
484 I2C_Master_Receive( DEVICE_COMPASS_303D, &data, 1);
|
|
485 return data;
|
|
486 }
|
|
487
|
|
488
|
|
489 // ===============================================================================
|
|
490 // LSM303D_write_reg
|
|
491 // ===============================================================================
|
|
492 void LSM303D_write_reg(uint8_t addr, uint8_t value)
|
|
493 {
|
|
494 uint8_t data[2];
|
|
495
|
|
496 /* enable accel*/
|
|
497 data[0] = addr;
|
|
498 data[1] = value;
|
|
499 I2C_Master_Transmit( DEVICE_COMPASS_303D, data, 2);
|
|
500 }
|
|
501
|
|
502
|
|
503 // ===============================================================================
|
|
504 // LSM303D_write_checked_reg
|
|
505 // ===============================================================================
|
|
506 void LSM303D_write_checked_reg(uint8_t addr, uint8_t value)
|
|
507 {
|
|
508 LSM303D_write_reg(addr, value);
|
|
509 }
|
|
510
|
|
511
|
|
512 // ===============================================================================
|
|
513 // LSM303D_modify_reg
|
|
514 // ===============================================================================
|
|
515 void LSM303D_modify_reg(unsigned reg, uint8_t clearbits, uint8_t setbits)
|
|
516 {
|
|
517 uint8_t val;
|
|
518
|
|
519 val = LSM303D_read_reg(reg);
|
|
520 val &= ~clearbits;
|
|
521 val |= setbits;
|
|
522 LSM303D_write_checked_reg(reg, val);
|
|
523 }
|
|
524
|
|
525 // ===============================================================================
|
|
526 // LSM303D_accel_set_onchip_lowpass_filter_bandwidth
|
|
527 // ===============================================================================
|
|
528 int LSM303D_accel_set_onchip_lowpass_filter_bandwidth(unsigned bandwidth)
|
|
529 {
|
|
530 uint8_t setbits = 0;
|
|
531 uint8_t clearbits = REG2_ANTIALIAS_FILTER_BW_BITS_A;
|
|
532
|
|
533 if (bandwidth == 0) {
|
|
534 bandwidth = 773;
|
|
535 }
|
|
536
|
|
537 if (bandwidth <= 50) {
|
|
538 setbits |= REG2_AA_FILTER_BW_50HZ_A;
|
|
539 _accel_onchip_filter_bandwith = 50;
|
|
540
|
|
541 } else if (bandwidth <= 194) {
|
|
542 setbits |= REG2_AA_FILTER_BW_194HZ_A;
|
|
543 _accel_onchip_filter_bandwith = 194;
|
|
544
|
|
545 } else if (bandwidth <= 362) {
|
|
546 setbits |= REG2_AA_FILTER_BW_362HZ_A;
|
|
547 _accel_onchip_filter_bandwith = 362;
|
|
548
|
|
549 } else if (bandwidth <= 773) {
|
|
550 setbits |= REG2_AA_FILTER_BW_773HZ_A;
|
|
551 _accel_onchip_filter_bandwith = 773;
|
|
552
|
|
553 } else {
|
|
554 return -1;
|
|
555 }
|
|
556
|
|
557 LSM303D_modify_reg(ADDR_CTRL_REG2, clearbits, setbits);
|
|
558
|
|
559 return 0;
|
|
560 }
|
|
561
|
|
562
|
|
563 // ===============================================================================
|
|
564 // LSM303D_accel_set_driver_lowpass_filter
|
|
565 // ===============================================================================
|
|
566 int LSM303D_accel_set_driver_lowpass_filter(float samplerate, float bandwidth)
|
|
567 {
|
|
568 /*
|
|
569 _accel_filter_x_set_cutoff_frequency(samplerate, bandwidth);
|
|
570 _accel_filter_y_set_cutoff_frequency(samplerate, bandwidth);
|
|
571 _accel_filter_z_set_cutoff_frequency(samplerate, bandwidth);
|
|
572 */
|
|
573 return 0;
|
|
574 }
|
|
575
|
|
576
|
|
577 // rotate_mag_3f: nicht genutzt aber praktisch; rotate_accel_3f wird benutzt
|
|
578 // ===============================================================================
|
|
579 // rotate_mag_3f
|
|
580 /// @brief swap axis in convient way, by hw
|
|
581 /// @param *x raw input is set to *y input
|
|
582 /// @param *y raw input is set to -*x input
|
|
583 /// @param *z raw is not touched
|
|
584 // ===============================================================================
|
|
585 void rotate_mag_3f(float *x, float *y, float *z)
|
|
586 {
|
|
587 return;
|
|
588 /*
|
|
589 *x = *x; // HMC: *x = -*y
|
|
590 *y = *y; // HMC: *y = *x // change 20.04.2016: zuvor *y = -*y
|
|
591 *z = *z; // HMC: *z = *z
|
|
592 */
|
|
593 }
|
|
594
|
|
595
|
|
596 // ===============================================================================
|
|
597 // rotate_accel_3f
|
|
598 /// @brief swap axis in convient way, by hw, same as MMA8452Q
|
|
599 /// @param *x raw input, output is with sign change
|
|
600 /// @param *y raw input, output is with sign change
|
|
601 /// @param *z raw input, output is with sign change
|
|
602 // ===============================================================================
|
|
603 void rotate_accel_3f(float *x, float *y, float *z)
|
|
604 {
|
|
605 *x = -*x;
|
|
606 *y = -*y;
|
|
607 *z = -*z;
|
|
608 /* tested:
|
|
609 x = x, y =-y, z=-z: does not work with roll
|
|
610 x = x, y =y, z=-z: does not work with pitch
|
|
611 x = x, y =y, z=z: does not work at all
|
|
612 */
|
|
613 }
|
|
614
|
|
615
|
|
616 // ===============================================================================
|
357
|
617 // compass_init_LSM303D
|
38
|
618 /// This might be called several times with different gain values during calibration
|
|
619 /// but gain change is not supported at the moment.
|
|
620 ///
|
|
621 /// @param gain: 7 is max gain and set with here, compass_calib() might reduce it
|
|
622 // ===============================================================================
|
|
623
|
|
624 //uint8_t testCompassLS303D[11];
|
|
625
|
|
626 void compass_init_LSM303D(uint8_t fast, uint8_t gain)
|
|
627 {
|
|
628 if(fast == 0)
|
|
629 {
|
|
630 LSM303D_write_checked_reg(ADDR_CTRL_REG0, 0x00);
|
|
631 LSM303D_write_checked_reg(ADDR_CTRL_REG1, 0x3F); // mod 12,5 Hz 3 instead of 6,25 Hz 2
|
|
632 LSM303D_write_checked_reg(ADDR_CTRL_REG2, 0xC0);
|
|
633 LSM303D_write_checked_reg(ADDR_CTRL_REG3, 0x00);
|
|
634 LSM303D_write_checked_reg(ADDR_CTRL_REG4, 0x00);
|
|
635 LSM303D_write_checked_reg(ADDR_CTRL_REG5, 0x68); // mod 12,5 Hz 8 instead of 6,25 Hz 4
|
|
636 }
|
|
637 else
|
|
638 {
|
|
639 LSM303D_write_checked_reg(ADDR_CTRL_REG0, 0x00);
|
|
640 LSM303D_write_checked_reg(ADDR_CTRL_REG1, 0x6F); // 100 Hz
|
|
641 LSM303D_write_checked_reg(ADDR_CTRL_REG2, 0xC0);
|
|
642 LSM303D_write_checked_reg(ADDR_CTRL_REG3, 0x00);
|
|
643 LSM303D_write_checked_reg(ADDR_CTRL_REG4, 0x00);
|
|
644 LSM303D_write_checked_reg(ADDR_CTRL_REG5, 0x74); // 100 Hz
|
|
645 }
|
|
646 LSM303D_write_checked_reg(ADDR_CTRL_REG6, 0x00);
|
|
647 LSM303D_write_checked_reg(ADDR_CTRL_REG7, 0x00);
|
|
648
|
|
649 return;
|
|
650 }
|
|
651
|
|
652
|
|
653 // ===============================================================================
|
|
654 // compass_sleep_LSM303D
|
357
|
655 // @brief Gen 2 chip
|
38
|
656 // ===============================================================================
|
|
657 void compass_sleep_LSM303D(void)
|
|
658 {
|
|
659 LSM303D_write_checked_reg(ADDR_CTRL_REG1, 0x00); // CNTRL1: acceleration sensor Power-down mode
|
|
660 LSM303D_write_checked_reg(ADDR_CTRL_REG7, 0x02); // CNTRL7: magnetic sensor Power-down mode
|
|
661 }
|
|
662
|
|
663
|
|
664 // ===============================================================================
|
|
665 // acceleration_read_LSM303D
|
357
|
666 // output is accel_DX_f, accel_DY_f, accel_DZ_f
|
38
|
667 // ===============================================================================
|
|
668 void acceleration_read_LSM303D(void)
|
|
669 {
|
|
670 uint8_t data;
|
|
671 float xraw_f, yraw_f, zraw_f;
|
|
672 float accel_report_x, accel_report_y, accel_report_z;
|
|
673
|
|
674 memset(accDataBuffer,0,6);
|
|
675
|
|
676 accel_DX_f = 0;
|
|
677 accel_DY_f = 0;
|
|
678 accel_DZ_f = 0;
|
|
679
|
|
680 for(int i=0;i<6;i++)
|
|
681 {
|
|
682 data = ADDR_OUT_X_L_A + i;
|
|
683 I2C_Master_Transmit( DEVICE_COMPASS_303D, &data, 1);
|
|
684 I2C_Master_Receive( DEVICE_COMPASS_303D, &accDataBuffer[i], 1);
|
|
685 }
|
|
686
|
|
687 xraw_f = ((float)( (int16_t)((accDataBuffer[1] << 8) | (accDataBuffer[0]))));
|
|
688 yraw_f = ((float)( (int16_t)((accDataBuffer[3] << 8) | (accDataBuffer[2]))));
|
|
689 zraw_f = ((float)( (int16_t)((accDataBuffer[5] << 8) | (accDataBuffer[4]))));
|
|
690
|
|
691 rotate_accel_3f(&xraw_f, &yraw_f, &zraw_f);
|
|
692
|
|
693 // mh
|
|
694 accel_report_x = xraw_f;
|
|
695 accel_report_y = yraw_f;
|
|
696 accel_report_z = zraw_f;
|
|
697
|
|
698 accel_DX_f = ((int16_t)(accel_report_x));
|
|
699 accel_DY_f = ((int16_t)(accel_report_y));
|
|
700 accel_DZ_f = ((int16_t)(accel_report_z));
|
|
701 }
|
|
702
|
|
703
|
|
704 // ===============================================================================
|
|
705 // compass_read_LSM303D
|
|
706 ///
|
|
707 /// output is compass_DX_f, compass_DY_f, compass_DZ_f
|
|
708 // ===============================================================================
|
|
709 void compass_read_LSM303D(void)
|
|
710 {
|
|
711 uint8_t data;
|
|
712 // float xraw_f, yraw_f, zraw_f;
|
|
713 // float mag_report_x, mag_report_y, mag_report_z;
|
|
714
|
|
715 memset(magDataBuffer,0,6);
|
|
716
|
|
717 compass_DX_f = 0;
|
|
718 compass_DY_f = 0;
|
|
719 compass_DZ_f = 0;
|
|
720
|
|
721 for(int i=0;i<6;i++)
|
|
722 {
|
|
723 data = ADDR_OUT_X_L_M + i;
|
|
724 I2C_Master_Transmit( DEVICE_COMPASS_303D, &data, 1);
|
|
725 I2C_Master_Receive( DEVICE_COMPASS_303D, &magDataBuffer[i], 1);
|
|
726 }
|
|
727
|
|
728 // mh 160620 flip x and y if flip display
|
|
729 compass_DX_f = (((int16_t)((magDataBuffer[1] << 8) | (magDataBuffer[0]))));
|
|
730 compass_DY_f = (((int16_t)((magDataBuffer[3] << 8) | (magDataBuffer[2]))));
|
|
731 compass_DZ_f = (((int16_t)((magDataBuffer[5] << 8) | (magDataBuffer[4]))));
|
|
732 // no rotation
|
|
733 return;
|
357
|
734 }
|
|
735
|
|
736
|
|
737 // ===============================================================================
|
|
738 // compass_init_LSM303AGR
|
|
739 /// This might be called several times with different gain values during calibration
|
|
740 /// but gain change is not supported at the moment.
|
|
741 ///
|
|
742 /// @param gain: 7 is max gain and set with here, compass_calib() might reduce it
|
|
743 // ===============================================================================
|
38
|
744
|
357
|
745 void compass_init_LSM303AGR(uint8_t fast, uint8_t gain)
|
|
746 {
|
|
747 if(fast == 0)
|
|
748 {
|
358
|
749 // init compass
|
357
|
750 LSM303AGR_write_checked_reg(0x60, 0x80); // 10Hz
|
|
751 LSM303AGR_write_checked_reg(0x61, 0x03); // CFG_REG_B_M
|
|
752 LSM303AGR_write_checked_reg(0x62, 0x10); // CFG_REG_C_M
|
358
|
753 LSM303AGR_write_checked_reg(0x63, 0x00); // INT_CTRL_REG_M
|
|
754
|
|
755 // init accel (Same chip, but different address...)
|
|
756 LSM303AGR_acc_write_checked_reg(0x1F, 0x00); // TEMP_CFG_REG_A (Temp sensor off)
|
|
757 LSM303AGR_acc_write_checked_reg(0x20, 0x4F); // CTRL_REG1_A (50Hz, x,y,z = ON)
|
|
758 LSM303AGR_acc_write_checked_reg(0x21, 0x00); // CTRL_REG2_A
|
|
759 LSM303AGR_acc_write_checked_reg(0x22, 0x00); // CTRL_REG3_A
|
|
760 LSM303AGR_acc_write_checked_reg(0x23, 0x08); // CTRL_REG4_A, High Resolution Mode enabled
|
357
|
761 }
|
|
762 else
|
|
763 {
|
358
|
764 // init compass
|
|
765 LSM303AGR_write_checked_reg(0x60, 0x84); // 20Hz
|
357
|
766 LSM303AGR_write_checked_reg(0x61, 0x03); // CFG_REG_B_M
|
|
767 LSM303AGR_write_checked_reg(0x62, 0x10); // CFG_REG_C_M
|
358
|
768 LSM303AGR_write_checked_reg(0x63, 0x00); // INT_CTRL_REG_M
|
|
769
|
|
770 // init accel (Same chip, but different address...)
|
|
771 LSM303AGR_acc_write_checked_reg(0x1F, 0x00); // TEMP_CFG_REG_A (Temp sensor off)
|
|
772 LSM303AGR_acc_write_checked_reg(0x20, 0x4F); // CTRL_REG1_A (50Hz, x,y,z = ON)
|
|
773 LSM303AGR_acc_write_checked_reg(0x21, 0x00); // CTRL_REG2_A
|
|
774 LSM303AGR_acc_write_checked_reg(0x22, 0x00); // CTRL_REG3_A
|
|
775 LSM303AGR_acc_write_checked_reg(0x23, 0x0); // CTRL_REG4_A, High Resolution Mode enabled
|
357
|
776 }
|
|
777
|
|
778 return;
|
|
779 }
|
|
780
|
|
781
|
|
782 // ===============================================================================
|
|
783 // compass_sleep_LSM303D
|
|
784 // @brief Gen 2 chip
|
|
785 // ===============================================================================
|
|
786 void compass_sleep_LSM303AGR(void)
|
|
787 {
|
|
788 LSM303AGR_write_checked_reg(0x60, 0x03); //
|
|
789 LSM303AGR_write_checked_reg(0x61, 0x04); //
|
|
790 LSM303AGR_write_checked_reg(0x62, 0x51); //
|
|
791 LSM303AGR_write_checked_reg(0x63, 0x00); //
|
|
792
|
|
793
|
|
794 LSM303AGR_acc_write_checked_reg(0x1F, 0x00); //
|
|
795 LSM303AGR_acc_write_checked_reg(0x20, 0x00); //
|
|
796 }
|
|
797
|
38
|
798
|
357
|
799 // ===============================================================================
|
|
800 // acceleration_read_LSM303AGR
|
|
801 // output is accel_DX_f, accel_DY_f, accel_DZ_f
|
|
802 // ===============================================================================
|
|
803 void acceleration_read_LSM303AGR(void)
|
|
804 {
|
|
805 uint8_t data;
|
|
806 float xraw_f, yraw_f, zraw_f;
|
|
807 float accel_report_x, accel_report_y, accel_report_z;
|
|
808
|
|
809 memset(accDataBuffer,0,6);
|
|
810
|
|
811 accel_DX_f = 0;
|
|
812 accel_DY_f = 0;
|
|
813 accel_DZ_f = 0;
|
|
814
|
|
815 for(int i=0;i<6;i++)
|
|
816 {
|
358
|
817 data = 0x28 + i; // OUT_X_L_A
|
357
|
818 I2C_Master_Transmit( DEVICE_ACCELARATOR_303AGR, &data, 1);
|
|
819 I2C_Master_Receive( DEVICE_ACCELARATOR_303AGR, &accDataBuffer[i], 1);
|
|
820 }
|
|
821
|
|
822 xraw_f = ((float)( (int16_t)((accDataBuffer[1] << 8) | (accDataBuffer[0]))));
|
|
823 yraw_f = ((float)( (int16_t)((accDataBuffer[3] << 8) | (accDataBuffer[2]))));
|
|
824 zraw_f = ((float)( (int16_t)((accDataBuffer[5] << 8) | (accDataBuffer[4]))));
|
|
825
|
|
826 rotate_accel_3f(&xraw_f, &yraw_f, &zraw_f);
|
|
827
|
|
828 // mh
|
|
829 accel_report_x = xraw_f;
|
|
830 accel_report_y = yraw_f;
|
358
|
831 accel_report_z = -zraw_f; // flip Z in gen 2 hardware
|
357
|
832
|
|
833 accel_DX_f = ((int16_t)(accel_report_x));
|
|
834 accel_DY_f = ((int16_t)(accel_report_y));
|
|
835 accel_DZ_f = ((int16_t)(accel_report_z));
|
|
836 }
|
|
837
|
|
838
|
|
839 // ===============================================================================
|
|
840 // compass_read_LSM303AGR
|
|
841 ///
|
|
842 /// output is compass_DX_f, compass_DY_f, compass_DZ_f
|
|
843 // ===============================================================================
|
|
844 void compass_read_LSM303AGR(void)
|
|
845 {
|
|
846 uint8_t data;
|
|
847 // float xraw_f, yraw_f, zraw_f;
|
|
848 // float mag_report_x, mag_report_y, mag_report_z;
|
|
849
|
|
850 memset(magDataBuffer,0,6);
|
|
851
|
|
852 compass_DX_f = 0;
|
|
853 compass_DY_f = 0;
|
|
854 compass_DZ_f = 0;
|
|
855
|
|
856 for(int i=0;i<6;i++)
|
|
857 {
|
|
858 data = 0x68 + i; // OUTX_L_REG_M
|
|
859 I2C_Master_Transmit( DEVICE_COMPASS_303AGR, &data, 1);
|
|
860 I2C_Master_Receive( DEVICE_COMPASS_303AGR, &magDataBuffer[i], 1);
|
|
861 }
|
|
862
|
|
863 // mh 160620 flip x and y if flip display
|
|
864 compass_DX_f = (((int16_t)((magDataBuffer[1] << 8) | (magDataBuffer[0]))));
|
|
865 compass_DY_f = (((int16_t)((magDataBuffer[3] << 8) | (magDataBuffer[2]))));
|
|
866 compass_DZ_f = (((int16_t)((magDataBuffer[5] << 8) | (magDataBuffer[4]))));
|
358
|
867
|
|
868 // align axis in gen 2 hardware
|
|
869 compass_DZ_f *= -1;
|
|
870
|
357
|
871 return;
|
38
|
872 }
|
|
873
|
|
874
|
|
875 // --------------------------------------------------------------------------------
|
|
876 // ----------EARLIER COMPONENTS ---------------------------------------------------
|
|
877 // --------------------------------------------------------------------------------
|
|
878
|
|
879 // ===============================================================================
|
|
880 // compass_init_HMC5883L
|
|
881 /// @brief The horrible Honeywell compass chip
|
|
882 /// This might be called several times during calibration
|
|
883 ///
|
|
884 /// @param fast: 1 is fast mode, 0 is normal mode
|
|
885 /// @param gain: 7 is max gain and set with here, compass_calib() might reduce it
|
|
886 // ===============================================================================
|
|
887 void compass_init_HMC5883L(uint8_t fast, uint8_t gain)
|
|
888 {
|
|
889 uint8_t write_buffer[4];
|
|
890
|
|
891 compass_gain = gain;
|
|
892 uint16_t length = 0;
|
|
893 write_buffer[0] = 0x00; // 00 = config Register A
|
|
894
|
|
895 if( fast )
|
|
896 write_buffer[1] = 0x38; // 0b 0011 1000; // ConfigA: 75Hz, 2 Samples averaged
|
|
897 else
|
|
898 write_buffer[1] = 0x68; // 0b 0110 1000; // ConfigA: 3Hz, 8 Samples averaged
|
|
899
|
|
900 switch(gain)
|
|
901 {
|
|
902 case 7:
|
|
903 write_buffer[2] = 0xE0; //0b 1110 0000; // ConfigB: gain
|
|
904 break;
|
|
905 case 6:
|
|
906 write_buffer[2] = 0xC0; //0b 1100 0000; // ConfigB: gain
|
|
907 break;
|
|
908 case 5:
|
|
909 write_buffer[2] = 0xA0; //0b 1010 0000; // ConfigB: gain
|
|
910 break;
|
|
911 case 4:
|
|
912 write_buffer[2] = 0x80; //0b 1000 0000; // ConfigB: gain
|
|
913 break;
|
|
914 case 3:
|
|
915 write_buffer[2] = 0x60; //0b 0110 0000; // ConfigB: gain
|
|
916 break;
|
|
917 case 2:
|
|
918 write_buffer[2] = 0x40; //0b 01000 0000; // ConfigB: gain
|
|
919 break;
|
|
920 case 1:
|
|
921 write_buffer[2] = 0x20; //0b 00100 0000; // ConfigB: gain
|
|
922 break;
|
|
923 case 0:
|
|
924 write_buffer[2] = 0x00; //0b 00000 0000; // ConfigB: gain
|
|
925 break;
|
|
926 }
|
|
927 write_buffer[3] = 0x00; // Mode: continuous mode
|
|
928 length = 4;
|
|
929 //hmc_twi_write(0);
|
|
930 I2C_Master_Transmit( DEVICE_COMPASS_HMC5883L, write_buffer, length);
|
|
931 }
|
|
932
|
|
933
|
|
934
|
|
935 // ===============================================================================
|
|
936 // compass_sleep_HMC5883L
|
|
937 /// @brief Power-down mode for Honeywell compass chip
|
|
938 // ===============================================================================
|
|
939 void compass_sleep_HMC5883L(void)
|
|
940 {
|
|
941 uint8_t write_buffer[4];
|
|
942 uint16_t length = 0;
|
|
943
|
|
944 write_buffer[0] = 0x00; // 00 = config Register A
|
|
945 write_buffer[1] = 0x68; // 0b 0110 1000; // ConfigA
|
|
946 write_buffer[2] = 0x20; // 0b 0010 0000; // ConfigB
|
|
947 write_buffer[3] = 0x02; // 0b 0000 0010; // Idle Mode
|
|
948 length = 4;
|
|
949 I2C_Master_Transmit( DEVICE_COMPASS_HMC5883L, write_buffer, length);
|
|
950 }
|
|
951
|
|
952
|
|
953 // ===============================================================================
|
|
954 // accelerator_init_MMA8452Q
|
|
955 /// @brief Power-down mode for acceleration chip used in combination with Honeywell compass
|
|
956 // ===============================================================================
|
|
957 void accelerator_init_MMA8452Q(void)
|
|
958 {
|
|
959 uint8_t write_buffer[4];
|
|
960 uint16_t length = 0;
|
|
961 //HAL_Delay(1);
|
|
962 //return;
|
|
963 write_buffer[0] = 0x0E; // XYZ_DATA_CFG
|
|
964 write_buffer[1] = 0x00;//0b00000000; // High pass Filter=0 , +/- 2g range
|
|
965 length = 2;
|
|
966 I2C_Master_Transmit( DEVICE_ACCELARATOR_MMA8452Q, write_buffer, length);
|
|
967 //HAL_Delay(1);
|
|
968 write_buffer[0] = 0x2A; // CTRL_REG1
|
|
969 write_buffer[1] = 0x34; //0b00110100; // CTRL_REG1: 160ms data rate, St.By Mode, reduced noise mode
|
|
970 write_buffer[2] = 0x02; //0b00000010; // CTRL_REG2: High Res in Active mode
|
|
971 length = 3;
|
|
972 I2C_Master_Transmit( DEVICE_ACCELARATOR_MMA8452Q, write_buffer, length);
|
|
973
|
|
974 //HAL_Delay(1);
|
|
975 //hw_delay_us(100);
|
|
976 write_buffer[0] = 0x2A; // CTRL_REG1
|
|
977 write_buffer[1] = 0x35; //0b00110101; // CTRL_REG1: ... Active Mode
|
|
978 length = 2;
|
|
979 I2C_Master_Transmit( DEVICE_ACCELARATOR_MMA8452Q, write_buffer, length);
|
|
980 /*
|
|
981 HAL_Delay(6);
|
|
982 compass_read();
|
|
983 HAL_Delay(1);
|
|
984 acceleration_read();
|
|
985
|
|
986 compass_calc();
|
|
987 */
|
|
988 }
|
|
989
|
|
990
|
|
991 // ===============================================================================
|
|
992 // accelerator_sleep_MMA8452Q
|
|
993 /// @brief compass_sleep_HMC5883L
|
|
994 // ===============================================================================
|
|
995 void accelerator_sleep_MMA8452Q(void)
|
|
996 {
|
|
997 uint16_t length = 0;
|
|
998 uint8_t write_buffer[4];
|
|
999
|
|
1000 write_buffer[0] = 0x2A; // CTRL_REG1
|
|
1001 write_buffer[1] = 0x00; //0b00000000; // CTRL_REG1: Standby Mode
|
|
1002 length = 2;
|
|
1003 I2C_Master_Transmit( DEVICE_ACCELARATOR_MMA8452Q, write_buffer, length);
|
|
1004 }
|
|
1005
|
|
1006
|
|
1007 // ===============================================================================
|
|
1008 // compass_read_HMC5883L
|
|
1009 /// @brief The new ST 303D - get ALL data and store in static variables
|
|
1010 ///
|
|
1011 /// output is compass_DX_f, compass_DY_f, compass_DZ_f
|
|
1012 // ===============================================================================
|
|
1013 void compass_read_HMC5883L(void)
|
|
1014 {
|
|
1015 uint8_t buffer[20];
|
|
1016 compass_DX_f = 0;
|
|
1017 compass_DY_f = 0;
|
|
1018 compass_DZ_f = 0;
|
|
1019 uint8_t length = 0;
|
|
1020 uint8_t read_buffer[6];
|
|
1021 signed_tword data;
|
|
1022 for(int i = 0; i<6;i++)
|
|
1023 read_buffer[i] = 0;
|
|
1024 buffer[0] = 0x03; // 03 = Data Output X MSB Register
|
|
1025 length = 1;
|
|
1026 I2C_Master_Transmit( DEVICE_COMPASS_HMC5883L, buffer, length);
|
|
1027 length = 6;
|
|
1028 I2C_Master_Receive( DEVICE_COMPASS_HMC5883L, read_buffer, length);
|
|
1029
|
|
1030
|
|
1031 data.Byte.hi = read_buffer[0];
|
|
1032 data.Byte.low = read_buffer[1];
|
|
1033 //Y = Z
|
|
1034 compass_DY_f = - data.Word;
|
|
1035
|
|
1036 data.Byte.hi = read_buffer[2];
|
|
1037 data.Byte.low = read_buffer[3];
|
|
1038 compass_DZ_f = data.Word;
|
|
1039
|
|
1040 data.Byte.hi = read_buffer[4];
|
|
1041 data.Byte.low = read_buffer[5];
|
|
1042 //X = -Y
|
|
1043 compass_DX_f = data.Word;
|
|
1044 }
|
|
1045
|
|
1046
|
|
1047 // ===============================================================================
|
|
1048 // acceleration_read_MMA8452Q
|
|
1049 /// @brief The old MMA8452Q used with the Honeywell compass
|
|
1050 /// get the acceleration data and store in static variables
|
|
1051 ///
|
|
1052 /// output is accel_DX_f, accel_DY_f, accel_DZ_f
|
|
1053 // ===============================================================================
|
|
1054 void acceleration_read_MMA8452Q(void)
|
|
1055 {
|
|
1056 uint8_t buffer[20];
|
|
1057 accel_DX_f = 0;
|
|
1058 accel_DY_f = 0;
|
|
1059 accel_DZ_f = 0;
|
|
1060 uint8_t length = 0;
|
|
1061 // bit8_Type status ;
|
|
1062 uint8_t read_buffer[7];
|
|
1063 signed_tword data;
|
|
1064 for(int i = 0; i<6;i++)
|
|
1065 read_buffer[i] = 0;
|
|
1066 buffer[0] = 0x00; // 03 = Data Output X MSB Register
|
|
1067 length = 1;
|
|
1068 I2C_Master_Transmit( DEVICE_ACCELARATOR_MMA8452Q, buffer, length);
|
|
1069 length = 7;
|
|
1070 I2C_Master_Receive( DEVICE_ACCELARATOR_MMA8452Q, read_buffer, length);
|
|
1071
|
|
1072 // status.uw = read_buffer[0];
|
|
1073 data.Byte.hi = read_buffer[1];
|
|
1074 data.Byte.low = read_buffer[2];
|
|
1075 accel_DX_f =data.Word/16;
|
|
1076 data.Byte.hi = read_buffer[3];
|
|
1077 data.Byte.low = read_buffer[4];
|
|
1078 accel_DY_f =data.Word/16;
|
|
1079 data.Byte.hi = read_buffer[5];
|
|
1080 data.Byte.low = read_buffer[6];
|
|
1081 accel_DZ_f =data.Word/16;
|
|
1082
|
|
1083 accel_DX_f *= -1;
|
|
1084 accel_DY_f *= -1;
|
|
1085 accel_DZ_f *= -1;
|
|
1086 }
|
|
1087
|
|
1088
|
|
1089 // ===============================================================================
|
|
1090 // compass_calc_roll_pitch_only
|
|
1091 /// @brief only the roll and pitch parts of compass_calc()
|
|
1092 ///
|
|
1093 /// input is accel_DX_f, accel_DY_f, accel_DZ_f
|
|
1094 /// output is compass_pitch and compass_roll
|
|
1095 // ===============================================================================
|
|
1096 void compass_calc_roll_pitch_only(void)
|
|
1097 {
|
|
1098 float sinPhi, cosPhi;
|
|
1099 float Phi, Teta;
|
|
1100
|
|
1101 //---- Calculate sine and cosine of roll angle Phi -----------------------
|
|
1102 Phi= atan2f(accel_DY_f, accel_DZ_f) ;
|
|
1103 compass_roll = Phi * 180.0f /PI;
|
|
1104 sinPhi = sinf(Phi);
|
|
1105 cosPhi = cosf(Phi);
|
|
1106
|
|
1107 //---- calculate sin and cosine of pitch angle Theta ---------------------
|
|
1108 Teta = atanf(-(float)accel_DX_f/(accel_DY_f * sinPhi + accel_DZ_f * cosPhi));
|
|
1109 compass_pitch = Teta * 180.0f /PI;
|
|
1110 }
|
|
1111
|
|
1112
|
|
1113 // ===============================================================================
|
|
1114 // compass_calc
|
|
1115 /// @brief all the fancy stuff first implemented in OSTC3
|
|
1116 ///
|
|
1117 /// input is compass_DX_f, compass_DY_f, compass_DZ_f, accel_DX_f, accel_DY_f, accel_DZ_f
|
|
1118 /// and compass_CX_f, compass_CY_f, compass_CZ_f
|
|
1119 /// output is compass_heading, compass_pitch and compass_roll
|
|
1120 // ===============================================================================
|
|
1121 void compass_calc(void)
|
|
1122 {
|
|
1123 float sinPhi, cosPhi, sinTeta, cosTeta;
|
|
1124 float Phi, Teta, Psi;
|
|
1125 int16_t iBfx, iBfy;
|
|
1126 int16_t iBpx, iBpy, iBpz;
|
|
1127
|
|
1128 //---- Make hard iron correction -----------------------------------------
|
|
1129 // Measured magnetometer orientation, measured ok.
|
|
1130 // From matthias drawing: (X,Y,Z) --> (X,Y,Z) : no rotation.
|
|
1131 iBpx = compass_DX_f - compass_CX_f; // X
|
|
1132 iBpy = compass_DY_f - compass_CY_f; // Y
|
|
1133 iBpz = compass_DZ_f - compass_CZ_f; // Z
|
|
1134
|
|
1135 //---- Calculate sine and cosine of roll angle Phi -----------------------
|
|
1136 //sincos(accel_DZ_f, accel_DY_f, &sin, &cos);
|
|
1137 Phi= atan2f(accel_DY_f, accel_DZ_f) ;
|
|
1138 compass_roll = Phi * 180.0f /PI;
|
|
1139 sinPhi = sinf(Phi);
|
|
1140 cosPhi = cosf(Phi);
|
|
1141
|
|
1142 //---- rotate by roll angle (-Phi) ---------------------------------------
|
|
1143 iBfy = iBpy * cosPhi - iBpz * sinPhi;
|
|
1144 iBpz = iBpy * sinPhi + iBpz * cosPhi;
|
|
1145 //Gz = imul(accel_DY_f, sin) + imul(accel_DZ_f, cos);
|
|
1146
|
|
1147 //---- calculate sin and cosine of pitch angle Theta ---------------------
|
|
1148 //sincos(Gz, -accel_DX_f, &sin, &cos); // NOTE: changed sin sign.
|
|
1149 // Teta takes into account roll of computer and sends combination of Y and Z :-) understand now hw 160421
|
|
1150 Teta = atanf(-(float)accel_DX_f/(accel_DY_f * sinPhi + accel_DZ_f * cosPhi));
|
|
1151 compass_pitch = Teta * 180.0f /PI;
|
|
1152 sinTeta = sinf(Teta);
|
|
1153 cosTeta = cosf(Teta);
|
|
1154 /* correct cosine if pitch not in range -90 to 90 degrees */
|
|
1155 if( cosTeta < 0 ) cosTeta = -cosTeta;
|
|
1156
|
|
1157 ///---- de-rotate by pitch angle Theta -----------------------------------
|
|
1158 iBfx = iBpx * cosTeta + iBpz * sinTeta;
|
|
1159
|
|
1160 //---- Detect uncalibrated compass ---------------------------------------
|
|
1161 if( !compass_CX_f && !compass_CY_f && !compass_CZ_f )
|
|
1162 {
|
|
1163 compass_heading = -1;
|
|
1164 return;
|
|
1165 }
|
|
1166
|
|
1167 //---- calculate current yaw = e-compass angle Psi -----------------------
|
|
1168 // Result in degree (no need of 0.01 deg precision...
|
|
1169 Psi = atan2f(-iBfy,iBfx);
|
|
1170 compass_heading = Psi * 180.0f /PI;
|
|
1171 // Result in 0..360 range:
|
|
1172 if( compass_heading < 0 )
|
|
1173 compass_heading += 360;
|
|
1174 }
|
|
1175
|
|
1176
|
|
1177 // //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
|
|
1178 // // - Calibration - ///////////////////////////////////////////////////////////////////////////////////////////////////////
|
|
1179 // //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
|
|
1180
|
|
1181 /* can be lost during sleep as those are reset with compass_reset_calibration() */
|
|
1182
|
|
1183 // ===============================================================================
|
|
1184 // compass_reset_calibration
|
|
1185 /// @brief all the fancy stuff first implemented in OSTC3
|
|
1186 ///
|
|
1187 /// output is struct g and compass_CX_f, compass_CY_f, compass_CZ_f
|
|
1188 ///
|
|
1189 /// @param g: is a struct with crazy stuff like Suuu, Svvv, Svvu, etc.
|
|
1190 /// all is set to zero here
|
|
1191 // ===============================================================================
|
|
1192 void compass_reset_calibration(SCompassCalib *g)
|
|
1193 {
|
|
1194 g->compass_N = 0;
|
|
1195 g->Su = g->Sv = g->Sw = 0.0;
|
|
1196 g->Suu = g->Svv = g->Sww = g->Suv = g->Suw = g->Svw = 0.0;
|
|
1197 g->Suuu = g->Svvv = g->Swww = 0.0;
|
|
1198 g->Suuv = g->Suuw = g->Svvu = g->Svvw = g->Swwu = g->Swwv = 0.0;
|
|
1199 compass_CX_f = compass_CY_f = compass_CZ_f = 0.0;
|
|
1200 }
|
|
1201
|
|
1202
|
|
1203 // ===============================================================================
|
|
1204 // compass_add_calibration
|
|
1205 /// @brief all the fancy stuff first implemented in OSTC3
|
|
1206 ///
|
|
1207 /// input is compass_DX_f, compass_DY_f, compass_DZ_f
|
|
1208 /// and compass_CX_f, compass_CY_f, compass_CZ_f
|
|
1209 /// output is struct g
|
|
1210 ///
|
|
1211 /// @param g: is a struct with crazy stuff like Suuu, Svvv, Svvu, etc.
|
|
1212 // ===============================================================================
|
|
1213 void compass_add_calibration(SCompassCalib *g)
|
|
1214 {
|
|
1215 float u, v, w;
|
|
1216
|
|
1217 u = (compass_DX_f - compass_CX_f) / 32768.0f;
|
|
1218 v = (compass_DY_f - compass_CY_f) / 32768.0f;
|
|
1219 w = (compass_DZ_f - compass_CZ_f) / 32768.0f;
|
|
1220
|
|
1221 g->compass_N++;
|
|
1222 g->Su += u;
|
|
1223 g->Sv += v;
|
|
1224 g->Sw += w;
|
|
1225 g->Suv += u*v;
|
|
1226 g->Suw += u*w;
|
|
1227 g->Svw += v*w;
|
|
1228 g->Suu += u*u;
|
|
1229 g->Suuu += u*u*u;
|
|
1230 g->Suuv += v*u*u;
|
|
1231 g->Suuw += w*u*u;
|
|
1232 g->Svv += v*v;
|
|
1233 g->Svvv += v*v*v;
|
|
1234 g->Svvu += u*v*v;
|
|
1235 g->Svvw += w*v*v;
|
|
1236 g->Sww += w*w;
|
|
1237 g->Swww += w*w*w;
|
|
1238 g->Swwu += u*w*w;
|
|
1239 g->Swwv += v*w*w;
|
|
1240 }
|
|
1241
|
|
1242 //////////////////////////////////////////////////////////////////////////////
|
|
1243
|
|
1244 // ===============================================================================
|
|
1245 // compass_solve_calibration
|
|
1246 /// @brief all the fancy stuff first implemented in OSTC3
|
|
1247 ///
|
|
1248 /// input is compass_CX_f, compass_CY_f, compass_CZ_f and g
|
|
1249 /// output is struct g
|
|
1250 ///
|
|
1251 /// @param g: is a struct with crazy stuff like Suuu, Svvv, Svvu, etc.
|
|
1252 // ===============================================================================
|
|
1253 void compass_solve_calibration(SCompassCalib *g)
|
|
1254 {
|
|
1255 float yu, yv, yw;
|
|
1256 float delta;
|
|
1257 float uc, vc, wc;
|
|
1258
|
|
1259
|
|
1260 //---- Normalize partial sums --------------------------------------------
|
|
1261 //
|
|
1262 // u, v, w should be centered on the mean value um, vm, wm:
|
|
1263 // x = u + um, with um = Sx/N
|
|
1264 //
|
|
1265 // So:
|
|
1266 // (u + um)**2 = u**2 + 2u*um + um**2
|
|
1267 // Su = 0, um = Sx/N
|
|
1268 // Sxx = Suu + 2 um Su + N*(Sx/N)**2 = Suu + Sx**2/N
|
|
1269 // Suu = Sxx - Sx**2/N
|
|
1270 yu = g->Su/g->compass_N;
|
|
1271 yv = g->Sv/g->compass_N;
|
|
1272 yw = g->Sw/g->compass_N;
|
|
1273
|
|
1274 g->Suu -= g->Su*yu;
|
|
1275 g->Svv -= g->Sv*yv;
|
|
1276 g->Sww -= g->Sw*yw;
|
|
1277
|
|
1278 // (u + um)(v + vm) = uv + u vm + v um + um vm
|
|
1279 // Sxy = Suv + N * um vm
|
|
1280 // Suv = Sxy - N * (Sx/N)(Sy/N);
|
|
1281 g->Suv -= g->Su*yv;
|
|
1282 g->Suw -= g->Su*yw;
|
|
1283 g->Svw -= g->Sv*yw;
|
|
1284
|
|
1285 // (u + um)**3 = u**3 + 3 u**2 um + 3 u um**2 + um**3
|
|
1286 // Sxxx = Suuu + 3 um Suu + 3 um**2 Su + N.um**3
|
|
1287 // Su = 0, um = Sx/N:
|
|
1288 // Suuu = Sxxx - 3 Sx*Suu/N - N.(Sx/N)**3
|
|
1289 // = Sxxx - 3 Sx*Suu/N - Sx**3/N**2
|
|
1290
|
|
1291 // (u + um)**2 (v + vm) = (u**2 + 2 u um + um**2)(v + vm)
|
|
1292 // Sxxy = Suuv + vm Suu + 2 um (Suv + vm Su) + um**2 (Sv + N.vm)
|
|
1293 //
|
|
1294 // Su = 0, Sv = 0, vm = Sy/N:
|
|
1295 // Sxxy = Suuv + vm Suu + 2 um Suv + N um**2 vm
|
|
1296 //
|
|
1297 // Suuv = Sxxy - (Sy/N) Suu - 2 (Sx/N) Suv - (Sx/N)**2 Sy
|
|
1298 // = Sxxy - Suu*Sy/N - 2 Suv*Sx/N - Sx*Sx*Sy/N/N
|
|
1299 // = Sxxy - (Suu + Sx*Sx/N)*Sy/N - 2 Suv*Sx/N
|
|
1300 g->Suuu -= (3*g->Suu + g->Su*yu)*yu;
|
|
1301 g->Suuv -= (g->Suu + g->Su*yu)*yv + 2*g->Suv*yu;
|
|
1302 g->Suuw -= (g->Suu + g->Su*yu)*yw + 2*g->Suw*yu;
|
|
1303
|
|
1304 g->Svvu -= (g->Svv + g->Sv*yv)*yu + 2*g->Suv*yv;
|
|
1305 g->Svvv -= (3*g->Svv + g->Sv*yv)*yv;
|
|
1306 g->Svvw -= (g->Svv + g->Sv*yv)*yw + 2*g->Svw*yv;
|
|
1307
|
|
1308 g->Swwu -= (g->Sww + g->Sw*yw)*yu + 2*g->Suw*yw;
|
|
1309 g->Swwv -= (g->Sww + g->Sw*yw)*yv + 2*g->Svw*yw;
|
|
1310 g->Swww -= (3*g->Sww + g->Sw*yw)*yw;
|
|
1311
|
|
1312 //---- Solve the system --------------------------------------------------
|
|
1313 // uc Suu + vc Suv + wc Suw = (Suuu + Svvu + Swwu) / 2
|
|
1314 // uc Suv + vc Svv + wc Svw = (Suuv + Svvv + Swwv) / 2
|
|
1315 // uc Suw + vc Svw + wc Sww = (Suuw + Svvw + Swww) / 2
|
|
1316 // Note this is symetric, with a positiv diagonal, hence
|
|
1317 // it always have a uniq solution.
|
|
1318 yu = 0.5f * (g->Suuu + g->Svvu + g->Swwu);
|
|
1319 yv = 0.5f * (g->Suuv + g->Svvv + g->Swwv);
|
|
1320 yw = 0.5f * (g->Suuw + g->Svvw + g->Swww);
|
|
1321 delta = g->Suu * (g->Svv * g->Sww - g->Svw * g->Svw)
|
|
1322 - g->Suv * (g->Suv * g->Sww - g->Svw * g->Suw)
|
|
1323 + g->Suw * (g->Suv * g->Svw - g->Svv * g->Suw);
|
|
1324
|
|
1325 uc = (yu * (g->Svv * g->Sww - g->Svw * g->Svw)
|
|
1326 - yv * (g->Suv * g->Sww - g->Svw * g->Suw)
|
|
1327 + yw * (g->Suv * g->Svw - g->Svv * g->Suw) )/delta;
|
|
1328 vc = (g->Suu * ( yv * g->Sww - yw * g->Svw)
|
|
1329 - g->Suv * ( yu * g->Sww - yw * g->Suw)
|
|
1330 + g->Suw * ( yu * g->Svw - yv * g->Suw) )/delta;
|
|
1331 wc = (g->Suu * (g->Svv * yw - g->Svw * yv )
|
|
1332 - g->Suv * (g->Suv * yw - g->Svw * yu )
|
|
1333 + g->Suw * (g->Suv * yv - g->Svv * yu ) )/delta;
|
|
1334
|
|
1335 // Back to uncentered coordinates:
|
|
1336 // xc = um + uc
|
|
1337 uc = g->Su/g->compass_N + compass_CX_f/32768.0f + uc;
|
|
1338 vc = g->Sv/g->compass_N + compass_CY_f/32768.0f + vc;
|
|
1339 wc = g->Sw/g->compass_N + compass_CZ_f/32768.0f + wc;
|
|
1340
|
|
1341 // Then save the new calibrated center:
|
|
1342 compass_CX_f = (short)(32768 * uc);
|
|
1343 compass_CY_f = (short)(32768 * vc);
|
|
1344 compass_CZ_f = (short)(32768 * wc);
|
|
1345 }
|
|
1346
|
|
1347
|
|
1348 // ===============================================================================
|
|
1349 // compass_calib
|
|
1350 /// @brief the main loop for calibration
|
|
1351 /// output is compass_CX_f, compass_CY_f, compass_CZ_f and g
|
|
1352 /// 160704 removed -4096 limit for LSM303D
|
|
1353 ///
|
|
1354 /// @return always 0
|
|
1355 // ===============================================================================
|
|
1356 int compass_calib_common(void)
|
|
1357 {
|
410
|
1358 SCompassCalib g;
|
38
|
1359
|
|
1360 // Starts with no calibration at all:
|
410
|
1361 compass_reset_calibration(&g);
|
|
1362 uint32_t tickstart = 0;
|
|
1363 tickstart = HAL_GetTick();
|
|
1364 /* run calibration for one minute */
|
|
1365 while(time_elapsed_ms(tickstart,HAL_GetTick()) < 60000)
|
38
|
1366 {
|
410
|
1367 while((SPI_Evaluate_RX_Data() == 0) && (time_elapsed_ms(tickstart,HAL_GetTick()) < 60000))
|
|
1368 {
|
|
1369 HAL_Delay(1);
|
|
1370 }
|
|
1371 compass_read();
|
|
1372 acceleration_read();
|
|
1373 compass_calc_roll_pitch_only();
|
38
|
1374
|
410
|
1375 if((hardwareCompass == compass_generation1 ) //HMC5883L)
|
38
|
1376 &&((compass_DX_f == -4096) ||
|
410
|
1377 (compass_DY_f == -4096) ||
|
|
1378 (compass_DZ_f == -4096) ))
|
|
1379 {
|
|
1380 if(compass_gain == 0)
|
|
1381 return -1;
|
|
1382 compass_gain--;
|
|
1383 compass_init(1, compass_gain);
|
|
1384 compass_reset_calibration(&g);
|
|
1385 continue;
|
|
1386 }
|
38
|
1387
|
410
|
1388 copyCompassDataDuringCalibration(compass_DX_f,compass_DY_f,compass_DZ_f);
|
|
1389 compass_add_calibration(&g);
|
104
|
1390 }
|
38
|
1391
|
|
1392 compass_solve_calibration(&g);
|
|
1393
|
410
|
1394 tfull32 dataBlock[4];
|
|
1395 dataBlock[0].Word16.low16 = compass_CX_f;
|
|
1396 dataBlock[0].Word16.hi16 = compass_CY_f;
|
|
1397 dataBlock[1].Word16.low16 = compass_CZ_f;
|
|
1398 dataBlock[1].Word16.hi16 = 0xFFFF;
|
|
1399 dataBlock[2].Full32 = 0x7FFFFFFF;
|
|
1400 dataBlock[3].Full32 = 0x7FFFFFFF;
|
|
1401 BFA_writeDataBlock((uint32_t *)dataBlock);
|
38
|
1402
|
410
|
1403 return 0;
|
38
|
1404 }
|
|
1405
|