Mercurial > public > ostc4
annotate Small_CPU/Src/compass.c @ 876:5b325372db4b Evo_2_23 tip
Minor: Enable CO2-Sensor as default now
also enable voltage display in addition to %-Battery
author | heinrichsweikamp |
---|---|
date | Fri, 16 Aug 2024 16:17:51 +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 |