Mercurial > public > ostc4

comparison Small_CPU/Src/compass.c @ 416:bcf447646e07

Find changesets by keywords (author, files, the commit message), revision number or hash, or revset expression.
Merged in Ideenmodellierer/ostc4/Improment_NVM (pull request #37) Improment NVM
author heinrichsweikamp <bitbucket@heinrichsweikamp.com>
date Wed, 15 Jan 2020 10:53:15 +0000
parents f9458e979154
children 9eeab3fead8f
comparison
equal deleted inserted replaced
403:6f30f2011667 416:bcf447646e07
36 #include "i2c.h" 36 #include "i2c.h"
37 #include "spi.h" 37 #include "spi.h"
38 #include "RTE_FlashAccess.h" // to store compass_calib_data 38 #include "RTE_FlashAccess.h" // to store compass_calib_data
39 39
40 #include "stm32f4xx_hal.h" 40 #include "stm32f4xx_hal.h"
41
42 extern uint32_t time_elapsed_ms(uint32_t ticksstart,uint32_t ticksnow);
41 43
42 /// split byte to bits 44 /// split byte to bits
43 typedef struct{ 45 typedef struct{
44 uint8_t bit0:1; ///< split byte to bits 46 uint8_t bit0:1; ///< split byte to bits
45 uint8_t bit1:1; ///< split byte to bits 47 uint8_t bit1:1; ///< split byte to bits
1351 /// 1353 ///
1352 /// @return always 0 1354 /// @return always 0
1353 // =============================================================================== 1355 // ===============================================================================
1354 int compass_calib_common(void) 1356 int compass_calib_common(void)
1355 { 1357 {
1356 SCompassCalib g; 1358 SCompassCalib g;
1357 1359
1358 // Starts with no calibration at all: 1360 // Starts with no calibration at all:
1359 compass_reset_calibration(&g); 1361 compass_reset_calibration(&g);
1360 1362 uint32_t tickstart = 0;
1361 int64_t tickstart = 0; 1363 tickstart = HAL_GetTick();
1362 uint32_t ticks = 0; 1364 /* run calibration for one minute */
1363 uint32_t lasttick = 0; 1365 while(time_elapsed_ms(tickstart,HAL_GetTick()) < 60000)
1364 tickstart = HAL_GetTick();
1365 // Eine Minute kalibrieren
1366 while((ticks) < 60 * 1000)
1367 { 1366 {
1368 compass_read(); 1367 while((SPI_Evaluate_RX_Data() == 0) && (time_elapsed_ms(tickstart,HAL_GetTick()) < 60000))
1369 acceleration_read(); 1368 {
1370 compass_calc_roll_pitch_only(); 1369 HAL_Delay(1);
1370 }
1371 compass_read();
1372 acceleration_read();
1373 compass_calc_roll_pitch_only();
1371 1374
1372 if((hardwareCompass == compass_generation1 ) //HMC5883L) 1375 if((hardwareCompass == compass_generation1 ) //HMC5883L)
1373 &&((compass_DX_f == -4096) || 1376 &&((compass_DX_f == -4096) ||
1374 (compass_DY_f == -4096) || 1377 (compass_DY_f == -4096) ||
1375 (compass_DZ_f == -4096) )) 1378 (compass_DZ_f == -4096) ))
1376 { 1379 {
1377 if(compass_gain == 0) 1380 if(compass_gain == 0)
1378 return -1; 1381 return -1;
1379 compass_gain--; 1382 compass_gain--;
1380 1383 compass_init(1, compass_gain);
1381 compass_init(1, compass_gain); 1384 compass_reset_calibration(&g);
1382 compass_reset_calibration(&g); 1385 continue;
1383 //tickstart = HAL_GetTick(); 1386 }
1384 continue; 1387
1385 } 1388 copyCompassDataDuringCalibration(compass_DX_f,compass_DY_f,compass_DZ_f);
1386 1389 compass_add_calibration(&g);
1387 copyCompassDataDuringCalibration(compass_DX_f,compass_DY_f,compass_DZ_f);
1388 compass_add_calibration(&g);
1389 HAL_Delay(1);
1390 lasttick = HAL_GetTick();
1391 if(lasttick == 0)
1392 {
1393 tickstart = -ticks;
1394 }
1395 HAL_Delay(1);
1396 ticks = lasttick - tickstart;
1397 SPI_Evaluate_RX_Data();
1398 } 1390 }
1399 1391
1400 compass_solve_calibration(&g); 1392 compass_solve_calibration(&g);
1401 1393
1402 tfull32 dataBlock[4]; 1394 tfull32 dataBlock[4];
1403 dataBlock[0].Word16.low16 = compass_CX_f; 1395 dataBlock[0].Word16.low16 = compass_CX_f;
1404 dataBlock[0].Word16.hi16 = compass_CY_f; 1396 dataBlock[0].Word16.hi16 = compass_CY_f;
1405 dataBlock[1].Word16.low16 = compass_CZ_f; 1397 dataBlock[1].Word16.low16 = compass_CZ_f;
1406 dataBlock[1].Word16.hi16 = 0xFFFF; 1398 dataBlock[1].Word16.hi16 = 0xFFFF;
1407 dataBlock[2].Full32 = 0x7FFFFFFF; 1399 dataBlock[2].Full32 = 0x7FFFFFFF;
1408 dataBlock[3].Full32 = 0x7FFFFFFF; 1400 dataBlock[3].Full32 = 0x7FFFFFFF;
1409 BFA_writeDataBlock((uint32_t *)dataBlock); 1401 BFA_writeDataBlock((uint32_t *)dataBlock);
1410 1402
1411 return 0; 1403 return 0;
1412 } 1404 }
1413 1405