Mercurial > public > ostc4
annotate Small_CPU/Src/compass.c @ 1010:08f76d3e9856 GasConsumption
Bugfix: Pitch compensation
The pitch was not calculated correct during calibration process. As result the heading value is sensetive to pitch movment. The issue has been solved by applying the correct formular for calculation.
| author | Ideenmodellierer |
|---|---|
| date | Tue, 06 May 2025 22:07:39 +0200 |
| parents | 65d35e66efb9 |
| 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 |