Mercurial > public > ostc4

comparison Small_CPU/Src/compass.c @ 357:c3d511365552

Find changesets by keywords (author, files, the commit message), revision number or hash, or revset expression.
Add Support for new end-2019 hardware: support LSM303AGR compass (Not yet working!) cleanup compass code a bit
author heinrichsweikamp
date Sat, 23 Nov 2019 18:39:50 +0100
parents 49f5db6139d5
children c6a084d1433f
comparison
equal deleted inserted replaced
356:cb3870f79e9d 357:c3d511365552
107 107
108 108
109 #define Q_PI (18000) 109 #define Q_PI (18000)
110 #define Q_PIO2 (9000) 110 #define Q_PIO2 (9000)
111 111
112 #define HMC5883L (1) ///< id used with hardwareCompass
113 #define LSM303D (2) ///< id used with hardwareCompass
114 #define COMPASS_NOT_RECOGNIZED (4) ///< id used with hardwareCompass
115 112
116 113
117 ////////////////////////////////////////////////////////////////////////////// 114 //////////////////////////////////////////////////////////////////////////////
118 // fifth order of polynomial approximation of atan(), giving 0.05 deg max error 115 // fifth order of polynomial approximation of atan(), giving 0.05 deg max error
119 // 116 //
152 float compass_pitch; ///< the final result calculated in compass_calc() 149 float compass_pitch; ///< the final result calculated in compass_calc()
153 150
154 151
155 uint8_t compass_gain; ///< 7 on start, can be reduced during calibration 152 uint8_t compass_gain; ///< 7 on start, can be reduced during calibration
156 153
157 uint8_t hardwareCompass = 0; ///< either HMC5883L or LSM303D or not defined yet ( = 0 ) 154 uint8_t hardwareCompass = 0; ///< either HMC5883L (=1) or LSM303D (=2) or LSM303AGR (=3) or not defined yet (=0)
158 155
159 /// LSM303D variables 156 /// LSM303D variables
160 uint8_t magDataBuffer[6]; ///< here raw data from LSM303D is stored, can be local 157 uint8_t magDataBuffer[6]; ///< here raw data from LSM303D is stored, can be local
161 uint8_t accDataBuffer[6]; ///< here raw data from LSM303D is stored, can be local 158 uint8_t accDataBuffer[6]; ///< here raw data from LSM303D is stored, can be local
162 159
163 //uint16_t velMag = 0;
164 //uint16_t velAcc = 0;
165
166 //uint16_t magODR[] = {31,62,125,250,500,1000,2000};
167 //uint16_t accODR[] = {0,31,62,125,250,500,1000,2000,4000,8000,16000};
168 //uint8_t fastest = 10; //no sensor is the fastest
169 //uint8_t datas1 = 0;
170 //uint8_t zoffFlag = 0;
171 //uint8_t sendFlag = 0;
172
173
174 // all by pixhawk code:
175 160
176 // struct accel_scale _accel_scale; 161 // struct accel_scale _accel_scale;
177 unsigned _accel_range_m_s2; 162 unsigned _accel_range_m_s2;
178 float _accel_range_scale; 163 float _accel_range_scale;
179 unsigned _accel_samplerate; 164 unsigned _accel_samplerate;
221 void compass_init_LSM303D(uint8_t fast, uint8_t gain); 206 void compass_init_LSM303D(uint8_t fast, uint8_t gain);
222 void compass_sleep_LSM303D(void); 207 void compass_sleep_LSM303D(void);
223 void compass_read_LSM303D(void); 208 void compass_read_LSM303D(void);
224 void acceleration_read_LSM303D(void); 209 void acceleration_read_LSM303D(void);
225 210
211 void compass_init_LSM303AGR(uint8_t fast, uint8_t gain);
212 void compass_sleep_LSM303AGR(void);
213 void compass_read_LSM303AGR(void);
214 void acceleration_read_LSM303AGR(void);
215
226 int LSM303D_accel_set_onchip_lowpass_filter_bandwidth(unsigned bandwidth); 216 int LSM303D_accel_set_onchip_lowpass_filter_bandwidth(unsigned bandwidth);
227 int compass_calib_common(void); 217 int compass_calib_common(void);
228 218
229 void compass_calc_roll_pitch_only(void); 219 void compass_calc_roll_pitch_only(void);
230 220
263 if(hardwareCompass == 0) 253 if(hardwareCompass == 0)
264 { 254 {
265 uint8_t data = WHO_AM_I; 255 uint8_t data = WHO_AM_I;
266 I2C_Master_Transmit( DEVICE_COMPASS_303D, &data, 1); 256 I2C_Master_Transmit( DEVICE_COMPASS_303D, &data, 1);
267 I2C_Master_Receive( DEVICE_COMPASS_303D, &data, 1); 257 I2C_Master_Receive( DEVICE_COMPASS_303D, &data, 1);
268 if(data == WHOIAM_VALUE) 258 if(data == WHOIAM_VALUE_LSM303D)
269 hardwareCompass = LSM303D; 259 hardwareCompass = compass_generation2; //LSM303D;
270 else 260 else
271 hardwareCompass = HMC5883L; 261 hardwareCompass = compass_generation1; //HMC5883L
262 if(data == WHOIAM_VALUE_LSM303AGR)
263 hardwareCompass = compass_generation3; //LSM303AGR;
272 } 264 }
273 265
274 /* No compass identified => Retry */ 266 /* No compass identified => Retry */
275 if(hardwareCompass == 0) 267 if(hardwareCompass == 0)
276 { 268 {
277 uint8_t data = WHO_AM_I; 269 uint8_t data = WHO_AM_I;
278 I2C_Master_Transmit( DEVICE_COMPASS_303D, &data, 1); 270 I2C_Master_Transmit( DEVICE_COMPASS_303D, &data, 1);
279 I2C_Master_Receive( DEVICE_COMPASS_303D, &data, 1); 271 I2C_Master_Receive( DEVICE_COMPASS_303D, &data, 1);
280 if(data == WHOIAM_VALUE) 272 if(data == WHOIAM_VALUE_LSM303D)
281 hardwareCompass = LSM303D; 273 hardwareCompass = compass_generation2; //LSM303D;
282 else 274 else
283 hardwareCompass = HMC5883L; 275 hardwareCompass = compass_generation1; //HMC5883L;
276 if(data == WHOIAM_VALUE_LSM303AGR)
277 hardwareCompass = compass_generation3; //LSM303AGR;
284 } 278 }
285 279
286 /* Assume that a HMC5883L is equipped by default if detection still failed */ 280 /* Assume that a HMC5883L is equipped by default if detection still failed */
287 if(hardwareCompass == 0) 281 if(hardwareCompass == 0)
288 hardwareCompass = HMC5883L; 282 hardwareCompass = compass_generation1; //HMC5883L;
289 283
290 #ifdef TEST_IF_HMC5883L 284 #ifdef TEST_IF_HMC5883L
291 HAL_StatusTypeDef resultOfOperationHMC_MMA = HAL_TIMEOUT; 285 HAL_StatusTypeDef resultOfOperationHMC_MMA = HAL_TIMEOUT;
292 286
293 if(hardwareCompass == HMC5883L) 287 if(hardwareCompass == compass_generation1) // HMC5883L)
294 { 288 {
295 uint8_t data = 0x2A; // CTRL_REG1 of DEVICE_ACCELARATOR_MMA8452Q 289 uint8_t data = 0x2A; // CTRL_REG1 of DEVICE_ACCELARATOR_MMA8452Q
296 resultOfOperationHMC_MMA = I2C_Master_Transmit( DEVICE_ACCELARATOR_MMA8452Q, &data, 1); 290 resultOfOperationHMC_MMA = I2C_Master_Transmit( DEVICE_ACCELARATOR_MMA8452Q, &data, 1);
297 if(resultOfOperationHMC_MMA == HAL_OK) 291 if(resultOfOperationHMC_MMA == HAL_OK)
298 { 292 {
299 hardwareCompass = HMC5883L; // all fine, keep it 293 hardwareCompass = compass_generation1; //HMC5883L; // all fine, keep it
300 } 294 }
301 else 295 else
302 { 296 {
303 hardwareCompass = COMPASS_NOT_RECOGNIZED; 297 hardwareCompass = COMPASS_NOT_RECOGNIZED;
304 testCompassTypeDebug = 0xEC; 298 testCompassTypeDebug = 0xEC;
305 } 299 }
306 } 300 }
307 #endif 301 #endif
308 302
309 if(hardwareCompass == LSM303D) 303 if(hardwareCompass == compass_generation2) //LSM303D)
310 {
311 compass_init_LSM303D(fast, gain); 304 compass_init_LSM303D(fast, gain);
312 } 305 if(hardwareCompass == compass_generation3) //LSM303AGR)
313 else 306 compass_init_LSM303AGR(fast, gain);
314 if(hardwareCompass == HMC5883L) 307 if(hardwareCompass == compass_generation1) //HMC5883L)
315 {
316 compass_init_HMC5883L(fast, gain); 308 compass_init_HMC5883L(fast, gain);
317 } 309
318 310 tfull32 dataBlock[4];
319 tfull32 dataBlock[4]; 311 if(BFA_readLastDataBlock((uint32_t *)dataBlock) == BFA_OK)
320 if(BFA_readLastDataBlock((uint32_t *)dataBlock) == BFA_OK)
321 { 312 {
322 compass_CX_f = dataBlock[0].Word16.low16; 313 compass_CX_f = dataBlock[0].Word16.low16;
323 compass_CY_f = dataBlock[0].Word16.hi16; 314 compass_CY_f = dataBlock[0].Word16.hi16;
324 compass_CZ_f = dataBlock[1].Word16.low16; 315 compass_CZ_f = dataBlock[1].Word16.low16;
325 } 316 }
331 // compass_calib 322 // compass_calib
332 /// @brief with onchip_lowpass_filter configuration for accelerometer of LSM303D 323 /// @brief with onchip_lowpass_filter configuration for accelerometer of LSM303D
333 // =============================================================================== 324 // ===============================================================================
334 int compass_calib(void) 325 int compass_calib(void)
335 { 326 {
336 if(hardwareCompass == LSM303D) 327 if(hardwareCompass == compass_generation2) //LSM303D)
337 { 328 {
338 LSM303D_accel_set_onchip_lowpass_filter_bandwidth(773); 329 LSM303D_accel_set_onchip_lowpass_filter_bandwidth(773);
339 int out = compass_calib_common(); 330 int out = compass_calib_common();
340 LSM303D_accel_set_onchip_lowpass_filter_bandwidth(LSM303D_ACCEL_DEFAULT_ONCHIP_FILTER_FREQ); 331 LSM303D_accel_set_onchip_lowpass_filter_bandwidth(LSM303D_ACCEL_DEFAULT_ONCHIP_FILTER_FREQ);
341 return out; 332 return out;
342 } 333 }
343 else 334 else
344 if(hardwareCompass == HMC5883L) 335 if(hardwareCompass == compass_generation1) //HMC5883L)
336 {
337 return compass_calib_common();
338 }
339 else
340 if(hardwareCompass == compass_generation3) //LSM303AGR)
345 { 341 {
346 return compass_calib_common(); 342 return compass_calib_common();
347 } 343 }
348 else 344 else
349 { 345 {
358 // compass_sleep 354 // compass_sleep
359 /// @brief low power mode 355 /// @brief low power mode
360 // =============================================================================== 356 // ===============================================================================
361 void compass_sleep(void) 357 void compass_sleep(void)
362 { 358 {
363 if(hardwareCompass == LSM303D) 359 if(hardwareCompass == compass_generation2) //LSM303D)
364 { 360 {
365 compass_sleep_LSM303D(); 361 compass_sleep_LSM303D();
366 } 362 }
367 else 363 else
368 if(hardwareCompass == HMC5883L) 364 if(hardwareCompass == compass_generation1) //HMC5883L)
369 { 365 {
370 compass_sleep_HMC5883L(); 366 compass_sleep_HMC5883L();
371 } 367 }
372 } 368 }
373 369
377 /// @brief reads magnetometer and accelerometer for LSM303D, 373 /// @brief reads magnetometer and accelerometer for LSM303D,
378 /// otherwise magnetometer only 374 /// otherwise magnetometer only
379 // =============================================================================== 375 // ===============================================================================
380 void compass_read(void) 376 void compass_read(void)
381 { 377 {
382 if(hardwareCompass == LSM303D) 378 if(hardwareCompass == compass_generation2) //LSM303D)
383 {
384 compass_read_LSM303D(); 379 compass_read_LSM303D();
385 } 380 if(hardwareCompass == compass_generation1) //HMC5883L)
386 else
387 if(hardwareCompass == HMC5883L)
388 {
389 compass_read_HMC5883L(); 381 compass_read_HMC5883L();
390 } 382 if(hardwareCompass == compass_generation3) //LSM303AGR)
383 compass_read_LSM303AGR();
384
391 } 385 }
392 386
393 387
394 // =============================================================================== 388 // ===============================================================================
395 // accelerator_init 389 // accelerator_init
396 /// @brief empty for for LSM303D 390 /// @brief empty for for LSM303D
397 // =============================================================================== 391 // ===============================================================================
398 void accelerator_init(void) 392 void accelerator_init(void)
399 { 393 {
400 if(hardwareCompass == HMC5883L) 394 if(hardwareCompass == compass_generation1) //HMC5883L)
401 accelerator_init_MMA8452Q(); 395 accelerator_init_MMA8452Q();
402 } 396 }
403 397
404 398
405 // =============================================================================== 399 // ===============================================================================
406 // accelerator_sleep 400 // accelerator_sleep
407 /// @brief empty for for LSM303D 401 /// @brief empty for for LSM303D
408 // =============================================================================== 402 // ===============================================================================
409 void accelerator_sleep(void) 403 void accelerator_sleep(void)
410 { 404 {
411 if(hardwareCompass == HMC5883L) 405 if(hardwareCompass == compass_generation1) //HMC5883L)
412 accelerator_sleep_MMA8452Q(); 406 accelerator_sleep_MMA8452Q();
413 } 407 }
414 408
415 409
416 // =============================================================================== 410 // ===============================================================================
417 // acceleration_read 411 // acceleration_read
418 /// @brief empty for for LSM303D 412 /// @brief empty for for LSM303D
419 // =============================================================================== 413 // ===============================================================================
420 void acceleration_read(void) 414 void acceleration_read(void)
421 { 415 {
422 if(hardwareCompass == LSM303D) 416 if(hardwareCompass == compass_generation2) //LSM303D)
423 {
424 acceleration_read_LSM303D(); 417 acceleration_read_LSM303D();
425 } 418 if(hardwareCompass == compass_generation1) //HMC5883L)
426 else
427 if(hardwareCompass == HMC5883L)
428 {
429 acceleration_read_MMA8452Q(); 419 acceleration_read_MMA8452Q();
430 } 420 if(hardwareCompass == compass_generation3) //LSM303AGR)
421 acceleration_read_LSM303AGR();
431 } 422 }
432 423
433 424
434 /* Private functions ---------------------------------------------------------*/ 425 /* Private functions ---------------------------------------------------------*/
435 426
436 // =============================================================================== 427 // ===============================================================================
428 // LSM303AGR_read_reg
429 // ===============================================================================
430 uint8_t LSM303AGR_read_reg(uint8_t addr)
431 {
432 uint8_t data;
433
434 I2C_Master_Transmit( DEVICE_COMPASS_303AGR, &addr, 1);
435 I2C_Master_Receive( DEVICE_COMPASS_303AGR, &data, 1);
436 return data;
437 }
438
439
440 // ===============================================================================
441 // LSM303AGR_write_reg
442 // ===============================================================================
443 void LSM303AGR_write_reg(uint8_t addr, uint8_t value)
444 {
445 uint8_t data[2];
446
447 data[0] = addr;
448 data[1] = value;
449 I2C_Master_Transmit( DEVICE_COMPASS_303AGR, data, 2);
450 }
451
452 // ===============================================================================
453 // LSM303AGR_acc_write_reg
454 // ===============================================================================
455 void LSM303AGR_acc_write_reg(uint8_t addr, uint8_t value)
456 {
457 uint8_t data[2];
458
459 data[0] = addr;
460 data[1] = value;
461 I2C_Master_Transmit( DEVICE_ACCELARATOR_303AGR, data, 2);
462 }
463
464
465 // ===============================================================================
466 // LSM303AGR_write_checked_reg
467 // ===============================================================================
468 void LSM303AGR_write_checked_reg(uint8_t addr, uint8_t value)
469 {
470 LSM303AGR_write_reg(addr, value);
471 }
472
473 // ===============================================================================
474 // LSM303AGR_acc_write_checked_reg
475 // ===============================================================================
476 void LSM303AGR_acc_write_checked_reg(uint8_t addr, uint8_t value)
477 {
478 LSM303AGR_acc_write_reg(addr, value);
479 }
480
481 // ===============================================================================
437 // LSM303D_read_reg 482 // LSM303D_read_reg
438 /// @brief tiny helpers by pixhawk
439 // =============================================================================== 483 // ===============================================================================
440 uint8_t LSM303D_read_reg(uint8_t addr) 484 uint8_t LSM303D_read_reg(uint8_t addr)
441 { 485 {
442 uint8_t data; 486 uint8_t data;
443 487
447 } 491 }
448 492
449 493
450 // =============================================================================== 494 // ===============================================================================
451 // LSM303D_write_reg 495 // LSM303D_write_reg
452 /// @brief tiny helpers by pixhawk
453 // =============================================================================== 496 // ===============================================================================
454 void LSM303D_write_reg(uint8_t addr, uint8_t value) 497 void LSM303D_write_reg(uint8_t addr, uint8_t value)
455 { 498 {
456 uint8_t data[2]; 499 uint8_t data[2];
457 500
462 } 505 }
463 506
464 507
465 // =============================================================================== 508 // ===============================================================================
466 // LSM303D_write_checked_reg 509 // LSM303D_write_checked_reg
467 /// @brief tiny helpers by pixhawk. This runs unchecked at the moment.
468 // =============================================================================== 510 // ===============================================================================
469 void LSM303D_write_checked_reg(uint8_t addr, uint8_t value) 511 void LSM303D_write_checked_reg(uint8_t addr, uint8_t value)
470 { 512 {
471 LSM303D_write_reg(addr, value); 513 LSM303D_write_reg(addr, value);
472 } 514 }
473 515
474 516
475 // =============================================================================== 517 // ===============================================================================
476 // LSM303D_modify_reg 518 // LSM303D_modify_reg
477 /// @brief tiny helpers by pixhawk
478 // =============================================================================== 519 // ===============================================================================
479 void LSM303D_modify_reg(unsigned reg, uint8_t clearbits, uint8_t setbits) 520 void LSM303D_modify_reg(unsigned reg, uint8_t clearbits, uint8_t setbits)
480 { 521 {
481 uint8_t val; 522 uint8_t val;
482 523
484 val &= ~clearbits; 525 val &= ~clearbits;
485 val |= setbits; 526 val |= setbits;
486 LSM303D_write_checked_reg(reg, val); 527 LSM303D_write_checked_reg(reg, val);
487 } 528 }
488 529
489 /*
490 // ===============================================================================
491 // LSM303D_accel_set_range
492 /// @brief tiny helpers by pixhawk
493 // ===============================================================================
494 int LSM303D_accel_set_range(unsigned max_g)
495 {
496 uint8_t setbits = 0;
497 uint8_t clearbits = REG2_FULL_SCALE_BITS_A;
498 float new_scale_g_digit = 0.0f;
499
500 if (max_g == 0) {
501 max_g = 16;
502 }
503
504 if (max_g <= 2) {
505 _accel_range_m_s2 = 2.0f * LSM303D_ONE_G;
506 setbits |= REG2_FULL_SCALE_2G_A;
507 new_scale_g_digit = 0.061e-3f;
508
509 } else if (max_g <= 4) {
510 _accel_range_m_s2 = 4.0f * LSM303D_ONE_G;
511 setbits |= REG2_FULL_SCALE_4G_A;
512 new_scale_g_digit = 0.122e-3f;
513
514 } else if (max_g <= 6) {
515 _accel_range_m_s2 = 6.0f * LSM303D_ONE_G;
516 setbits |= REG2_FULL_SCALE_6G_A;
517 new_scale_g_digit = 0.183e-3f;
518
519 } else if (max_g <= 8) {
520 _accel_range_m_s2 = 8.0f * LSM303D_ONE_G;
521 setbits |= REG2_FULL_SCALE_8G_A;
522 new_scale_g_digit = 0.244e-3f;
523
524 } else if (max_g <= 16) {
525 _accel_range_m_s2 = 16.0f * LSM303D_ONE_G;
526 setbits |= REG2_FULL_SCALE_16G_A;
527 new_scale_g_digit = 0.732e-3f;
528
529 } else {
530 return -1;
531 }
532
533 _accel_range_scale = new_scale_g_digit * LSM303D_ONE_G;
534
535
536 LSM303D_modify_reg(ADDR_CTRL_REG2, clearbits, setbits);
537
538 return 0;
539 }
540 */
541 /*
542 // ===============================================================================
543 // LSM303D_mag_set_range
544 /// @brief tiny helpers by pixhawk
545 // ===============================================================================
546 int LSM303D_mag_set_range(unsigned max_ga)
547 {
548 uint8_t setbits = 0;
549 uint8_t clearbits = REG6_FULL_SCALE_BITS_M;
550 float new_scale_ga_digit = 0.0f;
551
552 if (max_ga == 0) {
553 max_ga = 12;
554 }
555
556 if (max_ga <= 2) {
557 _mag_range_ga = 2;
558 setbits |= REG6_FULL_SCALE_2GA_M;
559 new_scale_ga_digit = 0.080e-3f;
560
561 } else if (max_ga <= 4) {
562 _mag_range_ga = 4;
563 setbits |= REG6_FULL_SCALE_4GA_M;
564 new_scale_ga_digit = 0.160e-3f;
565
566 } else if (max_ga <= 8) {
567 _mag_range_ga = 8;
568 setbits |= REG6_FULL_SCALE_8GA_M;
569 new_scale_ga_digit = 0.320e-3f;
570
571 } else if (max_ga <= 12) {
572 _mag_range_ga = 12;
573 setbits |= REG6_FULL_SCALE_12GA_M;
574 new_scale_ga_digit = 0.479e-3f;
575
576 } else {
577 return -1;
578 }
579
580 _mag_range_scale = new_scale_ga_digit;
581
582 LSM303D_modify_reg(ADDR_CTRL_REG6, clearbits, setbits);
583
584 return 0;
585 }
586 */
587
588 // =============================================================================== 530 // ===============================================================================
589 // LSM303D_accel_set_onchip_lowpass_filter_bandwidth 531 // LSM303D_accel_set_onchip_lowpass_filter_bandwidth
590 /// @brief tiny helpers by pixhawk
591 // =============================================================================== 532 // ===============================================================================
592 int LSM303D_accel_set_onchip_lowpass_filter_bandwidth(unsigned bandwidth) 533 int LSM303D_accel_set_onchip_lowpass_filter_bandwidth(unsigned bandwidth)
593 { 534 {
594 uint8_t setbits = 0; 535 uint8_t setbits = 0;
595 uint8_t clearbits = REG2_ANTIALIAS_FILTER_BW_BITS_A; 536 uint8_t clearbits = REG2_ANTIALIAS_FILTER_BW_BITS_A;
624 } 565 }
625 566
626 567
627 // =============================================================================== 568 // ===============================================================================
628 // LSM303D_accel_set_driver_lowpass_filter 569 // LSM303D_accel_set_driver_lowpass_filter
629 /// @brief tiny helpers by pixhawk. This one is not used at the moment!
630 // =============================================================================== 570 // ===============================================================================
631 int LSM303D_accel_set_driver_lowpass_filter(float samplerate, float bandwidth) 571 int LSM303D_accel_set_driver_lowpass_filter(float samplerate, float bandwidth)
632 { 572 {
633 /* 573 /*
634 _accel_filter_x_set_cutoff_frequency(samplerate, bandwidth); 574 _accel_filter_x_set_cutoff_frequency(samplerate, bandwidth);
635 _accel_filter_y_set_cutoff_frequency(samplerate, bandwidth); 575 _accel_filter_y_set_cutoff_frequency(samplerate, bandwidth);
636 _accel_filter_z_set_cutoff_frequency(samplerate, bandwidth); 576 _accel_filter_z_set_cutoff_frequency(samplerate, bandwidth);
637 */ 577 */
638 return 0; 578 return 0;
639 } 579 }
640
641 /* unused 170821
642 // ===============================================================================
643 // LSM303D_accel_set_samplerate
644 /// @brief tiny helpers by pixhawk
645 // ===============================================================================
646 int LSM303D_accel_set_samplerate(unsigned frequency)
647 {
648 uint8_t setbits = 0;
649 uint8_t clearbits = REG1_RATE_BITS_A;
650
651 // if (frequency == 0 || frequency == ACCEL_SAMPLERATE_DEFAULT) {
652 frequency = 1600;
653 // }
654
655 if (frequency <= 3) {
656 setbits |= REG1_RATE_3_125HZ_A;
657 _accel_samplerate = 3;
658
659 } else if (frequency <= 6) {
660 setbits |= REG1_RATE_6_25HZ_A;
661 _accel_samplerate = 6;
662
663 } else if (frequency <= 12) {
664 setbits |= REG1_RATE_12_5HZ_A;
665 _accel_samplerate = 12;
666
667 } else if (frequency <= 25) {
668 setbits |= REG1_RATE_25HZ_A;
669 _accel_samplerate = 25;
670
671 } else if (frequency <= 50) {
672 setbits |= REG1_RATE_50HZ_A;
673 _accel_samplerate = 50;
674
675 } else if (frequency <= 100) {
676 setbits |= REG1_RATE_100HZ_A;
677 _accel_samplerate = 100;
678
679 } else if (frequency <= 200) {
680 setbits |= REG1_RATE_200HZ_A;
681 _accel_samplerate = 200;
682
683 } else if (frequency <= 400) {
684 setbits |= REG1_RATE_400HZ_A;
685 _accel_samplerate = 400;
686
687 } else if (frequency <= 800) {
688 setbits |= REG1_RATE_800HZ_A;
689 _accel_samplerate = 800;
690
691 } else if (frequency <= 1600) {
692 setbits |= REG1_RATE_1600HZ_A;
693 _accel_samplerate = 1600;
694
695 } else {
696 return -1;
697 }
698
699 LSM303D_modify_reg(ADDR_CTRL_REG1, clearbits, setbits);
700 return 0;
701 }
702 // ===============================================================================
703 // LSM303D_mag_set_samplerate
704 /// @brief tiny helpers by pixhawk
705 // ===============================================================================
706 int LSM303D_mag_set_samplerate(unsigned frequency)
707 {
708 uint8_t setbits = 0;
709 uint8_t clearbits = REG5_RATE_BITS_M;
710
711 if (frequency == 0) {
712 frequency = 100;
713 }
714
715 if (frequency <= 3) {
716 setbits |= REG5_RATE_3_125HZ_M;
717 _mag_samplerate = 25;
718
719 } else if (frequency <= 6) {
720 setbits |= REG5_RATE_6_25HZ_M;
721 _mag_samplerate = 25;
722
723 } else if (frequency <= 12) {
724 setbits |= REG5_RATE_12_5HZ_M;
725 _mag_samplerate = 25;
726
727 } else if (frequency <= 25) {
728 setbits |= REG5_RATE_25HZ_M;
729 _mag_samplerate = 25;
730
731 } else if (frequency <= 50) {
732 setbits |= REG5_RATE_50HZ_M;
733 _mag_samplerate = 50;
734
735 } else if (frequency <= 100) {
736 setbits |= REG5_RATE_100HZ_M;
737 _mag_samplerate = 100;
738
739 } else {
740 return -1;
741 }
742
743 LSM303D_modify_reg(ADDR_CTRL_REG5, clearbits, setbits);
744 return 0;
745 }
746 */
747 580
748 581
749 // rotate_mag_3f: nicht genutzt aber praktisch; rotate_accel_3f wird benutzt 582 // rotate_mag_3f: nicht genutzt aber praktisch; rotate_accel_3f wird benutzt
750 // =============================================================================== 583 // ===============================================================================
751 // rotate_mag_3f 584 // rotate_mag_3f
784 */ 617 */
785 } 618 }
786 619
787 620
788 // =============================================================================== 621 // ===============================================================================
789 // compass_init_LSM303D by PIXhawk (LSM303D::reset()) 622 // compass_init_LSM303D
790 // https://raw.githubusercontent.com/PX4/Firmware/master/src/drivers/lsm303d/lsm303d.cpp
791 /// @brief The new ST 303D
792 /// This might be called several times with different gain values during calibration 623 /// This might be called several times with different gain values during calibration
793 /// but gain change is not supported at the moment. 624 /// but gain change is not supported at the moment.
794 /// 625 ///
795 /// @param gain: 7 is max gain and set with here, compass_calib() might reduce it 626 /// @param gain: 7 is max gain and set with here, compass_calib() might reduce it
796 // =============================================================================== 627 // ===============================================================================
797 628
798 //uint8_t testCompassLS303D[11]; 629 //uint8_t testCompassLS303D[11];
799 630
800 void compass_init_LSM303D(uint8_t fast, uint8_t gain) 631 void compass_init_LSM303D(uint8_t fast, uint8_t gain)
801 { 632 {
802 // matthias version 160620
803 if(fast == 0) 633 if(fast == 0)
804 { 634 {
805 LSM303D_write_checked_reg(ADDR_CTRL_REG0, 0x00); 635 LSM303D_write_checked_reg(ADDR_CTRL_REG0, 0x00);
806 LSM303D_write_checked_reg(ADDR_CTRL_REG1, 0x3F); // mod 12,5 Hz 3 instead of 6,25 Hz 2 636 LSM303D_write_checked_reg(ADDR_CTRL_REG1, 0x3F); // mod 12,5 Hz 3 instead of 6,25 Hz 2
807 LSM303D_write_checked_reg(ADDR_CTRL_REG2, 0xC0); 637 LSM303D_write_checked_reg(ADDR_CTRL_REG2, 0xC0);
819 LSM303D_write_checked_reg(ADDR_CTRL_REG5, 0x74); // 100 Hz 649 LSM303D_write_checked_reg(ADDR_CTRL_REG5, 0x74); // 100 Hz
820 } 650 }
821 LSM303D_write_checked_reg(ADDR_CTRL_REG6, 0x00); 651 LSM303D_write_checked_reg(ADDR_CTRL_REG6, 0x00);
822 LSM303D_write_checked_reg(ADDR_CTRL_REG7, 0x00); 652 LSM303D_write_checked_reg(ADDR_CTRL_REG7, 0x00);
823 653
824 /*
825 uint8_t data;
826 for(int i=0;i<11;i++)
827 {
828 data = ADDR_INT_THS_L_M + i;
829 I2C_Master_Transmit( DEVICE_COMPASS_303D, &data, 1);
830 I2C_Master_Receive( DEVICE_COMPASS_303D, &testCompassLS303D[i], 1);
831 }
832 */
833
834 return; 654 return;
835 /*
836 LSM303D_accel_set_range(LSM303D_ACCEL_DEFAULT_RANGE_G); // modifies ADDR_CTRL_REG2
837 LSM303D_accel_set_samplerate(LSM303D_ACCEL_DEFAULT_RATE); // modifies ADDR_CTRL_REG1
838
839 LSM303D_mag_set_range(LSM303D_MAG_DEFAULT_RANGE_GA);
840 LSM303D_mag_set_samplerate(LSM303D_MAG_DEFAULT_RATE);
841 */
842
843 /*
844 // my stuff hw
845 // enable accel
846 LSM303D_write_checked_reg(ADDR_CTRL_REG1,
847 REG1_X_ENABLE_A | REG1_Y_ENABLE_A | REG1_Z_ENABLE_A | REG1_BDU_UPDATE | REG1_RATE_800HZ_A);
848
849 // enable mag
850 LSM303D_write_checked_reg(ADDR_CTRL_REG7, REG7_CONT_MODE_M);
851 LSM303D_write_checked_reg(ADDR_CTRL_REG5, REG5_RES_HIGH_M | REG5_ENABLE_T);
852 LSM303D_write_checked_reg(ADDR_CTRL_REG3, 0x04); // DRDY on ACCEL on INT1
853 LSM303D_write_checked_reg(ADDR_CTRL_REG4, 0x04); // DRDY on MAG on INT2
854
855 LSM303D_accel_set_range(LSM303D_ACCEL_DEFAULT_RANGE_G);
856 LSM303D_accel_set_samplerate(LSM303D_ACCEL_DEFAULT_RATE);
857 LSM303D_accel_set_driver_lowpass_filter((float)LSM303D_ACCEL_DEFAULT_RATE, (float)LSM303D_ACCEL_DEFAULT_DRIVER_FILTER_FREQ);
858 //LSM303D_accel_set_onchip_lowpass_filter_bandwidth(773); // factory setting
859
860 // we setup the anti-alias on-chip filter as 50Hz. We believe
861 // this operates in the analog domain, and is critical for
862 // anti-aliasing. The 2 pole software filter is designed to
863 // operate in conjunction with this on-chip filter
864 if(fast)
865 LSM303D_accel_set_onchip_lowpass_filter_bandwidth(773); // factory setting
866 else
867 LSM303D_accel_set_onchip_lowpass_filter_bandwidth(LSM303D_ACCEL_DEFAULT_ONCHIP_FILTER_FREQ);
868
869
870 LSM303D_mag_set_range(LSM303D_MAG_DEFAULT_RANGE_GA);
871 LSM303D_mag_set_samplerate(LSM303D_MAG_DEFAULT_RATE);
872 */
873 } 655 }
874 656
875 657
876 // =============================================================================== 658 // ===============================================================================
877 // compass_sleep_LSM303D 659 // compass_sleep_LSM303D
878 /// @brief The new compass chip, hopefully this works! 660 // @brief Gen 2 chip
879 // =============================================================================== 661 // ===============================================================================
880 void compass_sleep_LSM303D(void) 662 void compass_sleep_LSM303D(void)
881 { 663 {
882 LSM303D_write_checked_reg(ADDR_CTRL_REG1, 0x00); // CNTRL1: acceleration sensor Power-down mode 664 LSM303D_write_checked_reg(ADDR_CTRL_REG1, 0x00); // CNTRL1: acceleration sensor Power-down mode
883 LSM303D_write_checked_reg(ADDR_CTRL_REG7, 0x02); // CNTRL7: magnetic sensor Power-down mode 665 LSM303D_write_checked_reg(ADDR_CTRL_REG7, 0x02); // CNTRL7: magnetic sensor Power-down mode
884 } 666 }
885 667
886 668
887 // =============================================================================== 669 // ===============================================================================
888 // acceleration_read_LSM303D 670 // acceleration_read_LSM303D
889 /// @brief The new LSM303D, code by pixhawk 671 // output is accel_DX_f, accel_DY_f, accel_DZ_f
890 ///
891 /// output is accel_DX_f, accel_DY_f, accel_DZ_f
892 // =============================================================================== 672 // ===============================================================================
893 void acceleration_read_LSM303D(void) 673 void acceleration_read_LSM303D(void)
894 { 674 {
895 uint8_t data; 675 uint8_t data;
896 float xraw_f, yraw_f, zraw_f; 676 float xraw_f, yraw_f, zraw_f;
918 // mh 698 // mh
919 accel_report_x = xraw_f; 699 accel_report_x = xraw_f;
920 accel_report_y = yraw_f; 700 accel_report_y = yraw_f;
921 accel_report_z = zraw_f; 701 accel_report_z = zraw_f;
922 702
923 // my stuff
924 /*
925 accel_report_x = ((xraw_f * _accel_range_scale) - _accel_scale_x_offset) * _accel_scale_x_scale;
926 accel_report_y = ((yraw_f * _accel_range_scale) - _accel_scale_y_offset) * _accel_scale_y_scale;
927 accel_report_z = ((zraw_f * _accel_range_scale) - _accel_scale_z_offset) * _accel_scale_z_scale;
928 */
929 accel_DX_f = ((int16_t)(accel_report_x)); 703 accel_DX_f = ((int16_t)(accel_report_x));
930 accel_DY_f = ((int16_t)(accel_report_y)); 704 accel_DY_f = ((int16_t)(accel_report_y));
931 accel_DZ_f = ((int16_t)(accel_report_z)); 705 accel_DZ_f = ((int16_t)(accel_report_z));
932 } 706 }
933 /* special code after accel_report_z = ...
934 * prior to output
935 // we have logs where the accelerometers get stuck at a fixed
936 // large value. We want to detect this and mark the sensor as
937 // being faulty
938
939 if (fabsf(_last_accel[0] - x_in_new) < 0.001f &&
940 fabsf(_last_accel[1] - y_in_new) < 0.001f &&
941 fabsf(_last_accel[2] - z_in_new) < 0.001f &&
942 fabsf(x_in_new) > 20 &&
943 fabsf(y_in_new) > 20 &&
944 fabsf(z_in_new) > 20) {
945 _constant_accel_count += 1;
946
947 } else {
948 _constant_accel_count = 0;
949 }
950
951 if (_constant_accel_count > 100) {
952 // we've had 100 constant accel readings with large
953 // values. The sensor is almost certainly dead. We
954 // will raise the error_count so that the top level
955 // flight code will know to avoid this sensor, but
956 // we'll still give the data so that it can be logged
957 // and viewed
958 perf_count(_bad_values);
959 _constant_accel_count = 0;
960 }
961
962 _last_accel[0] = x_in_new;
963 _last_accel[1] = y_in_new;
964 _last_accel[2] = z_in_new;
965
966 accel_report.x = _accel_filter_x.apply(x_in_new);
967 accel_report.y = _accel_filter_y.apply(y_in_new);
968 accel_report.z = _accel_filter_z.apply(z_in_new);
969
970 math::Vector<3> aval(x_in_new, y_in_new, z_in_new);
971 math::Vector<3> aval_integrated;
972
973 bool accel_notify = _accel_int.put(accel_report.timestamp, aval, aval_integrated, accel_report.integral_dt);
974 accel_report.x_integral = aval_integrated(0);
975 accel_report.y_integral = aval_integrated(1);
976 accel_report.z_integral = aval_integrated(2);
977 */
978 707
979 708
980 // =============================================================================== 709 // ===============================================================================
981 // compass_read_LSM303D 710 // compass_read_LSM303D
982 /// @brief The new LSM303D, code by pixhawk
983 /// 711 ///
984 /// output is compass_DX_f, compass_DY_f, compass_DZ_f 712 /// output is compass_DX_f, compass_DY_f, compass_DZ_f
985 // =============================================================================== 713 // ===============================================================================
986 void compass_read_LSM303D(void) 714 void compass_read_LSM303D(void)
987 { 715 {
1006 compass_DX_f = (((int16_t)((magDataBuffer[1] << 8) | (magDataBuffer[0])))); 734 compass_DX_f = (((int16_t)((magDataBuffer[1] << 8) | (magDataBuffer[0]))));
1007 compass_DY_f = (((int16_t)((magDataBuffer[3] << 8) | (magDataBuffer[2])))); 735 compass_DY_f = (((int16_t)((magDataBuffer[3] << 8) | (magDataBuffer[2]))));
1008 compass_DZ_f = (((int16_t)((magDataBuffer[5] << 8) | (magDataBuffer[4])))); 736 compass_DZ_f = (((int16_t)((magDataBuffer[5] << 8) | (magDataBuffer[4]))));
1009 // no rotation 737 // no rotation
1010 return; 738 return;
1011 /* 739 }
1012 // my stuff 740
1013 compass_DX_f = (((int16_t)((magDataBuffer[1] << 8) | (magDataBuffer[0]))) / 10) - 200; 741
1014 compass_DY_f = (((int16_t)((magDataBuffer[3] << 8) | (magDataBuffer[2]))) / 10) - 200; 742 // ===============================================================================
1015 compass_DZ_f = (((int16_t)((magDataBuffer[5] << 8) | (magDataBuffer[4]))) / 10) - 200; 743 // compass_init_LSM303AGR
1016 */ 744 /// This might be called several times with different gain values during calibration
1017 // old 745 /// but gain change is not supported at the moment.
1018 /* 746 ///
1019 xraw_f = ((float)( (int16_t)((magDataBuffer[1] << 8) | (magDataBuffer[0])))); 747 /// @param gain: 7 is max gain and set with here, compass_calib() might reduce it
1020 yraw_f = ((float)( (int16_t)((magDataBuffer[3] << 8) | (magDataBuffer[2])))); 748 // ===============================================================================
1021 zraw_f = ((float)( (int16_t)((magDataBuffer[5] << 8) | (magDataBuffer[4])))); 749
1022 750 void compass_init_LSM303AGR(uint8_t fast, uint8_t gain)
1023 rotate_mag_3f(&xraw_f, &yraw_f, &zraw_f); 751 {
1024 752 if(fast == 0)
1025 compass_DX_f = (int16_t)((xraw_f * 0.1f) - 200.0f); 753 {
1026 compass_DY_f = (int16_t)((yraw_f * 0.1f) - 200.0f); 754 LSM303AGR_write_checked_reg(0x60, 0x80); // 10Hz
1027 compass_DZ_f = (int16_t)((zraw_f * 0.1f) - 200.0f); 755 LSM303AGR_write_checked_reg(0x61, 0x03); // CFG_REG_B_M
1028 */ 756 LSM303AGR_write_checked_reg(0x62, 0x10); // CFG_REG_C_M
1029 /* 757 }
1030 mag_report_x = ((xraw_f * _mag_range_scale) - _mag_scale_x_offset) * _mag_scale_x_scale; 758 else
1031 mag_report_y = ((yraw_f * _mag_range_scale) - _mag_scale_y_offset) * _mag_scale_y_scale; 759 {
1032 mag_report_z = ((zraw_f * _mag_range_scale) - _mag_scale_z_offset) * _mag_scale_z_scale; 760 LSM303AGR_write_checked_reg(0x60, 0x80); // 10Hz
1033 761 LSM303AGR_write_checked_reg(0x61, 0x03); // CFG_REG_B_M
1034 compass_DX_f = (int16_t)(mag_report_x * 1000.0f); // 1000.0 is just a wild guess by hw 762 LSM303AGR_write_checked_reg(0x62, 0x10); // CFG_REG_C_M
1035 compass_DY_f = (int16_t)(mag_report_y * 1000.0f); 763 }
1036 compass_DZ_f = (int16_t)(mag_report_z * 1000.0f); 764 // init accel (Same chip, but different address...)
1037 */ 765 LSM303AGR_acc_write_checked_reg(0x1F, 0x00); // TEMP_CFG_REG_A (Temp sensor off)
766 LSM303AGR_acc_write_checked_reg(0x20, 0x4F); // CTRL_REG1_A (10Hz, x,y,z = ON)
767 LSM303AGR_acc_write_checked_reg(0x21, 0x00); // CTRL_REG2_A
768 LSM303AGR_acc_write_checked_reg(0x22, 0x00); // CTRL_REG3_A
769 LSM303AGR_acc_write_checked_reg(0x23, 0x04); // CTRL_REG4_A
770
771 return;
772 }
773
774
775 // ===============================================================================
776 // compass_sleep_LSM303D
777 // @brief Gen 2 chip
778 // ===============================================================================
779 void compass_sleep_LSM303AGR(void)
780 {
781 LSM303AGR_write_checked_reg(0x60, 0x03); //
782 LSM303AGR_write_checked_reg(0x61, 0x04); //
783 LSM303AGR_write_checked_reg(0x62, 0x51); //
784 LSM303AGR_write_checked_reg(0x63, 0x00); //
785
786
787 LSM303AGR_acc_write_checked_reg(0x1F, 0x00); //
788 LSM303AGR_acc_write_checked_reg(0x20, 0x00); //
789 }
790
791
792 // ===============================================================================
793 // acceleration_read_LSM303AGR
794 // output is accel_DX_f, accel_DY_f, accel_DZ_f
795 // ===============================================================================
796 void acceleration_read_LSM303AGR(void)
797 {
798 uint8_t data;
799 float xraw_f, yraw_f, zraw_f;
800 float accel_report_x, accel_report_y, accel_report_z;
801
802 memset(accDataBuffer,0,6);
803
804 accel_DX_f = 0;
805 accel_DY_f = 0;
806 accel_DZ_f = 0;
807
808 for(int i=0;i<6;i++)
809 {
810 data = ADDR_OUT_X_L_A + i; // ADDR_OUT_X_L_A is the same as in the LSM303D (luckily)
811 I2C_Master_Transmit( DEVICE_ACCELARATOR_303AGR, &data, 1);
812 I2C_Master_Receive( DEVICE_ACCELARATOR_303AGR, &accDataBuffer[i], 1);
813 }
814
815 xraw_f = ((float)( (int16_t)((accDataBuffer[1] << 8) | (accDataBuffer[0]))));
816 yraw_f = ((float)( (int16_t)((accDataBuffer[3] << 8) | (accDataBuffer[2]))));
817 zraw_f = ((float)( (int16_t)((accDataBuffer[5] << 8) | (accDataBuffer[4]))));
818
819 rotate_accel_3f(&xraw_f, &yraw_f, &zraw_f);
820
821 // mh
822 accel_report_x = xraw_f;
823 accel_report_y = yraw_f;
824 accel_report_z = zraw_f;
825
826 accel_DX_f = ((int16_t)(accel_report_x));
827 accel_DY_f = ((int16_t)(accel_report_y));
828 accel_DZ_f = ((int16_t)(accel_report_z));
829 }
830
831
832 // ===============================================================================
833 // compass_read_LSM303AGR
834 ///
835 /// output is compass_DX_f, compass_DY_f, compass_DZ_f
836 // ===============================================================================
837 void compass_read_LSM303AGR(void)
838 {
839 uint8_t data;
840 // float xraw_f, yraw_f, zraw_f;
841 // float mag_report_x, mag_report_y, mag_report_z;
842
843 memset(magDataBuffer,0,6);
844
845 compass_DX_f = 0;
846 compass_DY_f = 0;
847 compass_DZ_f = 0;
848
849 for(int i=0;i<6;i++)
850 {
851 data = 0x68 + i; // OUTX_L_REG_M
852 I2C_Master_Transmit( DEVICE_COMPASS_303AGR, &data, 1);
853 I2C_Master_Receive( DEVICE_COMPASS_303AGR, &magDataBuffer[i], 1);
854 }
855
856 // mh 160620 flip x and y if flip display
857 compass_DX_f = (((int16_t)((magDataBuffer[1] << 8) | (magDataBuffer[0]))));
858 compass_DY_f = (((int16_t)((magDataBuffer[3] << 8) | (magDataBuffer[2]))));
859 compass_DZ_f = (((int16_t)((magDataBuffer[5] << 8) | (magDataBuffer[4]))));
860 // no rotation
861 return;
1038 } 862 }
1039 863
1040 864
1041 // -------------------------------------------------------------------------------- 865 // --------------------------------------------------------------------------------
1042 // ----------EARLIER COMPONENTS --------------------------------------------------- 866 // ----------EARLIER COMPONENTS ---------------------------------------------------
1338 if( compass_heading < 0 ) 1162 if( compass_heading < 0 )
1339 compass_heading += 360; 1163 compass_heading += 360;
1340 } 1164 }
1341 1165
1342 1166
1343 /*
1344 // ===============================================================================
1345 // compass_calc_mini_during_calibration
1346 /// @brief all the fancy stuff first implemented in OSTC3
1347 ///
1348 /// input is accel_DX_f, accel_DY_f, accel_DZ_f
1349 /// output is compass_pitch and compass_roll
1350 // ===============================================================================
1351 void compass_calc_mini_during_calibration(void)
1352 {
1353 float sinPhi, cosPhi;
1354 float Phi, Teta;
1355
1356 //---- Calculate sine and cosine of roll angle Phi -----------------------
1357 //sincos(accel_DZ_f, accel_DY_f, &sin, &cos);
1358 Phi= atan2f(accel_DY_f, accel_DZ_f) ;
1359 compass_roll = Phi * 180.0f /PI;
1360 sinPhi = sinf(Phi);
1361 cosPhi = cosf(Phi);
1362
1363 //---- calculate sin and cosine of pitch angle Theta ---------------------
1364 //sincos(Gz, -accel_DX_f, &sin, &cos); // NOTE: changed sin sign.
1365 Teta = atanf(-(float)accel_DX_f/(accel_DY_f * sinPhi + accel_DZ_f * cosPhi));
1366 compass_pitch = Teta * 180.0f /PI;
1367 }
1368 */
1369
1370
1371 // ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 1167 // //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
1372 // // - Calibration - /////////////////////////////////////////////////////////////////////////////////////////////////////// 1168 // // - Calibration - ///////////////////////////////////////////////////////////////////////////////////////////////////////
1373 // ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 1169 // //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
1374 1170
1375 /* can be lost during sleep as those are reset with compass_reset_calibration() */ 1171 /* can be lost during sleep as those are reset with compass_reset_calibration() */
1563 { 1359 {
1564 compass_read(); 1360 compass_read();
1565 acceleration_read(); 1361 acceleration_read();
1566 compass_calc_roll_pitch_only(); 1362 compass_calc_roll_pitch_only();
1567 1363
1568 if((hardwareCompass == HMC5883L) 1364 if((hardwareCompass == compass_generation1 ) //HMC5883L)
1569 &&((compass_DX_f == -4096) || 1365 &&((compass_DX_f == -4096) ||
1570 (compass_DY_f == -4096) || 1366 (compass_DY_f == -4096) ||
1571 (compass_DZ_f == -4096) )) 1367 (compass_DZ_f == -4096) ))
1572 { 1368 {
1573 if(compass_gain == 0) 1369 if(compass_gain == 0)
1605 BFA_writeDataBlock((uint32_t *)dataBlock); 1401 BFA_writeDataBlock((uint32_t *)dataBlock);
1606 1402
1607 return 0; 1403 return 0;
1608 } 1404 }
1609 1405
1610 // //////////////////////////// TEST CODE /////////////////////////////////////
1611
1612
1613
1614 //#include <QtDebug>
1615 //#include <stdio.h>
1616 //#include <math.h>
1617 /*#include <stdlib.h>
1618
1619 short compass_DX_f, compass_DY_f, compass_DZ_f;
1620 short compass_CX_f, compass_CY_f, compass_CZ_f;
1621
1622 inline float uniform(void) {
1623 return (rand() & 0xFFFF) / 65536.0f;
1624 }
1625 inline float sqr(float x) {
1626 return x*x;
1627 }
1628
1629 static const float radius = 0.21f;
1630 static const float cx = 0.79f, cy = -0.46f, cz = 0.24f;
1631 // const float cx = 0, cy = 0, cz = 0;
1632
1633 float check_compass_calib(void)
1634 {
1635
1636 // Starts with no calibration at all:
1637 compass_CX_f = compass_CY_f = compass_CZ_f = 0;
1638
1639 // Try 10 recalibration passes:
1640 for(int p=0; p<10; ++p)
1641 {
1642 compass_reset_calibration();
1643
1644 //---- Generates random points on a sphere -------------------------------
1645 // of radius,center (cx, cy, cz):
1646 for(int i=0; i<100; ++i)
1647 {
1648 float theta = uniform()*360.0f;
1649 float phi = uniform()*180.0f - 90.0f;
1650
1651 float x = cx + radius * cosf(phi)*cosf(theta);
1652 float y = cy + radius * cosf(phi)*sinf(theta);
1653 float z = cz + radius * sinf(phi);
1654
1655 compass_DX_f = (short)(32768 * x);
1656 compass_DY_f = (short)(32768 * y);
1657 compass_DZ_f = (short)(32768 * z);
1658 compass_add_calibration();
1659 }
1660
1661 compass_solve_calibration();
1662 //qDebug() << "Center ="
1663 // << compass_CX_f/32768.0f
1664 // << compass_CY_f/32768.0f
1665 // << compass_CZ_f/32768.0f;
1666
1667 float r2 = sqr(compass_CX_f/32768.0f - cx)
1668 + sqr(compass_CY_f/32768.0f - cy)
1669 + sqr(compass_CZ_f/32768.0f - cz);
1670 if( r2 > 0.01f*0.01f )
1671 return sqrtf(r2);
1672 }
1673 return 0;
1674 }*/
1675
1676
1677
1678 /*
1679 void compass_read_LSM303D_v3(void)
1680 {
1681 uint8_t data;
1682
1683 memset(magDataBuffer,0,6);
1684
1685 compass_DX_f = 0;
1686 compass_DY_f = 0;
1687 compass_DZ_f = 0;
1688
1689 //magnetometer multi read, order xl,xh, yl,yh, zl, zh
1690 data = REG_MAG_DATA_ADDR;
1691 I2C_Master_Transmit( DEVICE_COMPASS_303D, &data, 1);
1692 I2C_Master_Receive( DEVICE_COMPASS_303D, magDataBuffer, 6);
1693
1694 compass_DX_f = ((int16_t)( (int16_t)((magDataBuffer[1] << 8) | (magDataBuffer[0]))));
1695 compass_DY_f = ((int16_t)( (int16_t)((magDataBuffer[3] << 8) | (magDataBuffer[2]))));
1696 compass_DZ_f = ((int16_t)( (int16_t)((magDataBuffer[5] << 8) | (magDataBuffer[4]))));
1697
1698 // compass_DX_f = compass_DX_f * stat->sensitivity_mag;
1699 // compass_DY_f = compass_DY_f * stat->sensitivity_mag;
1700 // compass_DZ_f = compass_DZ_f * stat->sensitivity_mag;
1701 }
1702
1703
1704 // ===============================================================================
1705 // compass_init_LSM303D by STMicroelectronics 2013 V1.0.5 2013/Oct/23
1706 /// @brief The new ST 303D
1707 /// This might be called several times with different gain values during calibration
1708 ///
1709 /// @param gain: 7 is max gain and set with here, compass_calib() might reduce it
1710 // ===============================================================================
1711
1712 void compass_init_LSM303D_v3(uint8_t gain)
1713 {
1714 uint8_t data[10];
1715
1716 // CNTRL1
1717 // 0011 acceleration data rate 0011 = 12.5 Hz (3.125 Hz - 1600 Hz)
1718 // 0xxx block data update off
1719 // x111 enable all three axes
1720
1721 // CNTRL5
1722 // 0xxx xxxx temp sensor off
1723 // x00x xxxx magnetic resolution
1724 // xxx0 1xxx magentic data rate 01 = 6,25 Hz (3.125 Hz - 50 Hz (100 Hz))
1725 // xxxx xx00 latch irq requests off
1726
1727 // CNTRL7
1728 // 00xx high pass filter mode, 00 normal mode
1729 // xx0x filter for acceleration data bypassed
1730 // xxx0 temperature sensor mode only off
1731 // x0xx magnetic data low-power mode off
1732 // xx00 magnetic sensor mode 00 = continous-conversion mode (default 10 power-down)
1733
1734 data[0] = CNTRL0;
1735 data[1] = 0x00;
1736 I2C_Master_Transmit( DEVICE_COMPASS_303D, data, 2);
1737
1738 // acc
1739 data[0] = CNTRL1;
1740 data[1] = 0x00;
1741 data[2] = 0x0F;
1742 data[3] = 0x00;
1743 data[4] = 0x00;
1744 I2C_Master_Transmit( DEVICE_COMPASS_303D, data, 5);
1745
1746 // mag
1747 data[0] = CNTRL3;
1748 data[1] = 0x00;
1749 data[2] = 0x00;
1750 data[3] = 0x18;
1751 data[4] = 0x20;
1752 I2C_Master_Transmit( DEVICE_COMPASS_303D, data, 5);
1753
1754 data[0] = CNTRL7;
1755 data[1] = ((MSMS_MASK & CONTINUOS_CONVERSION) |
1756 ((~MSMS_MASK) & CNTRL7_RESUME_VALUE));
1757 I2C_Master_Transmit( DEVICE_COMPASS_303D, data, 2);
1758
1759 HAL_Delay(100);
1760 }
1761
1762
1763 // ===============================================================================
1764 // compass_init_LSM303D by nordevx for arduion
1765 /// @brief The new ST 303D
1766 /// This might be called several times with different gain values during calibration
1767 ///
1768 /// @param gain: 7 is max gain and set with here, compass_calib() might reduce it
1769 // ===============================================================================
1770 void compass_init_LSM303D_v2(uint8_t gain)
1771 {
1772 uint8_t data[2];
1773
1774 // CNTRL1
1775 // 0011 acceleration data rate 0011 = 12.5 Hz (3.125 Hz - 1600 Hz)
1776 // 0xxx block data update off
1777 // x111 enable all three axes
1778
1779 // CNTRL5
1780 // 0xxx xxxx temp sensor off
1781 // x00x xxxx magnetic resolution
1782 // xxx0 1xxx magentic data rate 01 = 6,25 Hz (3.125 Hz - 50 Hz (100 Hz))
1783 // xxxx xx00 latch irq requests off
1784
1785 // CNTRL7
1786 // 00xx high pass filter mode, 00 normal mode
1787 // xx0x filter for acceleration data bypassed
1788 // xxx0 temperature sensor mode only off
1789 // x0xx magnetic data low-power mode off
1790 // xx00 magnetic sensor mode 00 = continous-conversion mode (default 10 power-down)
1791
1792 data[0] = CNTRL1;
1793 data[1] = 0x37; //0b 0011 0111
1794 I2C_Master_Transmit( DEVICE_COMPASS_303D, data, 2);
1795
1796 data[0] = CNTRL5;
1797 data[1] = 0x08; // 0b 0000 1000
1798 I2C_Master_Transmit( DEVICE_COMPASS_303D, data, 2);
1799
1800 data[0] = CNTRL7;
1801 data[1] = 0x00; // 0b 0000 0000
1802 I2C_Master_Transmit( DEVICE_COMPASS_303D, data, 2);
1803
1804 HAL_Delay(100);
1805 }
1806
1807
1808 // ===============================================================================
1809 // compass_init_LSM303D_v1 by ST lsm303d.c
1810 /// @brief The new ST 303D
1811 /// This might be called several times with different gain values during calibration
1812 ///
1813 /// @param gain: 7 is max gain and set with here, compass_calib() might reduce it
1814 // ===============================================================================
1815 void compass_init_LSM303D_v1(uint8_t gain)
1816 {
1817 uint8_t data;
1818
1819 compass_gain = gain;
1820
1821 memset(magDataBuffer,0,6);
1822 memset(accDataBuffer,0,6);
1823
1824 data = CNTRL5;
1825 I2C_Master_Transmit( DEVICE_COMPASS_303D, &data, 1);
1826 I2C_Master_Receive( DEVICE_COMPASS_303D, &data, 1);
1827 data = (data & 0x1c) >> 2;
1828 velMag = magODR[data];
1829
1830 data = CNTRL1;
1831 I2C_Master_Transmit( DEVICE_COMPASS_303D, &data, 1);
1832 I2C_Master_Receive( DEVICE_COMPASS_303D, &data, 1);
1833 data = (data & 0xf0) >> 4;
1834 velAcc = accODR[data];
1835
1836 data = CNTRL7;
1837 I2C_Master_Transmit( DEVICE_COMPASS_303D, &data, 1);
1838 I2C_Master_Receive( DEVICE_COMPASS_303D, &datas1, 1);
1839 datas1 = (datas1 & 0x02);
1840
1841 //if mag is not pd
1842 //mag is bigger than gyro
1843 if( (velMag < velAcc) || datas1 != 0 ) {
1844 //acc is the biggest
1845 fastest = ACC_IS_FASTEST;
1846 }
1847 else {
1848 //acc is the biggest
1849 fastest = MAG_IS_FASTEST;
1850 }
1851
1852 zoffFlag = 1;
1853
1854 if( fastest == MAG_IS_FASTEST)
1855 {
1856 data = STATUS_REG_M;
1857 I2C_Master_Transmit( DEVICE_COMPASS_303D, &data, 1);
1858 I2C_Master_Receive( DEVICE_COMPASS_303D, &data, 1);
1859
1860 // if(ValBit(data, ZYXMDA)) {
1861 sendFlag = 1;
1862 // }
1863
1864 }
1865 else if(fastest == ACC_IS_FASTEST)
1866 {
1867 data = STATUS_REG_A;
1868 I2C_Master_Transmit( DEVICE_COMPASS_303D, &data, 1);
1869 I2C_Master_Receive( DEVICE_COMPASS_303D, &data, 1);
1870 // if(ValBit(data, DATAREADY_BIT)) {
1871 sendFlag = 1;
1872 // }
1873 }
1874 }
1875
1876 // ===============================================================================
1877 // compass_read_LSM303D
1878 /// @brief The new LSM303D :-)
1879 ///
1880 /// output is compass_DX_f, compass_DY_f, compass_DZ_f, accel_DX_f, accel_DY_f, accel_DZ_f
1881 // ===============================================================================
1882 void compass_read_LSM303D_v2(void)
1883 {
1884 uint8_t data;
1885
1886 memset(magDataBuffer,0,6);
1887 memset(accDataBuffer,0,6);
1888
1889 compass_DX_f = 0;
1890 compass_DY_f = 0;
1891 compass_DZ_f = 0;
1892
1893 accel_DX_f = 0;
1894 accel_DY_f = 0;
1895 accel_DZ_f = 0;
1896
1897 //Accelerometer multi read, order xl,xh, yl,yh, zl, zh
1898 data = REG_ACC_DATA_ADDR;
1899 I2C_Master_Transmit( DEVICE_COMPASS_303D, &data, 1);
1900 I2C_Master_Receive( DEVICE_COMPASS_303D, accDataBuffer, 6);
1901
1902 //magnetometer multi read, order xl,xh, yl,yh, zl, zh
1903 data = OUT_X_L_M;
1904 I2C_Master_Transmit( DEVICE_COMPASS_303D, &data, 1);
1905 I2C_Master_Receive( DEVICE_COMPASS_303D, magDataBuffer, 6);
1906
1907 accel_DX_f = ((int16_t)( (int16_t)((accDataBuffer[1] << 8) | (accDataBuffer[0]))));
1908 accel_DY_f = ((int16_t)( (int16_t)((accDataBuffer[3] << 8) | (accDataBuffer[2]))));
1909 accel_DZ_f = ((int16_t)( (int16_t)((accDataBuffer[5] << 8) | (accDataBuffer[4]))));
1910
1911 // accel_DX_f = accel_DX_f * stat->sensitivity_acc;
1912 // accel_DY_f = accel_DY_f * stat->sensitivity_acc;
1913 // accel_DZ_f = accel_DZ_f * stat->sensitivity_acc;
1914
1915
1916 compass_DX_f = magDataBuffer[1];
1917 compass_DX_f *= 256;
1918 compass_DX_f += magDataBuffer[0];
1919
1920 compass_DY_f = magDataBuffer[3];
1921 compass_DY_f *= 256;
1922 compass_DY_f += magDataBuffer[2];
1923
1924 compass_DY_f = magDataBuffer[5];
1925 compass_DY_f *= 256;
1926 compass_DY_f += magDataBuffer[4];
1927
1928 }
1929
1930
1931 */
1932