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