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