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