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