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