Mercurial > public > ostc4
annotate Small_CPU/Src/compass.c @ 345:4093ac18b25c FlightMode_Improvment
Added function to evaluate the state of the surface pressure history memory
Activation of the computer during or short after landing caused a switch into divemode which could then not be left.
To solve this the history memory is evaluated for significant pressure changes as the would not be expected in "normal" condition. In case of such a event (unstable) the pressure limit to enter dive mode is set to an absolute value (instead of relativ in normal mode)
| author | ideenmodellierer |
|---|---|
| date | Thu, 03 Oct 2019 21:26:46 +0200 |
| parents | 49f5db6139d5 |
| children | c3d511365552 |
| 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 | |
| 42 #define TEST_IF_HMC5883L | |
| 43 //#define COMPASS_DEACTIVATE | |
| 44 | |
| 45 /// split byte to bits | |
| 46 typedef struct{ | |
| 47 uint8_t bit0:1; ///< split byte to bits | |
| 48 uint8_t bit1:1; ///< split byte to bits | |
| 49 uint8_t bit2:1; ///< split byte to bits | |
| 50 uint8_t bit3:1; ///< split byte to bits | |
| 51 uint8_t bit4:1; ///< split byte to bits | |
| 52 uint8_t bit5:1; ///< split byte to bits | |
| 53 uint8_t bit6:1; ///< split byte to bits | |
| 54 uint8_t bit7:1; ///< split byte to bits | |
| 55 } ubit8_t; | |
| 56 | |
| 57 | |
| 58 /// split byte to bits | |
| 59 typedef union{ | |
| 60 ubit8_t ub; ///< split byte to bits | |
| 61 uint8_t uw; ///< split byte to bits | |
| 62 } bit8_Type; | |
| 63 | |
| 64 | |
| 65 /// split word to 2 bytes | |
| 66 typedef struct{ | |
| 67 uint8_t low; ///< split word to 2 bytes | |
| 68 uint8_t hi; ///< split word to 2 bytes | |
| 69 } two_byte; | |
| 70 | |
| 71 | |
| 72 /// split word to 2 bytes | |
| 73 typedef union{ | |
| 74 two_byte Byte; ///< split word to 2 bytes | |
| 75 uint16_t Word; ///< split word to 2 bytes | |
| 76 } tword; | |
| 77 | |
| 78 | |
| 79 /// split signed word to 2 bytes | |
| 80 typedef union{ | |
| 81 two_byte Byte; ///< split signed word to 2 bytes | |
| 82 int16_t Word; ///< split signed word to 2 bytes | |
| 83 } signed_tword; | |
| 84 | |
| 85 | |
| 86 /// split full32 to 2 words | |
| 87 typedef struct{ | |
| 88 uint16_t low16; ///< split word to 2 bytes | |
| 89 uint16_t hi16; ///< split word to 2 bytes | |
| 90 } two_word; | |
| 91 | |
| 92 typedef union{ | |
| 93 two_word Word16; ///< split word to 2 bytes | |
| 94 uint32_t Full32; ///< split word to 2 bytes | |
| 95 } tfull32; | |
| 96 | |
| 97 | |
| 98 /// crazy compass calibration stuff | |
| 99 typedef struct | |
| 100 { | |
| 101 unsigned short int compass_N; | |
| 102 float Su, Sv, Sw; | |
| 103 float Suu, Svv, Sww, Suv, Suw, Svw; | |
| 104 float Suuu, Svvv, Swww; | |
| 105 float Suuv, Suuw, Svvu, Svvw, Swwu, Swwv; | |
| 106 } SCompassCalib; | |
| 107 | |
| 108 | |
| 109 #define Q_PI (18000) | |
| 110 #define Q_PIO2 (9000) | |
| 111 | |
| 112 #define HMC5883L (1) ///< id used with hardwareCompass | |
| 113 #define LSM303D (2) ///< id used with hardwareCompass | |
| 114 #define COMPASS_NOT_RECOGNIZED (4) ///< id used with hardwareCompass | |
| 115 | |
| 116 | |
| 117 ////////////////////////////////////////////////////////////////////////////// | |
| 118 // fifth order of polynomial approximation of atan(), giving 0.05 deg max error | |
| 119 // | |
| 120 #define K1 (5701) // Needs K1/2**16 | |
| 121 #define K2 (1645) // Needs K2/2**48 WAS NEGATIV | |
| 122 #define K3 ( 446) // Needs K3/2**80 | |
| 123 | |
| 124 const float PI = 3.14159265; ///< pi, used in compass_calc() | |
| 125 | |
| 126 typedef short int Int16; | |
| 127 typedef signed char Int8; | |
| 128 typedef Int16 Angle; | |
| 129 | |
| 130 | |
| 131 /// The (filtered) components of the magnetometer sensor | |
| 132 int16_t compass_DX_f; ///< output from sensor | |
| 133 int16_t compass_DY_f; ///< output from sensor | |
| 134 int16_t compass_DZ_f; ///< output from sensor | |
| 135 | |
| 136 | |
| 137 /// Found soft-iron calibration values, deduced from already filtered values | |
| 138 int16_t compass_CX_f; ///< calibration value | |
| 139 int16_t compass_CY_f; ///< calibration value | |
| 140 int16_t compass_CZ_f; ///< calibration value | |
| 141 | |
| 142 | |
| 143 /// The (filtered) components of the accelerometer sensor | |
| 144 int16_t accel_DX_f; ///< output from sensor | |
| 145 int16_t accel_DY_f; ///< output from sensor | |
| 146 int16_t accel_DZ_f; ///< output from sensor | |
| 147 | |
| 148 | |
| 149 /// The compass result values | |
| 150 float compass_heading; ///< the final result calculated in compass_calc() | |
| 151 float compass_roll; ///< the final result calculated in compass_calc() | |
| 152 float compass_pitch; ///< the final result calculated in compass_calc() | |
| 153 | |
| 154 | |
| 155 uint8_t compass_gain; ///< 7 on start, can be reduced during calibration | |
| 156 | |
| 157 uint8_t hardwareCompass = 0; ///< either HMC5883L or LSM303D or not defined yet ( = 0 ) | |
| 158 | |
| 159 /// LSM303D variables | |
| 160 uint8_t magDataBuffer[6]; ///< here raw data from LSM303D is stored, can be local | |
| 161 uint8_t accDataBuffer[6]; ///< here raw data from LSM303D is stored, can be local | |
| 162 | |
| 163 //uint16_t velMag = 0; | |
| 164 //uint16_t velAcc = 0; | |
| 165 | |
| 166 //uint16_t magODR[] = {31,62,125,250,500,1000,2000}; | |
| 167 //uint16_t accODR[] = {0,31,62,125,250,500,1000,2000,4000,8000,16000}; | |
| 168 //uint8_t fastest = 10; //no sensor is the fastest | |
| 169 //uint8_t datas1 = 0; | |
| 170 //uint8_t zoffFlag = 0; | |
| 171 //uint8_t sendFlag = 0; | |
| 172 | |
| 173 | |
| 174 // all by pixhawk code: | |
| 175 | |
| 176 // struct accel_scale _accel_scale; | |
| 177 unsigned _accel_range_m_s2; | |
| 178 float _accel_range_scale; | |
| 179 unsigned _accel_samplerate; | |
| 180 unsigned _accel_onchip_filter_bandwith; | |
| 181 | |
| 182 // struct mag_scale _mag_scale; | |
| 183 unsigned _mag_range_ga; | |
| 184 float _mag_range_scale; | |
| 185 unsigned _mag_samplerate; | |
| 186 | |
| 187 // default scale factors | |
| 188 float _accel_scale_x_offset = 0.0f; | |
| 189 float _accel_scale_x_scale = 1.0f; | |
| 190 float _accel_scale_y_offset = 0.0f; | |
| 191 float _accel_scale_y_scale = 1.0f; | |
| 192 float _accel_scale_z_offset = 0.0f; | |
| 193 float _accel_scale_z_scale = 1.0f; | |
| 194 | |
| 195 float _mag_scale_x_offset = 0.0f; | |
| 196 float _mag_scale_x_scale = 1.0f; | |
| 197 float _mag_scale_y_offset = 0.0f; | |
| 198 float _mag_scale_y_scale = 1.0f; | |
| 199 float _mag_scale_z_offset = 0.0f; | |
| 200 float _mag_scale_z_scale = 1.0f; | |
| 201 | |
| 202 | |
| 203 /* External function prototypes ----------------------------------------------*/ | |
| 204 | |
| 205 extern void copyCompassDataDuringCalibration(int16_t dx, int16_t dy, int16_t dz); | |
| 206 | |
| 207 /* Private function prototypes -----------------------------------------------*/ | |
| 208 | |
| 209 void compass_reset_calibration(SCompassCalib *g); | |
| 210 void compass_add_calibration(SCompassCalib *g); | |
| 211 void compass_solve_calibration(SCompassCalib *g); | |
| 212 | |
| 213 void compass_init_HMC5883L(uint8_t fast, uint8_t gain); | |
| 214 void compass_sleep_HMC5883L(void); | |
| 215 void compass_read_HMC5883L(void); | |
| 216 | |
| 217 void accelerator_init_MMA8452Q(void); | |
| 218 void accelerator_sleep_MMA8452Q(void); | |
| 219 void acceleration_read_MMA8452Q(void); | |
| 220 | |
| 221 void compass_init_LSM303D(uint8_t fast, uint8_t gain); | |
| 222 void compass_sleep_LSM303D(void); | |
| 223 void compass_read_LSM303D(void); | |
| 224 void acceleration_read_LSM303D(void); | |
| 225 | |
| 226 int LSM303D_accel_set_onchip_lowpass_filter_bandwidth(unsigned bandwidth); | |
| 227 int compass_calib_common(void); | |
| 228 | |
| 229 void compass_calc_roll_pitch_only(void); | |
| 230 | |
| 231 void rotate_mag_3f(float *x, float *y, float *z); | |
| 232 void rotate_accel_3f(float *x, float *y, float *z); | |
| 233 | |
| 234 | |
| 235 /* Exported functions --------------------------------------------------------*/ | |
| 236 | |
| 237 | |
| 238 // =============================================================================== | |
| 239 // compass_init | |
| 240 /// @brief This might be called several times with different gain values during calibration | |
| 241 /// On first call it figures out which hardware is integrated | |
| 242 /// | |
| 243 /// @param gain: 7 is max gain, compass_calib() might reduce it | |
| 244 // =============================================================================== | |
| 245 | |
| 246 uint8_t testCompassTypeDebug = 0xFF; | |
| 247 | |
| 248 void compass_init(uint8_t fast, uint8_t gain) | |
| 249 { | |
| 250 // quick off | |
| 251 #ifdef COMPASS_DEACTIVATE | |
| 252 hardwareCompass = COMPASS_NOT_RECOGNIZED; | |
| 253 #endif | |
| 254 | |
| 255 // don't call again with fast, gain in calib mode etc. | |
| 256 // if unknown | |
| 257 if(hardwareCompass == COMPASS_NOT_RECOGNIZED) | |
| 258 { | |
| 259 return; | |
| 260 } | |
| 261 | |
| 262 // old code but without else | |
| 263 if(hardwareCompass == 0) | |
| 264 { | |
| 265 uint8_t data = WHO_AM_I; | |
| 266 I2C_Master_Transmit( DEVICE_COMPASS_303D, &data, 1); | |
| 267 I2C_Master_Receive( DEVICE_COMPASS_303D, &data, 1); | |
| 268 if(data == WHOIAM_VALUE) | |
| 269 hardwareCompass = LSM303D; | |
| 70 | 270 else |
| 271 hardwareCompass = HMC5883L; | |
| 38 | 272 } |
| 273 | |
| 180 | 274 /* No compass identified => Retry */ |
| 38 | 275 if(hardwareCompass == 0) |
| 276 { | |
| 277 uint8_t data = WHO_AM_I; | |
| 278 I2C_Master_Transmit( DEVICE_COMPASS_303D, &data, 1); | |
| 279 I2C_Master_Receive( DEVICE_COMPASS_303D, &data, 1); | |
| 280 if(data == WHOIAM_VALUE) | |
| 281 hardwareCompass = LSM303D; | |
| 282 else | |
| 283 hardwareCompass = HMC5883L; | |
| 284 } | |
| 70 | 285 |
| 180 | 286 /* Assume that a HMC5883L is equipped by default if detection still failed */ |
| 38 | 287 if(hardwareCompass == 0) |
| 288 hardwareCompass = HMC5883L; | |
| 289 | |
| 290 #ifdef TEST_IF_HMC5883L | |
| 291 HAL_StatusTypeDef resultOfOperationHMC_MMA = HAL_TIMEOUT; | |
| 292 | |
| 293 if(hardwareCompass == HMC5883L) | |
| 294 { | |
| 295 uint8_t data = 0x2A; // CTRL_REG1 of DEVICE_ACCELARATOR_MMA8452Q | |
| 296 resultOfOperationHMC_MMA = I2C_Master_Transmit( DEVICE_ACCELARATOR_MMA8452Q, &data, 1); | |
| 297 if(resultOfOperationHMC_MMA == HAL_OK) | |
| 298 { | |
| 299 hardwareCompass = HMC5883L; // all fine, keep it | |
| 300 } | |
| 301 else | |
| 302 { | |
| 303 hardwareCompass = COMPASS_NOT_RECOGNIZED; | |
| 304 testCompassTypeDebug = 0xEC; | |
| 305 } | |
| 306 } | |
| 307 #endif | |
| 308 | |
| 309 if(hardwareCompass == LSM303D) | |
| 310 { | |
| 311 compass_init_LSM303D(fast, gain); | |
| 312 } | |
| 313 else | |
| 314 if(hardwareCompass == HMC5883L) | |
| 315 { | |
| 316 compass_init_HMC5883L(fast, gain); | |
| 317 } | |
| 318 | |
| 319 tfull32 dataBlock[4]; | |
| 320 if(BFA_readLastDataBlock((uint32_t *)dataBlock) == BFA_OK) | |
| 321 { | |
| 322 compass_CX_f = dataBlock[0].Word16.low16; | |
| 323 compass_CY_f = dataBlock[0].Word16.hi16; | |
| 324 compass_CZ_f = dataBlock[1].Word16.low16; | |
| 325 } | |
| 326 | |
| 327 } | |
| 328 | |
| 329 | |
| 330 // =============================================================================== | |
| 331 // compass_calib | |
| 332 /// @brief with onchip_lowpass_filter configuration for accelerometer of LSM303D | |
| 333 // =============================================================================== | |
| 334 int compass_calib(void) | |
| 335 { | |
| 336 if(hardwareCompass == LSM303D) | |
| 337 { | |
| 338 LSM303D_accel_set_onchip_lowpass_filter_bandwidth(773); | |
| 339 int out = compass_calib_common(); | |
| 340 LSM303D_accel_set_onchip_lowpass_filter_bandwidth(LSM303D_ACCEL_DEFAULT_ONCHIP_FILTER_FREQ); | |
| 341 return out; | |
| 342 } | |
| 343 else | |
| 344 if(hardwareCompass == HMC5883L) | |
| 345 { | |
| 346 return compass_calib_common(); | |
| 347 } | |
| 348 else | |
| 349 { | |
| 350 return 0; // standard answer of compass_calib_common(); | |
| 351 } | |
| 352 | |
| 353 | |
| 354 } | |
| 355 | |
| 356 | |
| 357 // =============================================================================== | |
| 358 // compass_sleep | |
| 359 /// @brief low power mode | |
| 360 // =============================================================================== | |
| 361 void compass_sleep(void) | |
| 362 { | |
| 363 if(hardwareCompass == LSM303D) | |
| 364 { | |
| 365 compass_sleep_LSM303D(); | |
| 366 } | |
| 367 else | |
| 368 if(hardwareCompass == HMC5883L) | |
| 369 { | |
| 370 compass_sleep_HMC5883L(); | |
| 371 } | |
| 372 } | |
| 373 | |
| 374 | |
| 375 // =============================================================================== | |
| 376 // compass_read | |
| 377 /// @brief reads magnetometer and accelerometer for LSM303D, | |
| 378 /// otherwise magnetometer only | |
| 379 // =============================================================================== | |
| 380 void compass_read(void) | |
| 381 { | |
| 382 if(hardwareCompass == LSM303D) | |
| 383 { | |
| 384 compass_read_LSM303D(); | |
| 385 } | |
| 386 else | |
| 387 if(hardwareCompass == HMC5883L) | |
| 388 { | |
| 389 compass_read_HMC5883L(); | |
| 390 } | |
| 391 } | |
| 392 | |
| 393 | |
| 394 // =============================================================================== | |
| 395 // accelerator_init | |
| 396 /// @brief empty for for LSM303D | |
| 397 // =============================================================================== | |
| 398 void accelerator_init(void) | |
| 399 { | |
| 400 if(hardwareCompass == HMC5883L) | |
| 401 accelerator_init_MMA8452Q(); | |
| 402 } | |
| 403 | |
| 404 | |
| 405 // =============================================================================== | |
| 406 // accelerator_sleep | |
| 407 /// @brief empty for for LSM303D | |
| 408 // =============================================================================== | |
| 409 void accelerator_sleep(void) | |
| 410 { | |
| 411 if(hardwareCompass == HMC5883L) | |
| 412 accelerator_sleep_MMA8452Q(); | |
| 413 } | |
| 414 | |
| 415 | |
| 416 // =============================================================================== | |
| 417 // acceleration_read | |
| 418 /// @brief empty for for LSM303D | |
| 419 // =============================================================================== | |
| 420 void acceleration_read(void) | |
| 421 { | |
| 422 if(hardwareCompass == LSM303D) | |
| 423 { | |
| 424 acceleration_read_LSM303D(); | |
| 425 } | |
| 426 else | |
| 427 if(hardwareCompass == HMC5883L) | |
| 428 { | |
| 429 acceleration_read_MMA8452Q(); | |
| 430 } | |
| 431 } | |
| 432 | |
| 433 | |
| 434 /* Private functions ---------------------------------------------------------*/ | |
| 435 | |
| 436 // =============================================================================== | |
| 437 // LSM303D_read_reg | |
| 438 /// @brief tiny helpers by pixhawk | |
| 439 // =============================================================================== | |
| 440 uint8_t LSM303D_read_reg(uint8_t addr) | |
| 441 { | |
| 442 uint8_t data; | |
| 443 | |
| 444 I2C_Master_Transmit( DEVICE_COMPASS_303D, &addr, 1); | |
| 445 I2C_Master_Receive( DEVICE_COMPASS_303D, &data, 1); | |
| 446 return data; | |
| 447 } | |
| 448 | |
| 449 | |
| 450 // =============================================================================== | |
| 451 // LSM303D_write_reg | |
| 452 /// @brief tiny helpers by pixhawk | |
| 453 // =============================================================================== | |
| 454 void LSM303D_write_reg(uint8_t addr, uint8_t value) | |
| 455 { | |
| 456 uint8_t data[2]; | |
| 457 | |
| 458 /* enable accel*/ | |
| 459 data[0] = addr; | |
| 460 data[1] = value; | |
| 461 I2C_Master_Transmit( DEVICE_COMPASS_303D, data, 2); | |
| 462 } | |
| 463 | |
| 464 | |
| 465 // =============================================================================== | |
| 466 // LSM303D_write_checked_reg | |
| 467 /// @brief tiny helpers by pixhawk. This runs unchecked at the moment. | |
| 468 // =============================================================================== | |
| 469 void LSM303D_write_checked_reg(uint8_t addr, uint8_t value) | |
| 470 { | |
| 471 LSM303D_write_reg(addr, value); | |
| 472 } | |
| 473 | |
| 474 | |
| 475 // =============================================================================== | |
| 476 // LSM303D_modify_reg | |
| 477 /// @brief tiny helpers by pixhawk | |
| 478 // =============================================================================== | |
| 479 void LSM303D_modify_reg(unsigned reg, uint8_t clearbits, uint8_t setbits) | |
| 480 { | |
| 481 uint8_t val; | |
| 482 | |
| 483 val = LSM303D_read_reg(reg); | |
| 484 val &= ~clearbits; | |
| 485 val |= setbits; | |
| 486 LSM303D_write_checked_reg(reg, val); | |
| 487 } | |
| 488 | |
| 489 /* | |
| 490 // =============================================================================== | |
| 491 // LSM303D_accel_set_range | |
| 492 /// @brief tiny helpers by pixhawk | |
| 493 // =============================================================================== | |
| 494 int LSM303D_accel_set_range(unsigned max_g) | |
| 495 { | |
| 496 uint8_t setbits = 0; | |
| 497 uint8_t clearbits = REG2_FULL_SCALE_BITS_A; | |
| 498 float new_scale_g_digit = 0.0f; | |
| 499 | |
| 500 if (max_g == 0) { | |
| 501 max_g = 16; | |
| 502 } | |
| 503 | |
| 504 if (max_g <= 2) { | |
| 505 _accel_range_m_s2 = 2.0f * LSM303D_ONE_G; | |
| 506 setbits |= REG2_FULL_SCALE_2G_A; | |
| 507 new_scale_g_digit = 0.061e-3f; | |
| 508 | |
| 509 } else if (max_g <= 4) { | |
| 510 _accel_range_m_s2 = 4.0f * LSM303D_ONE_G; | |
| 511 setbits |= REG2_FULL_SCALE_4G_A; | |
| 512 new_scale_g_digit = 0.122e-3f; | |
| 513 | |
| 514 } else if (max_g <= 6) { | |
| 515 _accel_range_m_s2 = 6.0f * LSM303D_ONE_G; | |
| 516 setbits |= REG2_FULL_SCALE_6G_A; | |
| 517 new_scale_g_digit = 0.183e-3f; | |
| 518 | |
| 519 } else if (max_g <= 8) { | |
| 520 _accel_range_m_s2 = 8.0f * LSM303D_ONE_G; | |
| 521 setbits |= REG2_FULL_SCALE_8G_A; | |
| 522 new_scale_g_digit = 0.244e-3f; | |
| 523 | |
| 524 } else if (max_g <= 16) { | |
| 525 _accel_range_m_s2 = 16.0f * LSM303D_ONE_G; | |
| 526 setbits |= REG2_FULL_SCALE_16G_A; | |
| 527 new_scale_g_digit = 0.732e-3f; | |
| 528 | |
| 529 } else { | |
| 530 return -1; | |
| 531 } | |
| 532 | |
| 533 _accel_range_scale = new_scale_g_digit * LSM303D_ONE_G; | |
| 534 | |
| 535 | |
| 536 LSM303D_modify_reg(ADDR_CTRL_REG2, clearbits, setbits); | |
| 537 | |
| 538 return 0; | |
| 539 } | |
| 540 */ | |
| 541 /* | |
| 542 // =============================================================================== | |
| 543 // LSM303D_mag_set_range | |
| 544 /// @brief tiny helpers by pixhawk | |
| 545 // =============================================================================== | |
| 546 int LSM303D_mag_set_range(unsigned max_ga) | |
| 547 { | |
| 548 uint8_t setbits = 0; | |
| 549 uint8_t clearbits = REG6_FULL_SCALE_BITS_M; | |
| 550 float new_scale_ga_digit = 0.0f; | |
| 551 | |
| 552 if (max_ga == 0) { | |
| 553 max_ga = 12; | |
| 554 } | |
| 555 | |
| 556 if (max_ga <= 2) { | |
| 557 _mag_range_ga = 2; | |
| 558 setbits |= REG6_FULL_SCALE_2GA_M; | |
| 559 new_scale_ga_digit = 0.080e-3f; | |
| 560 | |
| 561 } else if (max_ga <= 4) { | |
| 562 _mag_range_ga = 4; | |
| 563 setbits |= REG6_FULL_SCALE_4GA_M; | |
| 564 new_scale_ga_digit = 0.160e-3f; | |
| 565 | |
| 566 } else if (max_ga <= 8) { | |
| 567 _mag_range_ga = 8; | |
| 568 setbits |= REG6_FULL_SCALE_8GA_M; | |
| 569 new_scale_ga_digit = 0.320e-3f; | |
| 570 | |
| 571 } else if (max_ga <= 12) { | |
| 572 _mag_range_ga = 12; | |
| 573 setbits |= REG6_FULL_SCALE_12GA_M; | |
| 574 new_scale_ga_digit = 0.479e-3f; | |
| 575 | |
| 576 } else { | |
| 577 return -1; | |
| 578 } | |
| 579 | |
| 580 _mag_range_scale = new_scale_ga_digit; | |
| 581 | |
| 582 LSM303D_modify_reg(ADDR_CTRL_REG6, clearbits, setbits); | |
| 583 | |
| 584 return 0; | |
| 585 } | |
| 586 */ | |
| 587 | |
| 588 // =============================================================================== | |
| 589 // LSM303D_accel_set_onchip_lowpass_filter_bandwidth | |
| 590 /// @brief tiny helpers by pixhawk | |
| 591 // =============================================================================== | |
| 592 int LSM303D_accel_set_onchip_lowpass_filter_bandwidth(unsigned bandwidth) | |
| 593 { | |
| 594 uint8_t setbits = 0; | |
| 595 uint8_t clearbits = REG2_ANTIALIAS_FILTER_BW_BITS_A; | |
| 596 | |
| 597 if (bandwidth == 0) { | |
| 598 bandwidth = 773; | |
| 599 } | |
| 600 | |
| 601 if (bandwidth <= 50) { | |
| 602 setbits |= REG2_AA_FILTER_BW_50HZ_A; | |
| 603 _accel_onchip_filter_bandwith = 50; | |
| 604 | |
| 605 } else if (bandwidth <= 194) { | |
| 606 setbits |= REG2_AA_FILTER_BW_194HZ_A; | |
| 607 _accel_onchip_filter_bandwith = 194; | |
| 608 | |
| 609 } else if (bandwidth <= 362) { | |
| 610 setbits |= REG2_AA_FILTER_BW_362HZ_A; | |
| 611 _accel_onchip_filter_bandwith = 362; | |
| 612 | |
| 613 } else if (bandwidth <= 773) { | |
| 614 setbits |= REG2_AA_FILTER_BW_773HZ_A; | |
| 615 _accel_onchip_filter_bandwith = 773; | |
| 616 | |
| 617 } else { | |
| 618 return -1; | |
| 619 } | |
| 620 | |
| 621 LSM303D_modify_reg(ADDR_CTRL_REG2, clearbits, setbits); | |
| 622 | |
| 623 return 0; | |
| 624 } | |
| 625 | |
| 626 | |
| 627 // =============================================================================== | |
| 628 // LSM303D_accel_set_driver_lowpass_filter | |
| 629 /// @brief tiny helpers by pixhawk. This one is not used at the moment! | |
| 630 // =============================================================================== | |
| 631 int LSM303D_accel_set_driver_lowpass_filter(float samplerate, float bandwidth) | |
| 632 { | |
| 633 /* | |
| 634 _accel_filter_x_set_cutoff_frequency(samplerate, bandwidth); | |
| 635 _accel_filter_y_set_cutoff_frequency(samplerate, bandwidth); | |
| 636 _accel_filter_z_set_cutoff_frequency(samplerate, bandwidth); | |
| 637 */ | |
| 638 return 0; | |
| 639 } | |
| 640 | |
| 641 /* unused 170821 | |
| 642 // =============================================================================== | |
| 643 // LSM303D_accel_set_samplerate | |
| 644 /// @brief tiny helpers by pixhawk | |
| 645 // =============================================================================== | |
| 646 int LSM303D_accel_set_samplerate(unsigned frequency) | |
| 647 { | |
| 648 uint8_t setbits = 0; | |
| 649 uint8_t clearbits = REG1_RATE_BITS_A; | |
| 650 | |
| 651 // if (frequency == 0 || frequency == ACCEL_SAMPLERATE_DEFAULT) { | |
| 652 frequency = 1600; | |
| 653 // } | |
| 654 | |
| 655 if (frequency <= 3) { | |
| 656 setbits |= REG1_RATE_3_125HZ_A; | |
| 657 _accel_samplerate = 3; | |
| 658 | |
| 659 } else if (frequency <= 6) { | |
| 660 setbits |= REG1_RATE_6_25HZ_A; | |
| 661 _accel_samplerate = 6; | |
| 662 | |
| 663 } else if (frequency <= 12) { | |
| 664 setbits |= REG1_RATE_12_5HZ_A; | |
| 665 _accel_samplerate = 12; | |
| 666 | |
| 667 } else if (frequency <= 25) { | |
| 668 setbits |= REG1_RATE_25HZ_A; | |
| 669 _accel_samplerate = 25; | |
| 670 | |
| 671 } else if (frequency <= 50) { | |
| 672 setbits |= REG1_RATE_50HZ_A; | |
| 673 _accel_samplerate = 50; | |
| 674 | |
| 675 } else if (frequency <= 100) { | |
| 676 setbits |= REG1_RATE_100HZ_A; | |
| 677 _accel_samplerate = 100; | |
| 678 | |
| 679 } else if (frequency <= 200) { | |
| 680 setbits |= REG1_RATE_200HZ_A; | |
| 681 _accel_samplerate = 200; | |
| 682 | |
| 683 } else if (frequency <= 400) { | |
| 684 setbits |= REG1_RATE_400HZ_A; | |
| 685 _accel_samplerate = 400; | |
| 686 | |
| 687 } else if (frequency <= 800) { | |
| 688 setbits |= REG1_RATE_800HZ_A; | |
| 689 _accel_samplerate = 800; | |
| 690 | |
| 691 } else if (frequency <= 1600) { | |
| 692 setbits |= REG1_RATE_1600HZ_A; | |
| 693 _accel_samplerate = 1600; | |
| 694 | |
| 695 } else { | |
| 696 return -1; | |
| 697 } | |
| 698 | |
| 699 LSM303D_modify_reg(ADDR_CTRL_REG1, clearbits, setbits); | |
| 700 return 0; | |
| 701 } | |
| 702 // =============================================================================== | |
| 703 // LSM303D_mag_set_samplerate | |
| 704 /// @brief tiny helpers by pixhawk | |
| 705 // =============================================================================== | |
| 706 int LSM303D_mag_set_samplerate(unsigned frequency) | |
| 707 { | |
| 708 uint8_t setbits = 0; | |
| 709 uint8_t clearbits = REG5_RATE_BITS_M; | |
| 710 | |
| 711 if (frequency == 0) { | |
| 712 frequency = 100; | |
| 713 } | |
| 714 | |
| 715 if (frequency <= 3) { | |
| 716 setbits |= REG5_RATE_3_125HZ_M; | |
| 717 _mag_samplerate = 25; | |
| 718 | |
| 719 } else if (frequency <= 6) { | |
| 720 setbits |= REG5_RATE_6_25HZ_M; | |
| 721 _mag_samplerate = 25; | |
| 722 | |
| 723 } else if (frequency <= 12) { | |
| 724 setbits |= REG5_RATE_12_5HZ_M; | |
| 725 _mag_samplerate = 25; | |
| 726 | |
| 727 } else if (frequency <= 25) { | |
| 728 setbits |= REG5_RATE_25HZ_M; | |
| 729 _mag_samplerate = 25; | |
| 730 | |
| 731 } else if (frequency <= 50) { | |
| 732 setbits |= REG5_RATE_50HZ_M; | |
| 733 _mag_samplerate = 50; | |
| 734 | |
| 735 } else if (frequency <= 100) { | |
| 736 setbits |= REG5_RATE_100HZ_M; | |
| 737 _mag_samplerate = 100; | |
| 738 | |
| 739 } else { | |
| 740 return -1; | |
| 741 } | |
| 742 | |
| 743 LSM303D_modify_reg(ADDR_CTRL_REG5, clearbits, setbits); | |
| 744 return 0; | |
| 745 } | |
| 746 */ | |
| 747 | |
| 748 | |
| 749 // rotate_mag_3f: nicht genutzt aber praktisch; rotate_accel_3f wird benutzt | |
| 750 // =============================================================================== | |
| 751 // rotate_mag_3f | |
| 752 /// @brief swap axis in convient way, by hw | |
| 753 /// @param *x raw input is set to *y input | |
| 754 /// @param *y raw input is set to -*x input | |
| 755 /// @param *z raw is not touched | |
| 756 // =============================================================================== | |
| 757 void rotate_mag_3f(float *x, float *y, float *z) | |
| 758 { | |
| 759 return; | |
| 760 /* | |
| 761 *x = *x; // HMC: *x = -*y | |
| 762 *y = *y; // HMC: *y = *x // change 20.04.2016: zuvor *y = -*y | |
| 763 *z = *z; // HMC: *z = *z | |
| 764 */ | |
| 765 } | |
| 766 | |
| 767 | |
| 768 // =============================================================================== | |
| 769 // rotate_accel_3f | |
| 770 /// @brief swap axis in convient way, by hw, same as MMA8452Q | |
| 771 /// @param *x raw input, output is with sign change | |
| 772 /// @param *y raw input, output is with sign change | |
| 773 /// @param *z raw input, output is with sign change | |
| 774 // =============================================================================== | |
| 775 void rotate_accel_3f(float *x, float *y, float *z) | |
| 776 { | |
| 777 *x = -*x; | |
| 778 *y = -*y; | |
| 779 *z = -*z; | |
| 780 /* tested: | |
| 781 x = x, y =-y, z=-z: does not work with roll | |
| 782 x = x, y =y, z=-z: does not work with pitch | |
| 783 x = x, y =y, z=z: does not work at all | |
| 784 */ | |
| 785 } | |
| 786 | |
| 787 | |
| 788 // =============================================================================== | |
| 789 // compass_init_LSM303D by PIXhawk (LSM303D::reset()) | |
| 790 // https://raw.githubusercontent.com/PX4/Firmware/master/src/drivers/lsm303d/lsm303d.cpp | |
| 791 /// @brief The new ST 303D | |
| 792 /// This might be called several times with different gain values during calibration | |
| 793 /// but gain change is not supported at the moment. | |
| 794 /// | |
| 795 /// @param gain: 7 is max gain and set with here, compass_calib() might reduce it | |
| 796 // =============================================================================== | |
| 797 | |
| 798 //uint8_t testCompassLS303D[11]; | |
| 799 | |
| 800 void compass_init_LSM303D(uint8_t fast, uint8_t gain) | |
| 801 { | |
| 802 // matthias version 160620 | |
| 803 if(fast == 0) | |
| 804 { | |
| 805 LSM303D_write_checked_reg(ADDR_CTRL_REG0, 0x00); | |
| 806 LSM303D_write_checked_reg(ADDR_CTRL_REG1, 0x3F); // mod 12,5 Hz 3 instead of 6,25 Hz 2 | |
| 807 LSM303D_write_checked_reg(ADDR_CTRL_REG2, 0xC0); | |
| 808 LSM303D_write_checked_reg(ADDR_CTRL_REG3, 0x00); | |
| 809 LSM303D_write_checked_reg(ADDR_CTRL_REG4, 0x00); | |
| 810 LSM303D_write_checked_reg(ADDR_CTRL_REG5, 0x68); // mod 12,5 Hz 8 instead of 6,25 Hz 4 | |
| 811 } | |
| 812 else | |
| 813 { | |
| 814 LSM303D_write_checked_reg(ADDR_CTRL_REG0, 0x00); | |
| 815 LSM303D_write_checked_reg(ADDR_CTRL_REG1, 0x6F); // 100 Hz | |
| 816 LSM303D_write_checked_reg(ADDR_CTRL_REG2, 0xC0); | |
| 817 LSM303D_write_checked_reg(ADDR_CTRL_REG3, 0x00); | |
| 818 LSM303D_write_checked_reg(ADDR_CTRL_REG4, 0x00); | |
| 819 LSM303D_write_checked_reg(ADDR_CTRL_REG5, 0x74); // 100 Hz | |
| 820 } | |
| 821 LSM303D_write_checked_reg(ADDR_CTRL_REG6, 0x00); | |
| 822 LSM303D_write_checked_reg(ADDR_CTRL_REG7, 0x00); | |
| 823 | |
| 824 /* | |
| 825 uint8_t data; | |
| 826 for(int i=0;i<11;i++) | |
| 827 { | |
| 828 data = ADDR_INT_THS_L_M + i; | |
| 829 I2C_Master_Transmit( DEVICE_COMPASS_303D, &data, 1); | |
| 830 I2C_Master_Receive( DEVICE_COMPASS_303D, &testCompassLS303D[i], 1); | |
| 831 } | |
| 832 */ | |
| 833 | |
| 834 return; | |
| 835 /* | |
| 836 LSM303D_accel_set_range(LSM303D_ACCEL_DEFAULT_RANGE_G); // modifies ADDR_CTRL_REG2 | |
| 837 LSM303D_accel_set_samplerate(LSM303D_ACCEL_DEFAULT_RATE); // modifies ADDR_CTRL_REG1 | |
| 838 | |
| 839 LSM303D_mag_set_range(LSM303D_MAG_DEFAULT_RANGE_GA); | |
| 840 LSM303D_mag_set_samplerate(LSM303D_MAG_DEFAULT_RATE); | |
| 841 */ | |
| 842 | |
| 843 /* | |
| 844 // my stuff hw | |
| 845 // enable accel | |
| 846 LSM303D_write_checked_reg(ADDR_CTRL_REG1, | |
| 847 REG1_X_ENABLE_A | REG1_Y_ENABLE_A | REG1_Z_ENABLE_A | REG1_BDU_UPDATE | REG1_RATE_800HZ_A); | |
| 848 | |
| 849 // enable mag | |
| 850 LSM303D_write_checked_reg(ADDR_CTRL_REG7, REG7_CONT_MODE_M); | |
| 851 LSM303D_write_checked_reg(ADDR_CTRL_REG5, REG5_RES_HIGH_M | REG5_ENABLE_T); | |
| 852 LSM303D_write_checked_reg(ADDR_CTRL_REG3, 0x04); // DRDY on ACCEL on INT1 | |
| 853 LSM303D_write_checked_reg(ADDR_CTRL_REG4, 0x04); // DRDY on MAG on INT2 | |
| 854 | |
| 855 LSM303D_accel_set_range(LSM303D_ACCEL_DEFAULT_RANGE_G); | |
| 856 LSM303D_accel_set_samplerate(LSM303D_ACCEL_DEFAULT_RATE); | |
| 857 LSM303D_accel_set_driver_lowpass_filter((float)LSM303D_ACCEL_DEFAULT_RATE, (float)LSM303D_ACCEL_DEFAULT_DRIVER_FILTER_FREQ); | |
| 858 //LSM303D_accel_set_onchip_lowpass_filter_bandwidth(773); // factory setting | |
| 859 | |
| 860 // we setup the anti-alias on-chip filter as 50Hz. We believe | |
| 861 // this operates in the analog domain, and is critical for | |
| 862 // anti-aliasing. The 2 pole software filter is designed to | |
| 863 // operate in conjunction with this on-chip filter | |
| 864 if(fast) | |
| 865 LSM303D_accel_set_onchip_lowpass_filter_bandwidth(773); // factory setting | |
| 866 else | |
| 867 LSM303D_accel_set_onchip_lowpass_filter_bandwidth(LSM303D_ACCEL_DEFAULT_ONCHIP_FILTER_FREQ); | |
| 868 | |
| 869 | |
| 870 LSM303D_mag_set_range(LSM303D_MAG_DEFAULT_RANGE_GA); | |
| 871 LSM303D_mag_set_samplerate(LSM303D_MAG_DEFAULT_RATE); | |
| 872 */ | |
| 873 } | |
| 874 | |
| 875 | |
| 876 // =============================================================================== | |
| 877 // compass_sleep_LSM303D | |
| 878 /// @brief The new compass chip, hopefully this works! | |
| 879 // =============================================================================== | |
| 880 void compass_sleep_LSM303D(void) | |
| 881 { | |
| 882 LSM303D_write_checked_reg(ADDR_CTRL_REG1, 0x00); // CNTRL1: acceleration sensor Power-down mode | |
| 883 LSM303D_write_checked_reg(ADDR_CTRL_REG7, 0x02); // CNTRL7: magnetic sensor Power-down mode | |
| 884 } | |
| 885 | |
| 886 | |
| 887 // =============================================================================== | |
| 888 // acceleration_read_LSM303D | |
| 889 /// @brief The new LSM303D, code by pixhawk | |
| 890 /// | |
| 891 /// output is accel_DX_f, accel_DY_f, accel_DZ_f | |
| 892 // =============================================================================== | |
| 893 void acceleration_read_LSM303D(void) | |
| 894 { | |
| 895 uint8_t data; | |
| 896 float xraw_f, yraw_f, zraw_f; | |
| 897 float accel_report_x, accel_report_y, accel_report_z; | |
| 898 | |
| 899 memset(accDataBuffer,0,6); | |
| 900 | |
| 901 accel_DX_f = 0; | |
| 902 accel_DY_f = 0; | |
| 903 accel_DZ_f = 0; | |
| 904 | |
| 905 for(int i=0;i<6;i++) | |
| 906 { | |
| 907 data = ADDR_OUT_X_L_A + i; | |
| 908 I2C_Master_Transmit( DEVICE_COMPASS_303D, &data, 1); | |
| 909 I2C_Master_Receive( DEVICE_COMPASS_303D, &accDataBuffer[i], 1); | |
| 910 } | |
| 911 | |
| 912 xraw_f = ((float)( (int16_t)((accDataBuffer[1] << 8) | (accDataBuffer[0])))); | |
| 913 yraw_f = ((float)( (int16_t)((accDataBuffer[3] << 8) | (accDataBuffer[2])))); | |
| 914 zraw_f = ((float)( (int16_t)((accDataBuffer[5] << 8) | (accDataBuffer[4])))); | |
| 915 | |
| 916 rotate_accel_3f(&xraw_f, &yraw_f, &zraw_f); | |
| 917 | |
| 918 // mh | |
| 919 accel_report_x = xraw_f; | |
| 920 accel_report_y = yraw_f; | |
| 921 accel_report_z = zraw_f; | |
| 922 | |
| 923 // my stuff | |
| 924 /* | |
| 925 accel_report_x = ((xraw_f * _accel_range_scale) - _accel_scale_x_offset) * _accel_scale_x_scale; | |
| 926 accel_report_y = ((yraw_f * _accel_range_scale) - _accel_scale_y_offset) * _accel_scale_y_scale; | |
| 927 accel_report_z = ((zraw_f * _accel_range_scale) - _accel_scale_z_offset) * _accel_scale_z_scale; | |
| 928 */ | |
| 929 accel_DX_f = ((int16_t)(accel_report_x)); | |
| 930 accel_DY_f = ((int16_t)(accel_report_y)); | |
| 931 accel_DZ_f = ((int16_t)(accel_report_z)); | |
| 932 } | |
| 933 /* special code after accel_report_z = ... | |
| 934 * prior to output | |
| 935 // we have logs where the accelerometers get stuck at a fixed | |
| 936 // large value. We want to detect this and mark the sensor as | |
| 937 // being faulty | |
| 938 | |
| 939 if (fabsf(_last_accel[0] - x_in_new) < 0.001f && | |
| 940 fabsf(_last_accel[1] - y_in_new) < 0.001f && | |
| 941 fabsf(_last_accel[2] - z_in_new) < 0.001f && | |
| 942 fabsf(x_in_new) > 20 && | |
| 943 fabsf(y_in_new) > 20 && | |
| 944 fabsf(z_in_new) > 20) { | |
| 945 _constant_accel_count += 1; | |
| 946 | |
| 947 } else { | |
| 948 _constant_accel_count = 0; | |
| 949 } | |
| 950 | |
| 951 if (_constant_accel_count > 100) { | |
| 952 // we've had 100 constant accel readings with large | |
| 953 // values. The sensor is almost certainly dead. We | |
| 954 // will raise the error_count so that the top level | |
| 955 // flight code will know to avoid this sensor, but | |
| 956 // we'll still give the data so that it can be logged | |
| 957 // and viewed | |
| 958 perf_count(_bad_values); | |
| 959 _constant_accel_count = 0; | |
| 960 } | |
| 961 | |
| 962 _last_accel[0] = x_in_new; | |
| 963 _last_accel[1] = y_in_new; | |
| 964 _last_accel[2] = z_in_new; | |
| 965 | |
| 966 accel_report.x = _accel_filter_x.apply(x_in_new); | |
| 967 accel_report.y = _accel_filter_y.apply(y_in_new); | |
| 968 accel_report.z = _accel_filter_z.apply(z_in_new); | |
| 969 | |
| 970 math::Vector<3> aval(x_in_new, y_in_new, z_in_new); | |
| 971 math::Vector<3> aval_integrated; | |
| 972 | |
| 973 bool accel_notify = _accel_int.put(accel_report.timestamp, aval, aval_integrated, accel_report.integral_dt); | |
| 974 accel_report.x_integral = aval_integrated(0); | |
| 975 accel_report.y_integral = aval_integrated(1); | |
| 976 accel_report.z_integral = aval_integrated(2); | |
| 977 */ | |
| 978 | |
| 979 | |
| 980 // =============================================================================== | |
| 981 // compass_read_LSM303D | |
| 982 /// @brief The new LSM303D, code by pixhawk | |
| 983 /// | |
| 984 /// output is compass_DX_f, compass_DY_f, compass_DZ_f | |
| 985 // =============================================================================== | |
| 986 void compass_read_LSM303D(void) | |
| 987 { | |
| 988 uint8_t data; | |
| 989 // float xraw_f, yraw_f, zraw_f; | |
| 990 // float mag_report_x, mag_report_y, mag_report_z; | |
| 991 | |
| 992 memset(magDataBuffer,0,6); | |
| 993 | |
| 994 compass_DX_f = 0; | |
| 995 compass_DY_f = 0; | |
| 996 compass_DZ_f = 0; | |
| 997 | |
| 998 for(int i=0;i<6;i++) | |
| 999 { | |
| 1000 data = ADDR_OUT_X_L_M + i; | |
| 1001 I2C_Master_Transmit( DEVICE_COMPASS_303D, &data, 1); | |
| 1002 I2C_Master_Receive( DEVICE_COMPASS_303D, &magDataBuffer[i], 1); | |
| 1003 } | |
| 1004 | |
| 1005 // mh 160620 flip x and y if flip display | |
| 1006 compass_DX_f = (((int16_t)((magDataBuffer[1] << 8) | (magDataBuffer[0])))); | |
| 1007 compass_DY_f = (((int16_t)((magDataBuffer[3] << 8) | (magDataBuffer[2])))); | |
| 1008 compass_DZ_f = (((int16_t)((magDataBuffer[5] << 8) | (magDataBuffer[4])))); | |
| 1009 // no rotation | |
| 1010 return; | |
| 1011 /* | |
| 1012 // my stuff | |
| 1013 compass_DX_f = (((int16_t)((magDataBuffer[1] << 8) | (magDataBuffer[0]))) / 10) - 200; | |
| 1014 compass_DY_f = (((int16_t)((magDataBuffer[3] << 8) | (magDataBuffer[2]))) / 10) - 200; | |
| 1015 compass_DZ_f = (((int16_t)((magDataBuffer[5] << 8) | (magDataBuffer[4]))) / 10) - 200; | |
| 1016 */ | |
| 1017 // old | |
| 1018 /* | |
| 1019 xraw_f = ((float)( (int16_t)((magDataBuffer[1] << 8) | (magDataBuffer[0])))); | |
| 1020 yraw_f = ((float)( (int16_t)((magDataBuffer[3] << 8) | (magDataBuffer[2])))); | |
| 1021 zraw_f = ((float)( (int16_t)((magDataBuffer[5] << 8) | (magDataBuffer[4])))); | |
| 1022 | |
| 1023 rotate_mag_3f(&xraw_f, &yraw_f, &zraw_f); | |
| 1024 | |
| 1025 compass_DX_f = (int16_t)((xraw_f * 0.1f) - 200.0f); | |
| 1026 compass_DY_f = (int16_t)((yraw_f * 0.1f) - 200.0f); | |
| 1027 compass_DZ_f = (int16_t)((zraw_f * 0.1f) - 200.0f); | |
| 1028 */ | |
| 1029 /* | |
| 1030 mag_report_x = ((xraw_f * _mag_range_scale) - _mag_scale_x_offset) * _mag_scale_x_scale; | |
| 1031 mag_report_y = ((yraw_f * _mag_range_scale) - _mag_scale_y_offset) * _mag_scale_y_scale; | |
| 1032 mag_report_z = ((zraw_f * _mag_range_scale) - _mag_scale_z_offset) * _mag_scale_z_scale; | |
| 1033 | |
| 1034 compass_DX_f = (int16_t)(mag_report_x * 1000.0f); // 1000.0 is just a wild guess by hw | |
| 1035 compass_DY_f = (int16_t)(mag_report_y * 1000.0f); | |
| 1036 compass_DZ_f = (int16_t)(mag_report_z * 1000.0f); | |
| 1037 */ | |
| 1038 } | |
| 1039 | |
| 1040 | |
| 1041 // -------------------------------------------------------------------------------- | |
| 1042 // ----------EARLIER COMPONENTS --------------------------------------------------- | |
| 1043 // -------------------------------------------------------------------------------- | |
| 1044 | |
| 1045 // =============================================================================== | |
| 1046 // compass_init_HMC5883L | |
| 1047 /// @brief The horrible Honeywell compass chip | |
| 1048 /// This might be called several times during calibration | |
| 1049 /// | |
| 1050 /// @param fast: 1 is fast mode, 0 is normal mode | |
| 1051 /// @param gain: 7 is max gain and set with here, compass_calib() might reduce it | |
| 1052 // =============================================================================== | |
| 1053 void compass_init_HMC5883L(uint8_t fast, uint8_t gain) | |
| 1054 { | |
| 1055 uint8_t write_buffer[4]; | |
| 1056 | |
| 1057 compass_gain = gain; | |
| 1058 uint16_t length = 0; | |
| 1059 write_buffer[0] = 0x00; // 00 = config Register A | |
| 1060 | |
| 1061 if( fast ) | |
| 1062 write_buffer[1] = 0x38; // 0b 0011 1000; // ConfigA: 75Hz, 2 Samples averaged | |
| 1063 else | |
| 1064 write_buffer[1] = 0x68; // 0b 0110 1000; // ConfigA: 3Hz, 8 Samples averaged | |
| 1065 | |
| 1066 switch(gain) | |
| 1067 { | |
| 1068 case 7: | |
| 1069 write_buffer[2] = 0xE0; //0b 1110 0000; // ConfigB: gain | |
| 1070 break; | |
| 1071 case 6: | |
| 1072 write_buffer[2] = 0xC0; //0b 1100 0000; // ConfigB: gain | |
| 1073 break; | |
| 1074 case 5: | |
| 1075 write_buffer[2] = 0xA0; //0b 1010 0000; // ConfigB: gain | |
| 1076 break; | |
| 1077 case 4: | |
| 1078 write_buffer[2] = 0x80; //0b 1000 0000; // ConfigB: gain | |
| 1079 break; | |
| 1080 case 3: | |
| 1081 write_buffer[2] = 0x60; //0b 0110 0000; // ConfigB: gain | |
| 1082 break; | |
| 1083 case 2: | |
| 1084 write_buffer[2] = 0x40; //0b 01000 0000; // ConfigB: gain | |
| 1085 break; | |
| 1086 case 1: | |
| 1087 write_buffer[2] = 0x20; //0b 00100 0000; // ConfigB: gain | |
| 1088 break; | |
| 1089 case 0: | |
| 1090 write_buffer[2] = 0x00; //0b 00000 0000; // ConfigB: gain | |
| 1091 break; | |
| 1092 } | |
| 1093 write_buffer[3] = 0x00; // Mode: continuous mode | |
| 1094 length = 4; | |
| 1095 //hmc_twi_write(0); | |
| 1096 I2C_Master_Transmit( DEVICE_COMPASS_HMC5883L, write_buffer, length); | |
| 1097 } | |
| 1098 | |
| 1099 | |
| 1100 | |
| 1101 // =============================================================================== | |
| 1102 // compass_sleep_HMC5883L | |
| 1103 /// @brief Power-down mode for Honeywell compass chip | |
| 1104 // =============================================================================== | |
| 1105 void compass_sleep_HMC5883L(void) | |
| 1106 { | |
| 1107 uint8_t write_buffer[4]; | |
| 1108 uint16_t length = 0; | |
| 1109 | |
| 1110 write_buffer[0] = 0x00; // 00 = config Register A | |
| 1111 write_buffer[1] = 0x68; // 0b 0110 1000; // ConfigA | |
| 1112 write_buffer[2] = 0x20; // 0b 0010 0000; // ConfigB | |
| 1113 write_buffer[3] = 0x02; // 0b 0000 0010; // Idle Mode | |
| 1114 length = 4; | |
| 1115 I2C_Master_Transmit( DEVICE_COMPASS_HMC5883L, write_buffer, length); | |
| 1116 } | |
| 1117 | |
| 1118 | |
| 1119 // =============================================================================== | |
| 1120 // accelerator_init_MMA8452Q | |
| 1121 /// @brief Power-down mode for acceleration chip used in combination with Honeywell compass | |
| 1122 // =============================================================================== | |
| 1123 void accelerator_init_MMA8452Q(void) | |
| 1124 { | |
| 1125 uint8_t write_buffer[4]; | |
| 1126 uint16_t length = 0; | |
| 1127 //HAL_Delay(1); | |
| 1128 //return; | |
| 1129 write_buffer[0] = 0x0E; // XYZ_DATA_CFG | |
| 1130 write_buffer[1] = 0x00;//0b00000000; // High pass Filter=0 , +/- 2g range | |
| 1131 length = 2; | |
| 1132 I2C_Master_Transmit( DEVICE_ACCELARATOR_MMA8452Q, write_buffer, length); | |
| 1133 //HAL_Delay(1); | |
| 1134 write_buffer[0] = 0x2A; // CTRL_REG1 | |
| 1135 write_buffer[1] = 0x34; //0b00110100; // CTRL_REG1: 160ms data rate, St.By Mode, reduced noise mode | |
| 1136 write_buffer[2] = 0x02; //0b00000010; // CTRL_REG2: High Res in Active mode | |
| 1137 length = 3; | |
| 1138 I2C_Master_Transmit( DEVICE_ACCELARATOR_MMA8452Q, write_buffer, length); | |
| 1139 | |
| 1140 //HAL_Delay(1); | |
| 1141 //hw_delay_us(100); | |
| 1142 write_buffer[0] = 0x2A; // CTRL_REG1 | |
| 1143 write_buffer[1] = 0x35; //0b00110101; // CTRL_REG1: ... Active Mode | |
| 1144 length = 2; | |
| 1145 I2C_Master_Transmit( DEVICE_ACCELARATOR_MMA8452Q, write_buffer, length); | |
| 1146 /* | |
| 1147 HAL_Delay(6); | |
| 1148 compass_read(); | |
| 1149 HAL_Delay(1); | |
| 1150 acceleration_read(); | |
| 1151 | |
| 1152 compass_calc(); | |
| 1153 */ | |
| 1154 } | |
| 1155 | |
| 1156 | |
| 1157 // =============================================================================== | |
| 1158 // accelerator_sleep_MMA8452Q | |
| 1159 /// @brief compass_sleep_HMC5883L | |
| 1160 // =============================================================================== | |
| 1161 void accelerator_sleep_MMA8452Q(void) | |
| 1162 { | |
| 1163 uint16_t length = 0; | |
| 1164 uint8_t write_buffer[4]; | |
| 1165 | |
| 1166 write_buffer[0] = 0x2A; // CTRL_REG1 | |
| 1167 write_buffer[1] = 0x00; //0b00000000; // CTRL_REG1: Standby Mode | |
| 1168 length = 2; | |
| 1169 I2C_Master_Transmit( DEVICE_ACCELARATOR_MMA8452Q, write_buffer, length); | |
| 1170 } | |
| 1171 | |
| 1172 | |
| 1173 // =============================================================================== | |
| 1174 // compass_read_HMC5883L | |
| 1175 /// @brief The new ST 303D - get ALL data and store in static variables | |
| 1176 /// | |
| 1177 /// output is compass_DX_f, compass_DY_f, compass_DZ_f | |
| 1178 // =============================================================================== | |
| 1179 void compass_read_HMC5883L(void) | |
| 1180 { | |
| 1181 uint8_t buffer[20]; | |
| 1182 compass_DX_f = 0; | |
| 1183 compass_DY_f = 0; | |
| 1184 compass_DZ_f = 0; | |
| 1185 uint8_t length = 0; | |
| 1186 uint8_t read_buffer[6]; | |
| 1187 signed_tword data; | |
| 1188 for(int i = 0; i<6;i++) | |
| 1189 read_buffer[i] = 0; | |
| 1190 buffer[0] = 0x03; // 03 = Data Output X MSB Register | |
| 1191 length = 1; | |
| 1192 I2C_Master_Transmit( DEVICE_COMPASS_HMC5883L, buffer, length); | |
| 1193 length = 6; | |
| 1194 I2C_Master_Receive( DEVICE_COMPASS_HMC5883L, read_buffer, length); | |
| 1195 | |
| 1196 | |
| 1197 data.Byte.hi = read_buffer[0]; | |
| 1198 data.Byte.low = read_buffer[1]; | |
| 1199 //Y = Z | |
| 1200 compass_DY_f = - data.Word; | |
| 1201 | |
| 1202 data.Byte.hi = read_buffer[2]; | |
| 1203 data.Byte.low = read_buffer[3]; | |
| 1204 compass_DZ_f = data.Word; | |
| 1205 | |
| 1206 data.Byte.hi = read_buffer[4]; | |
| 1207 data.Byte.low = read_buffer[5]; | |
| 1208 //X = -Y | |
| 1209 compass_DX_f = data.Word; | |
| 1210 } | |
| 1211 | |
| 1212 | |
| 1213 // =============================================================================== | |
| 1214 // acceleration_read_MMA8452Q | |
| 1215 /// @brief The old MMA8452Q used with the Honeywell compass | |
| 1216 /// get the acceleration data and store in static variables | |
| 1217 /// | |
| 1218 /// output is accel_DX_f, accel_DY_f, accel_DZ_f | |
| 1219 // =============================================================================== | |
| 1220 void acceleration_read_MMA8452Q(void) | |
| 1221 { | |
| 1222 uint8_t buffer[20]; | |
| 1223 accel_DX_f = 0; | |
| 1224 accel_DY_f = 0; | |
| 1225 accel_DZ_f = 0; | |
| 1226 uint8_t length = 0; | |
| 1227 // bit8_Type status ; | |
| 1228 uint8_t read_buffer[7]; | |
| 1229 signed_tword data; | |
| 1230 for(int i = 0; i<6;i++) | |
| 1231 read_buffer[i] = 0; | |
| 1232 buffer[0] = 0x00; // 03 = Data Output X MSB Register | |
| 1233 length = 1; | |
| 1234 I2C_Master_Transmit( DEVICE_ACCELARATOR_MMA8452Q, buffer, length); | |
| 1235 length = 7; | |
| 1236 I2C_Master_Receive( DEVICE_ACCELARATOR_MMA8452Q, read_buffer, length); | |
| 1237 | |
| 1238 // status.uw = read_buffer[0]; | |
| 1239 data.Byte.hi = read_buffer[1]; | |
| 1240 data.Byte.low = read_buffer[2]; | |
| 1241 accel_DX_f =data.Word/16; | |
| 1242 data.Byte.hi = read_buffer[3]; | |
| 1243 data.Byte.low = read_buffer[4]; | |
| 1244 accel_DY_f =data.Word/16; | |
| 1245 data.Byte.hi = read_buffer[5]; | |
| 1246 data.Byte.low = read_buffer[6]; | |
| 1247 accel_DZ_f =data.Word/16; | |
| 1248 | |
| 1249 accel_DX_f *= -1; | |
| 1250 accel_DY_f *= -1; | |
| 1251 accel_DZ_f *= -1; | |
| 1252 } | |
| 1253 | |
| 1254 | |
| 1255 // =============================================================================== | |
| 1256 // compass_calc_roll_pitch_only | |
| 1257 /// @brief only the roll and pitch parts of compass_calc() | |
| 1258 /// | |
| 1259 /// input is accel_DX_f, accel_DY_f, accel_DZ_f | |
| 1260 /// output is compass_pitch and compass_roll | |
| 1261 // =============================================================================== | |
| 1262 void compass_calc_roll_pitch_only(void) | |
| 1263 { | |
| 1264 float sinPhi, cosPhi; | |
| 1265 float Phi, Teta; | |
| 1266 | |
| 1267 //---- Calculate sine and cosine of roll angle Phi ----------------------- | |
| 1268 Phi= atan2f(accel_DY_f, accel_DZ_f) ; | |
| 1269 compass_roll = Phi * 180.0f /PI; | |
| 1270 sinPhi = sinf(Phi); | |
| 1271 cosPhi = cosf(Phi); | |
| 1272 | |
| 1273 //---- calculate sin and cosine of pitch angle Theta --------------------- | |
| 1274 Teta = atanf(-(float)accel_DX_f/(accel_DY_f * sinPhi + accel_DZ_f * cosPhi)); | |
| 1275 compass_pitch = Teta * 180.0f /PI; | |
| 1276 } | |
| 1277 | |
| 1278 | |
| 1279 // =============================================================================== | |
| 1280 // compass_calc | |
| 1281 /// @brief all the fancy stuff first implemented in OSTC3 | |
| 1282 /// | |
| 1283 /// input is compass_DX_f, compass_DY_f, compass_DZ_f, accel_DX_f, accel_DY_f, accel_DZ_f | |
| 1284 /// and compass_CX_f, compass_CY_f, compass_CZ_f | |
| 1285 /// output is compass_heading, compass_pitch and compass_roll | |
| 1286 // =============================================================================== | |
| 1287 void compass_calc(void) | |
| 1288 { | |
| 1289 float sinPhi, cosPhi, sinTeta, cosTeta; | |
| 1290 float Phi, Teta, Psi; | |
| 1291 int16_t iBfx, iBfy; | |
| 1292 int16_t iBpx, iBpy, iBpz; | |
| 1293 | |
| 1294 //---- Make hard iron correction ----------------------------------------- | |
| 1295 // Measured magnetometer orientation, measured ok. | |
| 1296 // From matthias drawing: (X,Y,Z) --> (X,Y,Z) : no rotation. | |
| 1297 iBpx = compass_DX_f - compass_CX_f; // X | |
| 1298 iBpy = compass_DY_f - compass_CY_f; // Y | |
| 1299 iBpz = compass_DZ_f - compass_CZ_f; // Z | |
| 1300 | |
| 1301 //---- Calculate sine and cosine of roll angle Phi ----------------------- | |
| 1302 //sincos(accel_DZ_f, accel_DY_f, &sin, &cos); | |
| 1303 Phi= atan2f(accel_DY_f, accel_DZ_f) ; | |
| 1304 compass_roll = Phi * 180.0f /PI; | |
| 1305 sinPhi = sinf(Phi); | |
| 1306 cosPhi = cosf(Phi); | |
| 1307 | |
| 1308 //---- rotate by roll angle (-Phi) --------------------------------------- | |
| 1309 iBfy = iBpy * cosPhi - iBpz * sinPhi; | |
| 1310 iBpz = iBpy * sinPhi + iBpz * cosPhi; | |
| 1311 //Gz = imul(accel_DY_f, sin) + imul(accel_DZ_f, cos); | |
| 1312 | |
| 1313 //---- calculate sin and cosine of pitch angle Theta --------------------- | |
| 1314 //sincos(Gz, -accel_DX_f, &sin, &cos); // NOTE: changed sin sign. | |
| 1315 // Teta takes into account roll of computer and sends combination of Y and Z :-) understand now hw 160421 | |
| 1316 Teta = atanf(-(float)accel_DX_f/(accel_DY_f * sinPhi + accel_DZ_f * cosPhi)); | |
| 1317 compass_pitch = Teta * 180.0f /PI; | |
| 1318 sinTeta = sinf(Teta); | |
| 1319 cosTeta = cosf(Teta); | |
| 1320 /* correct cosine if pitch not in range -90 to 90 degrees */ | |
| 1321 if( cosTeta < 0 ) cosTeta = -cosTeta; | |
| 1322 | |
| 1323 ///---- de-rotate by pitch angle Theta ----------------------------------- | |
| 1324 iBfx = iBpx * cosTeta + iBpz * sinTeta; | |
| 1325 | |
| 1326 //---- Detect uncalibrated compass --------------------------------------- | |
| 1327 if( !compass_CX_f && !compass_CY_f && !compass_CZ_f ) | |
| 1328 { | |
| 1329 compass_heading = -1; | |
| 1330 return; | |
| 1331 } | |
| 1332 | |
| 1333 //---- calculate current yaw = e-compass angle Psi ----------------------- | |
| 1334 // Result in degree (no need of 0.01 deg precision... | |
| 1335 Psi = atan2f(-iBfy,iBfx); | |
| 1336 compass_heading = Psi * 180.0f /PI; | |
| 1337 // Result in 0..360 range: | |
| 1338 if( compass_heading < 0 ) | |
| 1339 compass_heading += 360; | |
| 1340 } | |
| 1341 | |
| 1342 | |
| 1343 /* | |
| 1344 // =============================================================================== | |
| 1345 // compass_calc_mini_during_calibration | |
| 1346 /// @brief all the fancy stuff first implemented in OSTC3 | |
| 1347 /// | |
| 1348 /// input is accel_DX_f, accel_DY_f, accel_DZ_f | |
| 1349 /// output is compass_pitch and compass_roll | |
| 1350 // =============================================================================== | |
| 1351 void compass_calc_mini_during_calibration(void) | |
| 1352 { | |
| 1353 float sinPhi, cosPhi; | |
| 1354 float Phi, Teta; | |
| 1355 | |
| 1356 //---- Calculate sine and cosine of roll angle Phi ----------------------- | |
| 1357 //sincos(accel_DZ_f, accel_DY_f, &sin, &cos); | |
| 1358 Phi= atan2f(accel_DY_f, accel_DZ_f) ; | |
| 1359 compass_roll = Phi * 180.0f /PI; | |
| 1360 sinPhi = sinf(Phi); | |
| 1361 cosPhi = cosf(Phi); | |
| 1362 | |
| 1363 //---- calculate sin and cosine of pitch angle Theta --------------------- | |
| 1364 //sincos(Gz, -accel_DX_f, &sin, &cos); // NOTE: changed sin sign. | |
| 1365 Teta = atanf(-(float)accel_DX_f/(accel_DY_f * sinPhi + accel_DZ_f * cosPhi)); | |
| 1366 compass_pitch = Teta * 180.0f /PI; | |
| 1367 } | |
| 1368 */ | |
| 1369 | |
| 1370 | |
| 1371 // ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | |
| 1372 // // - Calibration - /////////////////////////////////////////////////////////////////////////////////////////////////////// | |
| 1373 // ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | |
| 1374 | |
| 1375 /* can be lost during sleep as those are reset with compass_reset_calibration() */ | |
| 1376 | |
| 1377 // =============================================================================== | |
| 1378 // compass_reset_calibration | |
| 1379 /// @brief all the fancy stuff first implemented in OSTC3 | |
| 1380 /// | |
| 1381 /// output is struct g and compass_CX_f, compass_CY_f, compass_CZ_f | |
| 1382 /// | |
| 1383 /// @param g: is a struct with crazy stuff like Suuu, Svvv, Svvu, etc. | |
| 1384 /// all is set to zero here | |
| 1385 // =============================================================================== | |
| 1386 void compass_reset_calibration(SCompassCalib *g) | |
| 1387 { | |
| 1388 g->compass_N = 0; | |
| 1389 g->Su = g->Sv = g->Sw = 0.0; | |
| 1390 g->Suu = g->Svv = g->Sww = g->Suv = g->Suw = g->Svw = 0.0; | |
| 1391 g->Suuu = g->Svvv = g->Swww = 0.0; | |
| 1392 g->Suuv = g->Suuw = g->Svvu = g->Svvw = g->Swwu = g->Swwv = 0.0; | |
| 1393 compass_CX_f = compass_CY_f = compass_CZ_f = 0.0; | |
| 1394 } | |
| 1395 | |
| 1396 | |
| 1397 // =============================================================================== | |
| 1398 // compass_add_calibration | |
| 1399 /// @brief all the fancy stuff first implemented in OSTC3 | |
| 1400 /// | |
| 1401 /// input is compass_DX_f, compass_DY_f, compass_DZ_f | |
| 1402 /// and compass_CX_f, compass_CY_f, compass_CZ_f | |
| 1403 /// output is struct g | |
| 1404 /// | |
| 1405 /// @param g: is a struct with crazy stuff like Suuu, Svvv, Svvu, etc. | |
| 1406 // =============================================================================== | |
| 1407 void compass_add_calibration(SCompassCalib *g) | |
| 1408 { | |
| 1409 float u, v, w; | |
| 1410 | |
| 1411 u = (compass_DX_f - compass_CX_f) / 32768.0f; | |
| 1412 v = (compass_DY_f - compass_CY_f) / 32768.0f; | |
| 1413 w = (compass_DZ_f - compass_CZ_f) / 32768.0f; | |
| 1414 | |
| 1415 g->compass_N++; | |
| 1416 g->Su += u; | |
| 1417 g->Sv += v; | |
| 1418 g->Sw += w; | |
| 1419 g->Suv += u*v; | |
| 1420 g->Suw += u*w; | |
| 1421 g->Svw += v*w; | |
| 1422 g->Suu += u*u; | |
| 1423 g->Suuu += u*u*u; | |
| 1424 g->Suuv += v*u*u; | |
| 1425 g->Suuw += w*u*u; | |
| 1426 g->Svv += v*v; | |
| 1427 g->Svvv += v*v*v; | |
| 1428 g->Svvu += u*v*v; | |
| 1429 g->Svvw += w*v*v; | |
| 1430 g->Sww += w*w; | |
| 1431 g->Swww += w*w*w; | |
| 1432 g->Swwu += u*w*w; | |
| 1433 g->Swwv += v*w*w; | |
| 1434 } | |
| 1435 | |
| 1436 ////////////////////////////////////////////////////////////////////////////// | |
| 1437 | |
| 1438 // =============================================================================== | |
| 1439 // compass_solve_calibration | |
| 1440 /// @brief all the fancy stuff first implemented in OSTC3 | |
| 1441 /// | |
| 1442 /// input is compass_CX_f, compass_CY_f, compass_CZ_f and g | |
| 1443 /// output is struct g | |
| 1444 /// | |
| 1445 /// @param g: is a struct with crazy stuff like Suuu, Svvv, Svvu, etc. | |
| 1446 // =============================================================================== | |
| 1447 void compass_solve_calibration(SCompassCalib *g) | |
| 1448 { | |
| 1449 float yu, yv, yw; | |
| 1450 float delta; | |
| 1451 float uc, vc, wc; | |
| 1452 | |
| 1453 | |
| 1454 //---- Normalize partial sums -------------------------------------------- | |
| 1455 // | |
| 1456 // u, v, w should be centered on the mean value um, vm, wm: | |
| 1457 // x = u + um, with um = Sx/N | |
| 1458 // | |
| 1459 // So: | |
| 1460 // (u + um)**2 = u**2 + 2u*um + um**2 | |
| 1461 // Su = 0, um = Sx/N | |
| 1462 // Sxx = Suu + 2 um Su + N*(Sx/N)**2 = Suu + Sx**2/N | |
| 1463 // Suu = Sxx - Sx**2/N | |
| 1464 yu = g->Su/g->compass_N; | |
| 1465 yv = g->Sv/g->compass_N; | |
| 1466 yw = g->Sw/g->compass_N; | |
| 1467 | |
| 1468 g->Suu -= g->Su*yu; | |
| 1469 g->Svv -= g->Sv*yv; | |
| 1470 g->Sww -= g->Sw*yw; | |
| 1471 | |
| 1472 // (u + um)(v + vm) = uv + u vm + v um + um vm | |
| 1473 // Sxy = Suv + N * um vm | |
| 1474 // Suv = Sxy - N * (Sx/N)(Sy/N); | |
| 1475 g->Suv -= g->Su*yv; | |
| 1476 g->Suw -= g->Su*yw; | |
| 1477 g->Svw -= g->Sv*yw; | |
| 1478 | |
| 1479 // (u + um)**3 = u**3 + 3 u**2 um + 3 u um**2 + um**3 | |
| 1480 // Sxxx = Suuu + 3 um Suu + 3 um**2 Su + N.um**3 | |
| 1481 // Su = 0, um = Sx/N: | |
| 1482 // Suuu = Sxxx - 3 Sx*Suu/N - N.(Sx/N)**3 | |
| 1483 // = Sxxx - 3 Sx*Suu/N - Sx**3/N**2 | |
| 1484 | |
| 1485 // (u + um)**2 (v + vm) = (u**2 + 2 u um + um**2)(v + vm) | |
| 1486 // Sxxy = Suuv + vm Suu + 2 um (Suv + vm Su) + um**2 (Sv + N.vm) | |
| 1487 // | |
| 1488 // Su = 0, Sv = 0, vm = Sy/N: | |
| 1489 // Sxxy = Suuv + vm Suu + 2 um Suv + N um**2 vm | |
| 1490 // | |
| 1491 // Suuv = Sxxy - (Sy/N) Suu - 2 (Sx/N) Suv - (Sx/N)**2 Sy | |
| 1492 // = Sxxy - Suu*Sy/N - 2 Suv*Sx/N - Sx*Sx*Sy/N/N | |
| 1493 // = Sxxy - (Suu + Sx*Sx/N)*Sy/N - 2 Suv*Sx/N | |
| 1494 g->Suuu -= (3*g->Suu + g->Su*yu)*yu; | |
| 1495 g->Suuv -= (g->Suu + g->Su*yu)*yv + 2*g->Suv*yu; | |
| 1496 g->Suuw -= (g->Suu + g->Su*yu)*yw + 2*g->Suw*yu; | |
| 1497 | |
| 1498 g->Svvu -= (g->Svv + g->Sv*yv)*yu + 2*g->Suv*yv; | |
| 1499 g->Svvv -= (3*g->Svv + g->Sv*yv)*yv; | |
| 1500 g->Svvw -= (g->Svv + g->Sv*yv)*yw + 2*g->Svw*yv; | |
| 1501 | |
| 1502 g->Swwu -= (g->Sww + g->Sw*yw)*yu + 2*g->Suw*yw; | |
| 1503 g->Swwv -= (g->Sww + g->Sw*yw)*yv + 2*g->Svw*yw; | |
| 1504 g->Swww -= (3*g->Sww + g->Sw*yw)*yw; | |
| 1505 | |
| 1506 //---- Solve the system -------------------------------------------------- | |
| 1507 // uc Suu + vc Suv + wc Suw = (Suuu + Svvu + Swwu) / 2 | |
| 1508 // uc Suv + vc Svv + wc Svw = (Suuv + Svvv + Swwv) / 2 | |
| 1509 // uc Suw + vc Svw + wc Sww = (Suuw + Svvw + Swww) / 2 | |
| 1510 // Note this is symetric, with a positiv diagonal, hence | |
| 1511 // it always have a uniq solution. | |
| 1512 yu = 0.5f * (g->Suuu + g->Svvu + g->Swwu); | |
| 1513 yv = 0.5f * (g->Suuv + g->Svvv + g->Swwv); | |
| 1514 yw = 0.5f * (g->Suuw + g->Svvw + g->Swww); | |
| 1515 delta = g->Suu * (g->Svv * g->Sww - g->Svw * g->Svw) | |
| 1516 - g->Suv * (g->Suv * g->Sww - g->Svw * g->Suw) | |
| 1517 + g->Suw * (g->Suv * g->Svw - g->Svv * g->Suw); | |
| 1518 | |
| 1519 uc = (yu * (g->Svv * g->Sww - g->Svw * g->Svw) | |
| 1520 - yv * (g->Suv * g->Sww - g->Svw * g->Suw) | |
| 1521 + yw * (g->Suv * g->Svw - g->Svv * g->Suw) )/delta; | |
| 1522 vc = (g->Suu * ( yv * g->Sww - yw * g->Svw) | |
| 1523 - g->Suv * ( yu * g->Sww - yw * g->Suw) | |
| 1524 + g->Suw * ( yu * g->Svw - yv * g->Suw) )/delta; | |
| 1525 wc = (g->Suu * (g->Svv * yw - g->Svw * yv ) | |
| 1526 - g->Suv * (g->Suv * yw - g->Svw * yu ) | |
| 1527 + g->Suw * (g->Suv * yv - g->Svv * yu ) )/delta; | |
| 1528 | |
| 1529 // Back to uncentered coordinates: | |
| 1530 // xc = um + uc | |
| 1531 uc = g->Su/g->compass_N + compass_CX_f/32768.0f + uc; | |
| 1532 vc = g->Sv/g->compass_N + compass_CY_f/32768.0f + vc; | |
| 1533 wc = g->Sw/g->compass_N + compass_CZ_f/32768.0f + wc; | |
| 1534 | |
| 1535 // Then save the new calibrated center: | |
| 1536 compass_CX_f = (short)(32768 * uc); | |
| 1537 compass_CY_f = (short)(32768 * vc); | |
| 1538 compass_CZ_f = (short)(32768 * wc); | |
| 1539 } | |
| 1540 | |
| 1541 | |
| 1542 // =============================================================================== | |
| 1543 // compass_calib | |
| 1544 /// @brief the main loop for calibration | |
| 1545 /// output is compass_CX_f, compass_CY_f, compass_CZ_f and g | |
| 1546 /// 160704 removed -4096 limit for LSM303D | |
| 1547 /// | |
| 1548 /// @return always 0 | |
| 1549 // =============================================================================== | |
| 1550 int compass_calib_common(void) | |
| 1551 { | |
| 1552 SCompassCalib g; | |
| 1553 | |
| 1554 // Starts with no calibration at all: | |
| 1555 compass_reset_calibration(&g); | |
| 1556 | |
| 1557 int64_t tickstart = 0; | |
| 1558 uint32_t ticks = 0; | |
| 1559 uint32_t lasttick = 0; | |
| 1560 tickstart = HAL_GetTick(); | |
| 1561 // Eine Minute kalibrieren | |
| 1562 while((ticks) < 60 * 1000) | |
| 1563 { | |
| 1564 compass_read(); | |
| 1565 acceleration_read(); | |
| 1566 compass_calc_roll_pitch_only(); | |
| 1567 | |
| 1568 if((hardwareCompass == HMC5883L) | |
| 1569 &&((compass_DX_f == -4096) || | |
| 1570 (compass_DY_f == -4096) || | |
| 1571 (compass_DZ_f == -4096) )) | |
| 1572 { | |
| 1573 if(compass_gain == 0) | |
| 1574 return -1; | |
| 1575 compass_gain--; | |
| 1576 | |
| 1577 compass_init(1, compass_gain); | |
| 1578 compass_reset_calibration(&g); | |
| 1579 //tickstart = HAL_GetTick(); | |
| 1580 continue; | |
| 1581 } | |
| 1582 | |
| 1583 copyCompassDataDuringCalibration(compass_DX_f,compass_DY_f,compass_DZ_f); | |
| 104 | 1584 compass_add_calibration(&g); |
| 38 | 1585 HAL_Delay(1); |
| 1586 lasttick = HAL_GetTick(); | |
| 1587 if(lasttick == 0) | |
| 1588 { | |
| 1589 tickstart = -ticks; | |
| 1590 } | |
| 104 | 1591 HAL_Delay(1); |
| 38 | 1592 ticks = lasttick - tickstart; |
|
147
14e4c83a7559
Forward compass data during calibration mode
Ideenmodellierer
parents:
104
diff
changeset
|
1593 SPI_Evaluate_RX_Data(); |
| 104 | 1594 } |
| 38 | 1595 |
| 1596 compass_solve_calibration(&g); | |
| 1597 | |
| 1598 tfull32 dataBlock[4]; | |
| 1599 dataBlock[0].Word16.low16 = compass_CX_f; | |
| 1600 dataBlock[0].Word16.hi16 = compass_CY_f; | |
| 1601 dataBlock[1].Word16.low16 = compass_CZ_f; | |
| 1602 dataBlock[1].Word16.hi16 = 0xFFFF; | |
| 1603 dataBlock[2].Full32 = 0x7FFFFFFF; | |
| 1604 dataBlock[3].Full32 = 0x7FFFFFFF; | |
| 1605 BFA_writeDataBlock((uint32_t *)dataBlock); | |
| 1606 | |
| 1607 return 0; | |
| 1608 } | |
| 1609 | |
| 1610 // //////////////////////////// TEST CODE ///////////////////////////////////// | |
| 1611 | |
| 1612 | |
| 1613 | |
| 1614 //#include <QtDebug> | |
| 1615 //#include <stdio.h> | |
| 1616 //#include <math.h> | |
| 1617 /*#include <stdlib.h> | |
| 1618 | |
| 1619 short compass_DX_f, compass_DY_f, compass_DZ_f; | |
| 1620 short compass_CX_f, compass_CY_f, compass_CZ_f; | |
| 1621 | |
| 1622 inline float uniform(void) { | |
| 1623 return (rand() & 0xFFFF) / 65536.0f; | |
| 1624 } | |
| 1625 inline float sqr(float x) { | |
| 1626 return x*x; | |
| 1627 } | |
| 1628 | |
| 1629 static const float radius = 0.21f; | |
| 1630 static const float cx = 0.79f, cy = -0.46f, cz = 0.24f; | |
| 1631 // const float cx = 0, cy = 0, cz = 0; | |
| 1632 | |
| 1633 float check_compass_calib(void) | |
| 1634 { | |
| 1635 | |
| 1636 // Starts with no calibration at all: | |
| 1637 compass_CX_f = compass_CY_f = compass_CZ_f = 0; | |
| 1638 | |
| 1639 // Try 10 recalibration passes: | |
| 1640 for(int p=0; p<10; ++p) | |
| 1641 { | |
| 1642 compass_reset_calibration(); | |
| 1643 | |
| 1644 //---- Generates random points on a sphere ------------------------------- | |
| 1645 // of radius,center (cx, cy, cz): | |
| 1646 for(int i=0; i<100; ++i) | |
| 1647 { | |
| 1648 float theta = uniform()*360.0f; | |
| 1649 float phi = uniform()*180.0f - 90.0f; | |
| 1650 | |
| 1651 float x = cx + radius * cosf(phi)*cosf(theta); | |
| 1652 float y = cy + radius * cosf(phi)*sinf(theta); | |
| 1653 float z = cz + radius * sinf(phi); | |
| 1654 | |
| 1655 compass_DX_f = (short)(32768 * x); | |
| 1656 compass_DY_f = (short)(32768 * y); | |
| 1657 compass_DZ_f = (short)(32768 * z); | |
| 1658 compass_add_calibration(); | |
| 1659 } | |
| 1660 | |
| 1661 compass_solve_calibration(); | |
| 1662 //qDebug() << "Center =" | |
| 1663 // << compass_CX_f/32768.0f | |
| 1664 // << compass_CY_f/32768.0f | |
| 1665 // << compass_CZ_f/32768.0f; | |
| 1666 | |
| 1667 float r2 = sqr(compass_CX_f/32768.0f - cx) | |
| 1668 + sqr(compass_CY_f/32768.0f - cy) | |
| 1669 + sqr(compass_CZ_f/32768.0f - cz); | |
| 1670 if( r2 > 0.01f*0.01f ) | |
| 1671 return sqrtf(r2); | |
| 1672 } | |
| 1673 return 0; | |
| 1674 }*/ | |
| 1675 | |
| 1676 | |
| 1677 | |
| 1678 /* | |
| 1679 void compass_read_LSM303D_v3(void) | |
| 1680 { | |
| 1681 uint8_t data; | |
| 1682 | |
| 1683 memset(magDataBuffer,0,6); | |
| 1684 | |
| 1685 compass_DX_f = 0; | |
| 1686 compass_DY_f = 0; | |
| 1687 compass_DZ_f = 0; | |
| 1688 | |
| 1689 //magnetometer multi read, order xl,xh, yl,yh, zl, zh | |
| 1690 data = REG_MAG_DATA_ADDR; | |
| 1691 I2C_Master_Transmit( DEVICE_COMPASS_303D, &data, 1); | |
| 1692 I2C_Master_Receive( DEVICE_COMPASS_303D, magDataBuffer, 6); | |
| 1693 | |
| 1694 compass_DX_f = ((int16_t)( (int16_t)((magDataBuffer[1] << 8) | (magDataBuffer[0])))); | |
| 1695 compass_DY_f = ((int16_t)( (int16_t)((magDataBuffer[3] << 8) | (magDataBuffer[2])))); | |
| 1696 compass_DZ_f = ((int16_t)( (int16_t)((magDataBuffer[5] << 8) | (magDataBuffer[4])))); | |
| 1697 | |
| 1698 // compass_DX_f = compass_DX_f * stat->sensitivity_mag; | |
| 1699 // compass_DY_f = compass_DY_f * stat->sensitivity_mag; | |
| 1700 // compass_DZ_f = compass_DZ_f * stat->sensitivity_mag; | |
| 1701 } | |
| 1702 | |
| 1703 | |
| 1704 // =============================================================================== | |
| 1705 // compass_init_LSM303D by STMicroelectronics 2013 V1.0.5 2013/Oct/23 | |
| 1706 /// @brief The new ST 303D | |
| 1707 /// This might be called several times with different gain values during calibration | |
| 1708 /// | |
| 1709 /// @param gain: 7 is max gain and set with here, compass_calib() might reduce it | |
| 1710 // =============================================================================== | |
| 1711 | |
| 1712 void compass_init_LSM303D_v3(uint8_t gain) | |
| 1713 { | |
| 1714 uint8_t data[10]; | |
| 1715 | |
| 1716 // CNTRL1 | |
| 1717 // 0011 acceleration data rate 0011 = 12.5 Hz (3.125 Hz - 1600 Hz) | |
| 1718 // 0xxx block data update off | |
| 1719 // x111 enable all three axes | |
| 1720 | |
| 1721 // CNTRL5 | |
| 1722 // 0xxx xxxx temp sensor off | |
| 1723 // x00x xxxx magnetic resolution | |
| 1724 // xxx0 1xxx magentic data rate 01 = 6,25 Hz (3.125 Hz - 50 Hz (100 Hz)) | |
| 1725 // xxxx xx00 latch irq requests off | |
| 1726 | |
| 1727 // CNTRL7 | |
| 1728 // 00xx high pass filter mode, 00 normal mode | |
| 1729 // xx0x filter for acceleration data bypassed | |
| 1730 // xxx0 temperature sensor mode only off | |
| 1731 // x0xx magnetic data low-power mode off | |
| 1732 // xx00 magnetic sensor mode 00 = continous-conversion mode (default 10 power-down) | |
| 1733 | |
| 1734 data[0] = CNTRL0; | |
| 1735 data[1] = 0x00; | |
| 1736 I2C_Master_Transmit( DEVICE_COMPASS_303D, data, 2); | |
| 1737 | |
| 1738 // acc | |
| 1739 data[0] = CNTRL1; | |
| 1740 data[1] = 0x00; | |
| 1741 data[2] = 0x0F; | |
| 1742 data[3] = 0x00; | |
| 1743 data[4] = 0x00; | |
| 1744 I2C_Master_Transmit( DEVICE_COMPASS_303D, data, 5); | |
| 1745 | |
| 1746 // mag | |
| 1747 data[0] = CNTRL3; | |
| 1748 data[1] = 0x00; | |
| 1749 data[2] = 0x00; | |
| 1750 data[3] = 0x18; | |
| 1751 data[4] = 0x20; | |
| 1752 I2C_Master_Transmit( DEVICE_COMPASS_303D, data, 5); | |
| 1753 | |
| 1754 data[0] = CNTRL7; | |
| 1755 data[1] = ((MSMS_MASK & CONTINUOS_CONVERSION) | | |
| 1756 ((~MSMS_MASK) & CNTRL7_RESUME_VALUE)); | |
| 1757 I2C_Master_Transmit( DEVICE_COMPASS_303D, data, 2); | |
| 1758 | |
| 1759 HAL_Delay(100); | |
| 1760 } | |
| 1761 | |
| 1762 | |
| 1763 // =============================================================================== | |
| 1764 // compass_init_LSM303D by nordevx for arduion | |
| 1765 /// @brief The new ST 303D | |
| 1766 /// This might be called several times with different gain values during calibration | |
| 1767 /// | |
| 1768 /// @param gain: 7 is max gain and set with here, compass_calib() might reduce it | |
| 1769 // =============================================================================== | |
| 1770 void compass_init_LSM303D_v2(uint8_t gain) | |
| 1771 { | |
| 1772 uint8_t data[2]; | |
| 1773 | |
| 1774 // CNTRL1 | |
| 1775 // 0011 acceleration data rate 0011 = 12.5 Hz (3.125 Hz - 1600 Hz) | |
| 1776 // 0xxx block data update off | |
| 1777 // x111 enable all three axes | |
| 1778 | |
| 1779 // CNTRL5 | |
| 1780 // 0xxx xxxx temp sensor off | |
| 1781 // x00x xxxx magnetic resolution | |
| 1782 // xxx0 1xxx magentic data rate 01 = 6,25 Hz (3.125 Hz - 50 Hz (100 Hz)) | |
| 1783 // xxxx xx00 latch irq requests off | |
| 1784 | |
| 1785 // CNTRL7 | |
| 1786 // 00xx high pass filter mode, 00 normal mode | |
| 1787 // xx0x filter for acceleration data bypassed | |
| 1788 // xxx0 temperature sensor mode only off | |
| 1789 // x0xx magnetic data low-power mode off | |
| 1790 // xx00 magnetic sensor mode 00 = continous-conversion mode (default 10 power-down) | |
| 1791 | |
| 1792 data[0] = CNTRL1; | |
| 1793 data[1] = 0x37; //0b 0011 0111 | |
| 1794 I2C_Master_Transmit( DEVICE_COMPASS_303D, data, 2); | |
| 1795 | |
| 1796 data[0] = CNTRL5; | |
| 1797 data[1] = 0x08; // 0b 0000 1000 | |
| 1798 I2C_Master_Transmit( DEVICE_COMPASS_303D, data, 2); | |
| 1799 | |
| 1800 data[0] = CNTRL7; | |
| 1801 data[1] = 0x00; // 0b 0000 0000 | |
| 1802 I2C_Master_Transmit( DEVICE_COMPASS_303D, data, 2); | |
| 1803 | |
| 1804 HAL_Delay(100); | |
| 1805 } | |
| 1806 | |
| 1807 | |
| 1808 // =============================================================================== | |
| 1809 // compass_init_LSM303D_v1 by ST lsm303d.c | |
| 1810 /// @brief The new ST 303D | |
| 1811 /// This might be called several times with different gain values during calibration | |
| 1812 /// | |
| 1813 /// @param gain: 7 is max gain and set with here, compass_calib() might reduce it | |
| 1814 // =============================================================================== | |
| 1815 void compass_init_LSM303D_v1(uint8_t gain) | |
| 1816 { | |
| 1817 uint8_t data; | |
| 1818 | |
| 1819 compass_gain = gain; | |
| 1820 | |
| 1821 memset(magDataBuffer,0,6); | |
| 1822 memset(accDataBuffer,0,6); | |
| 1823 | |
| 1824 data = CNTRL5; | |
| 1825 I2C_Master_Transmit( DEVICE_COMPASS_303D, &data, 1); | |
| 1826 I2C_Master_Receive( DEVICE_COMPASS_303D, &data, 1); | |
| 1827 data = (data & 0x1c) >> 2; | |
| 1828 velMag = magODR[data]; | |
| 1829 | |
| 1830 data = CNTRL1; | |
| 1831 I2C_Master_Transmit( DEVICE_COMPASS_303D, &data, 1); | |
| 1832 I2C_Master_Receive( DEVICE_COMPASS_303D, &data, 1); | |
| 1833 data = (data & 0xf0) >> 4; | |
| 1834 velAcc = accODR[data]; | |
| 1835 | |
| 1836 data = CNTRL7; | |
| 1837 I2C_Master_Transmit( DEVICE_COMPASS_303D, &data, 1); | |
| 1838 I2C_Master_Receive( DEVICE_COMPASS_303D, &datas1, 1); | |
| 1839 datas1 = (datas1 & 0x02); | |
| 1840 | |
| 1841 //if mag is not pd | |
| 1842 //mag is bigger than gyro | |
| 1843 if( (velMag < velAcc) || datas1 != 0 ) { | |
| 1844 //acc is the biggest | |
| 1845 fastest = ACC_IS_FASTEST; | |
| 1846 } | |
| 1847 else { | |
| 1848 //acc is the biggest | |
| 1849 fastest = MAG_IS_FASTEST; | |
| 1850 } | |
| 1851 | |
| 1852 zoffFlag = 1; | |
| 1853 | |
| 1854 if( fastest == MAG_IS_FASTEST) | |
| 1855 { | |
| 1856 data = STATUS_REG_M; | |
| 1857 I2C_Master_Transmit( DEVICE_COMPASS_303D, &data, 1); | |
| 1858 I2C_Master_Receive( DEVICE_COMPASS_303D, &data, 1); | |
| 1859 | |
| 1860 // if(ValBit(data, ZYXMDA)) { | |
| 1861 sendFlag = 1; | |
| 1862 // } | |
| 1863 | |
| 1864 } | |
| 1865 else if(fastest == ACC_IS_FASTEST) | |
| 1866 { | |
| 1867 data = STATUS_REG_A; | |
| 1868 I2C_Master_Transmit( DEVICE_COMPASS_303D, &data, 1); | |
| 1869 I2C_Master_Receive( DEVICE_COMPASS_303D, &data, 1); | |
| 1870 // if(ValBit(data, DATAREADY_BIT)) { | |
| 1871 sendFlag = 1; | |
| 1872 // } | |
| 1873 } | |
| 1874 } | |
| 1875 | |
| 1876 // =============================================================================== | |
| 1877 // compass_read_LSM303D | |
| 1878 /// @brief The new LSM303D :-) | |
| 1879 /// | |
| 1880 /// output is compass_DX_f, compass_DY_f, compass_DZ_f, accel_DX_f, accel_DY_f, accel_DZ_f | |
| 1881 // =============================================================================== | |
| 1882 void compass_read_LSM303D_v2(void) | |
| 1883 { | |
| 1884 uint8_t data; | |
| 1885 | |
| 1886 memset(magDataBuffer,0,6); | |
| 1887 memset(accDataBuffer,0,6); | |
| 1888 | |
| 1889 compass_DX_f = 0; | |
| 1890 compass_DY_f = 0; | |
| 1891 compass_DZ_f = 0; | |
| 1892 | |
| 1893 accel_DX_f = 0; | |
| 1894 accel_DY_f = 0; | |
| 1895 accel_DZ_f = 0; | |
| 1896 | |
| 1897 //Accelerometer multi read, order xl,xh, yl,yh, zl, zh | |
| 1898 data = REG_ACC_DATA_ADDR; | |
| 1899 I2C_Master_Transmit( DEVICE_COMPASS_303D, &data, 1); | |
| 1900 I2C_Master_Receive( DEVICE_COMPASS_303D, accDataBuffer, 6); | |
| 1901 | |
| 1902 //magnetometer multi read, order xl,xh, yl,yh, zl, zh | |
| 1903 data = OUT_X_L_M; | |
| 1904 I2C_Master_Transmit( DEVICE_COMPASS_303D, &data, 1); | |
| 1905 I2C_Master_Receive( DEVICE_COMPASS_303D, magDataBuffer, 6); | |
| 1906 | |
| 1907 accel_DX_f = ((int16_t)( (int16_t)((accDataBuffer[1] << 8) | (accDataBuffer[0])))); | |
| 1908 accel_DY_f = ((int16_t)( (int16_t)((accDataBuffer[3] << 8) | (accDataBuffer[2])))); | |
| 1909 accel_DZ_f = ((int16_t)( (int16_t)((accDataBuffer[5] << 8) | (accDataBuffer[4])))); | |
| 1910 | |
| 1911 // accel_DX_f = accel_DX_f * stat->sensitivity_acc; | |
| 1912 // accel_DY_f = accel_DY_f * stat->sensitivity_acc; | |
| 1913 // accel_DZ_f = accel_DZ_f * stat->sensitivity_acc; | |
| 1914 | |
| 1915 | |
| 1916 compass_DX_f = magDataBuffer[1]; | |
| 1917 compass_DX_f *= 256; | |
| 1918 compass_DX_f += magDataBuffer[0]; | |
| 1919 | |
| 1920 compass_DY_f = magDataBuffer[3]; | |
| 1921 compass_DY_f *= 256; | |
| 1922 compass_DY_f += magDataBuffer[2]; | |
| 1923 | |
| 1924 compass_DY_f = magDataBuffer[5]; | |
| 1925 compass_DY_f *= 256; | |
| 1926 compass_DY_f += magDataBuffer[4]; | |
| 1927 | |
| 1928 } | |
| 1929 | |
| 1930 | |
| 1931 */ | |
| 1932 |