Mercurial > public > ostc4
annotate Small_CPU/Src/compass.c @ 554:3328189786e7
Added external ADC interface functionality (MCP3424):
Added communication protocoll to read data from external ADC. At the moment 16bit and 18bit resolutions are supported. External data will be read and forwarded depending on the conversion time of the ADC.
| author | Ideenmodellierer |
|---|---|
| date | Thu, 12 Nov 2020 19:46:03 +0100 |
| parents | d9dbfa496f7e |
| children | 573a2bc796c8 |
| 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 | |
|
410
f9458e979154
Bugfix display compass calibration frozen:
ideenmodellierer
parents:
358
diff
changeset
|
42 extern uint32_t time_elapsed_ms(uint32_t ticksstart,uint32_t ticksnow); |
|
f9458e979154
Bugfix display compass calibration frozen:
ideenmodellierer
parents:
358
diff
changeset
|
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
d9dbfa496f7e
Properly send compass 3 into sleepmode (end-2019 hardware)
heinrichsweikamp
parents:
488
diff
changeset
|
263 I2C_DeInit(); |
|
d9dbfa496f7e
Properly send compass 3 into sleepmode (end-2019 hardware)
heinrichsweikamp
parents:
488
diff
changeset
|
264 HAL_Delay(100); |
|
d9dbfa496f7e
Properly send compass 3 into sleepmode (end-2019 hardware)
heinrichsweikamp
parents:
488
diff
changeset
|
265 MX_I2C1_Init(); |
|
d9dbfa496f7e
Properly send compass 3 into sleepmode (end-2019 hardware)
heinrichsweikamp
parents:
488
diff
changeset
|
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
9eeab3fead8f
Added "I2C_DeInit();" in hardware detection routines. It's the recommended way
heinrichsweikamp
parents:
410
diff
changeset
|
288 HAL_Delay(100); |
|
9eeab3fead8f
Added "I2C_DeInit();" in hardware detection routines. It's the recommended way
heinrichsweikamp
parents:
410
diff
changeset
|
289 I2C_DeInit(); |
|
9eeab3fead8f
Added "I2C_DeInit();" in hardware detection routines. It's the recommended way
heinrichsweikamp
parents:
410
diff
changeset
|
290 HAL_Delay(100); |
| 358 | 291 MX_I2C1_Init(); |
|
488
9eeab3fead8f
Added "I2C_DeInit();" in hardware detection routines. It's the recommended way
heinrichsweikamp
parents:
410
diff
changeset
|
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
d9dbfa496f7e
Properly send compass 3 into sleepmode (end-2019 hardware)
heinrichsweikamp
parents:
488
diff
changeset
|
361 if(hardwareCompass == compass_generation2) //LSM303D |
| 38 | 362 compass_sleep_LSM303D(); |
|
514
d9dbfa496f7e
Properly send compass 3 into sleepmode (end-2019 hardware)
heinrichsweikamp
parents:
488
diff
changeset
|
363 if(hardwareCompass == compass_generation1) //HMC5883L |
| 38 | 364 compass_sleep_HMC5883L(); |
|
514
d9dbfa496f7e
Properly send compass 3 into sleepmode (end-2019 hardware)
heinrichsweikamp
parents:
488
diff
changeset
|
365 if(hardwareCompass == compass_generation3) //LSM303AGR |
|
d9dbfa496f7e
Properly send compass 3 into sleepmode (end-2019 hardware)
heinrichsweikamp
parents:
488
diff
changeset
|
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
d9dbfa496f7e
Properly send compass 3 into sleepmode (end-2019 hardware)
heinrichsweikamp
parents:
488
diff
changeset
|
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
f9458e979154
Bugfix display compass calibration frozen:
ideenmodellierer
parents:
358
diff
changeset
|
1362 SCompassCalib g; |
| 38 | 1363 |
| 1364 // Starts with no calibration at all: | |
|
410
f9458e979154
Bugfix display compass calibration frozen:
ideenmodellierer
parents:
358
diff
changeset
|
1365 compass_reset_calibration(&g); |
|
f9458e979154
Bugfix display compass calibration frozen:
ideenmodellierer
parents:
358
diff
changeset
|
1366 uint32_t tickstart = 0; |
|
f9458e979154
Bugfix display compass calibration frozen:
ideenmodellierer
parents:
358
diff
changeset
|
1367 tickstart = HAL_GetTick(); |
|
f9458e979154
Bugfix display compass calibration frozen:
ideenmodellierer
parents:
358
diff
changeset
|
1368 /* run calibration for one minute */ |
|
f9458e979154
Bugfix display compass calibration frozen:
ideenmodellierer
parents:
358
diff
changeset
|
1369 while(time_elapsed_ms(tickstart,HAL_GetTick()) < 60000) |
| 38 | 1370 { |
|
410
f9458e979154
Bugfix display compass calibration frozen:
ideenmodellierer
parents:
358
diff
changeset
|
1371 while((SPI_Evaluate_RX_Data() == 0) && (time_elapsed_ms(tickstart,HAL_GetTick()) < 60000)) |
|
f9458e979154
Bugfix display compass calibration frozen:
ideenmodellierer
parents:
358
diff
changeset
|
1372 { |
|
f9458e979154
Bugfix display compass calibration frozen:
ideenmodellierer
parents:
358
diff
changeset
|
1373 HAL_Delay(1); |
|
f9458e979154
Bugfix display compass calibration frozen:
ideenmodellierer
parents:
358
diff
changeset
|
1374 } |
|
f9458e979154
Bugfix display compass calibration frozen:
ideenmodellierer
parents:
358
diff
changeset
|
1375 compass_read(); |
|
f9458e979154
Bugfix display compass calibration frozen:
ideenmodellierer
parents:
358
diff
changeset
|
1376 acceleration_read(); |
|
f9458e979154
Bugfix display compass calibration frozen:
ideenmodellierer
parents:
358
diff
changeset
|
1377 compass_calc_roll_pitch_only(); |
| 38 | 1378 |
|
410
f9458e979154
Bugfix display compass calibration frozen:
ideenmodellierer
parents:
358
diff
changeset
|
1379 if((hardwareCompass == compass_generation1 ) //HMC5883L) |
| 38 | 1380 &&((compass_DX_f == -4096) || |
|
410
f9458e979154
Bugfix display compass calibration frozen:
ideenmodellierer
parents:
358
diff
changeset
|
1381 (compass_DY_f == -4096) || |
|
f9458e979154
Bugfix display compass calibration frozen:
ideenmodellierer
parents:
358
diff
changeset
|
1382 (compass_DZ_f == -4096) )) |
|
f9458e979154
Bugfix display compass calibration frozen:
ideenmodellierer
parents:
358
diff
changeset
|
1383 { |
|
f9458e979154
Bugfix display compass calibration frozen:
ideenmodellierer
parents:
358
diff
changeset
|
1384 if(compass_gain == 0) |
|
f9458e979154
Bugfix display compass calibration frozen:
ideenmodellierer
parents:
358
diff
changeset
|
1385 return -1; |
|
f9458e979154
Bugfix display compass calibration frozen:
ideenmodellierer
parents:
358
diff
changeset
|
1386 compass_gain--; |
|
f9458e979154
Bugfix display compass calibration frozen:
ideenmodellierer
parents:
358
diff
changeset
|
1387 compass_init(1, compass_gain); |
|
f9458e979154
Bugfix display compass calibration frozen:
ideenmodellierer
parents:
358
diff
changeset
|
1388 compass_reset_calibration(&g); |
|
f9458e979154
Bugfix display compass calibration frozen:
ideenmodellierer
parents:
358
diff
changeset
|
1389 continue; |
|
f9458e979154
Bugfix display compass calibration frozen:
ideenmodellierer
parents:
358
diff
changeset
|
1390 } |
| 38 | 1391 |
|
410
f9458e979154
Bugfix display compass calibration frozen:
ideenmodellierer
parents:
358
diff
changeset
|
1392 copyCompassDataDuringCalibration(compass_DX_f,compass_DY_f,compass_DZ_f); |
|
f9458e979154
Bugfix display compass calibration frozen:
ideenmodellierer
parents:
358
diff
changeset
|
1393 compass_add_calibration(&g); |
| 104 | 1394 } |
| 38 | 1395 |
| 1396 compass_solve_calibration(&g); | |
| 1397 | |
|
410
f9458e979154
Bugfix display compass calibration frozen:
ideenmodellierer
parents:
358
diff
changeset
|
1398 tfull32 dataBlock[4]; |
|
f9458e979154
Bugfix display compass calibration frozen:
ideenmodellierer
parents:
358
diff
changeset
|
1399 dataBlock[0].Word16.low16 = compass_CX_f; |
|
f9458e979154
Bugfix display compass calibration frozen:
ideenmodellierer
parents:
358
diff
changeset
|
1400 dataBlock[0].Word16.hi16 = compass_CY_f; |
|
f9458e979154
Bugfix display compass calibration frozen:
ideenmodellierer
parents:
358
diff
changeset
|
1401 dataBlock[1].Word16.low16 = compass_CZ_f; |
|
f9458e979154
Bugfix display compass calibration frozen:
ideenmodellierer
parents:
358
diff
changeset
|
1402 dataBlock[1].Word16.hi16 = 0xFFFF; |
|
f9458e979154
Bugfix display compass calibration frozen:
ideenmodellierer
parents:
358
diff
changeset
|
1403 dataBlock[2].Full32 = 0x7FFFFFFF; |
|
f9458e979154
Bugfix display compass calibration frozen:
ideenmodellierer
parents:
358
diff
changeset
|
1404 dataBlock[3].Full32 = 0x7FFFFFFF; |
|
f9458e979154
Bugfix display compass calibration frozen:
ideenmodellierer
parents:
358
diff
changeset
|
1405 BFA_writeDataBlock((uint32_t *)dataBlock); |
| 38 | 1406 |
|
410
f9458e979154
Bugfix display compass calibration frozen:
ideenmodellierer
parents:
358
diff
changeset
|
1407 return 0; |
| 38 | 1408 } |
| 1409 |