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