Mercurial > public > ostc4

comparison Small_CPU/Src/compass.c @ 571:91a8f9893e68

Find changesets by keywords (author, files, the commit message), revision number or hash, or revset expression.
Reactivate compass parameter stored in NVM: The calibration parameters are stored in NVM but the automatic restore function during startup was no longer active. As result the compass needed to be calibration after every RTE update. In addition compass HW was detected at every startup causing some i2c "trouble" because of adressing not available devices. The compass HW info is now stored together with the calibration parameters to avoid i2C problems.
author Ideenmodellierer
date Wed, 25 Nov 2020 20:16:20 +0100
parents 84a4e1200726
children 08af5d707c5a
comparison
equal deleted inserted replaced
570:701ead8dddab 571:91a8f9893e68
41 #include "stm32f4xx_hal.h" 41 #include "stm32f4xx_hal.h"
42 42
43 extern uint32_t time_elapsed_ms(uint32_t ticksstart,uint32_t ticksnow); 43 extern uint32_t time_elapsed_ms(uint32_t ticksstart,uint32_t ticksnow);
44 extern SGlobal global; 44 extern SGlobal global;
45 45
46 /// split word to 2 bytes
47 typedef struct{
48 uint8_t low; ///< split word to 2 bytes
49 uint8_t hi; ///< split word to 2 bytes
50 } two_byte;
51
52
53 /// split word to 2 bytes
54 typedef union{
55 two_byte Byte; ///< split word to 2 bytes
56 uint16_t Word; ///< split word to 2 bytes
57 } tword;
58
59
60 /// split signed word to 2 bytes
61 typedef union{
62 two_byte Byte; ///< split signed word to 2 bytes
63 int16_t Word; ///< split signed word to 2 bytes
64 } signed_tword;
65
66
67 /// split full32 to 2 words
68 typedef struct{
69 uint16_t low16; ///< split word to 2 bytes
70 uint16_t hi16; ///< split word to 2 bytes
71 } two_word;
72
73 typedef union{
74 two_word Word16; ///< split word to 2 bytes
75 uint32_t Full32; ///< split word to 2 bytes
76 } tfull32;
77
78 46
79 /// crazy compass calibration stuff 47 /// crazy compass calibration stuff
80 typedef struct 48 typedef struct
81 { 49 {
82 unsigned short int compass_N; 50 unsigned short int compass_N;
130 float compass_pitch; ///< the final result calculated in compass_calc() 98 float compass_pitch; ///< the final result calculated in compass_calc()
131 99
132 100
133 uint8_t compass_gain; ///< 7 on start, can be reduced during calibration 101 uint8_t compass_gain; ///< 7 on start, can be reduced during calibration
134 102
135 uint8_t hardwareCompass = 0; ///< either HMC5883L (=1) or LSM303D (=2) or LSM303AGR (=3) or not defined yet (=0) 103 uint8_t hardwareCompass = compass_generation_undef; ///< either HMC5883L (=1) or LSM303D (=2) or LSM303AGR (=3) or not defined yet (=0)
136 104
137 /// LSM303D variables 105 /// LSM303D variables
138 uint8_t magDataBuffer[6]; ///< here raw data from LSM303D is stored, can be local 106 uint8_t magDataBuffer[6]; ///< here raw data from LSM303D is stored, can be local
139 uint8_t accDataBuffer[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
140 108
222 if(hardwareCompass == COMPASS_NOT_RECOGNIZED) 190 if(hardwareCompass == COMPASS_NOT_RECOGNIZED)
223 { 191 {
224 return; 192 return;
225 } 193 }
226 194
195 if(hardwareCompass == compass_generation_undef) /* check if compass had been detected before */
196 {
197 tfull32 dataBlock[4];
198 if(BFA_readLastDataBlock(dataBlock) == BFA_OK)
199 {
200 if(dataBlock[3].Word16.hi16 == BFA_calc_Block_Checksum(dataBlock))
201 {
202 compass_CX_f = dataBlock[0].Word16.low16;
203 compass_CY_f = dataBlock[0].Word16.hi16;
204 compass_CZ_f = dataBlock[1].Word16.low16;
205 hardwareCompass = dataBlock[1].Word16.hi16;
206 if(hardwareCompass >= compass_generation_future) /* no generation stored (including COMPASS_NOT_RECOGNIZED) */
207 {
208 hardwareCompass = compass_generation_undef;
209 }
210 }
211 }
212 }
213
227 // old code but without else 214 // old code but without else
228 if(hardwareCompass == 0) 215 if(hardwareCompass == compass_generation_undef)
229 { 216 {
230 uint8_t data = WHO_AM_I; 217 uint8_t data = WHO_AM_I;
231 I2C_Master_Transmit( DEVICE_COMPASS_303D, &data, 1); 218 I2C_Master_Transmit( DEVICE_COMPASS_303D, &data, 1);
232 I2C_Master_Receive( DEVICE_COMPASS_303D, &data, 1); 219 I2C_Master_Receive( DEVICE_COMPASS_303D, &data, 1);
233 if(data == WHOIAM_VALUE_LSM303D) 220 if(data == WHOIAM_VALUE_LSM303D)
238 if(data == WHOIAM_VALUE_LSM303AGR) 225 if(data == WHOIAM_VALUE_LSM303AGR)
239 hardwareCompass = compass_generation3; //LSM303AGR; 226 hardwareCompass = compass_generation3; //LSM303AGR;
240 } 227 }
241 228
242 /* No compass identified => Retry */ 229 /* No compass identified => Retry */
243 if(hardwareCompass == 0) 230 if(hardwareCompass == compass_generation_undef)
244 { 231 {
245 I2C_DeInit(); 232 I2C_DeInit();
246 HAL_Delay(100); 233 HAL_Delay(100);
247 MX_I2C1_Init(); 234 MX_I2C1_Init();
248 HAL_Delay(100); 235 HAL_Delay(100);
257 if(data == WHOIAM_VALUE_LSM303AGR) 244 if(data == WHOIAM_VALUE_LSM303AGR)
258 hardwareCompass = compass_generation3; //LSM303AGR; 245 hardwareCompass = compass_generation3; //LSM303AGR;
259 } 246 }
260 247
261 /* Assume that a HMC5883L is equipped by default if detection still failed */ 248 /* Assume that a HMC5883L is equipped by default if detection still failed */
262 if(hardwareCompass == 0) 249 if(hardwareCompass == compass_generation_undef)
263 hardwareCompass = compass_generation1; //HMC5883L; 250 hardwareCompass = compass_generation1; //HMC5883L;
264 251
265 HAL_StatusTypeDef resultOfOperationHMC_MMA = HAL_TIMEOUT; 252 HAL_StatusTypeDef resultOfOperationHMC_MMA = HAL_TIMEOUT;
266 253
267 // test if both chips of the two-chip solution (gen 1) are present 254 // test if both chips of the two-chip solution (gen 1) are present
294 if(global.deviceDataSendToMaster.hw_Info.compass == 0) 281 if(global.deviceDataSendToMaster.hw_Info.compass == 0)
295 { 282 {
296 global.deviceDataSendToMaster.hw_Info.compass = hardwareCompass; 283 global.deviceDataSendToMaster.hw_Info.compass = hardwareCompass;
297 global.deviceDataSendToMaster.hw_Info.checkCompass = 1; 284 global.deviceDataSendToMaster.hw_Info.checkCompass = 1;
298 } 285 }
299 tfull32 dataBlock[4];
300 if(BFA_readLastDataBlock((uint32_t *)dataBlock) == BFA_OK)
301 {
302 compass_CX_f = dataBlock[0].Word16.low16;
303 compass_CY_f = dataBlock[0].Word16.hi16;
304 compass_CZ_f = dataBlock[1].Word16.low16;
305 }
306
307 } 286 }
308 287
309 288
310 // =============================================================================== 289 // ===============================================================================
311 // compass_calib 290 // compass_calib
1380 compass_add_calibration(&g); 1359 compass_add_calibration(&g);
1381 } 1360 }
1382 1361
1383 compass_solve_calibration(&g); 1362 compass_solve_calibration(&g);
1384 1363
1385 tfull32 dataBlock[4]; 1364 if((hardwareCompass != compass_generation_undef) /* if compass is not know at this point in time storing data makes no sense */
1386 dataBlock[0].Word16.low16 = compass_CX_f; 1365 && (hardwareCompass != COMPASS_NOT_RECOGNIZED))
1387 dataBlock[0].Word16.hi16 = compass_CY_f; 1366 {
1388 dataBlock[1].Word16.low16 = compass_CZ_f; 1367 tfull32 dataBlock[4];
1389 dataBlock[1].Word16.hi16 = 0xFFFF; 1368 dataBlock[0].Word16.low16 = compass_CX_f;
1390 dataBlock[2].Full32 = 0x7FFFFFFF; 1369 dataBlock[0].Word16.hi16 = compass_CY_f;
1391 dataBlock[3].Full32 = 0x7FFFFFFF; 1370 dataBlock[1].Word16.low16 = compass_CZ_f;
1392 BFA_writeDataBlock((uint32_t *)dataBlock); 1371 dataBlock[1].Word16.hi16 = hardwareCompass;
1393 1372 dataBlock[2].Full32 = 0x7FFFFFFF;
1373 dataBlock[3].Word16.low16 = 0xFFFF;
1374 dataBlock[3].Word16.hi16 = BFA_calc_Block_Checksum(dataBlock);
1375 BFA_writeDataBlock(dataBlock);
1376 }
1394 return 0; 1377 return 0;
1395 } 1378 }
1396 1379