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