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