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