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