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