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