Eigen Revision
Dependencies: mbed LPS25HB_I2C LSM9DS1 PIDcontroller Autopilot_Eigen LoopTicker GPSUBX_UART_Eigen SBUS_without_mainfile MedianFilter Eigen UsaPack solaESKF_Eigen Vector3 CalibrateMagneto FastPWM
Diff: setup.cpp
- Revision:
- 59:7b9b842efd5e
- Parent:
- 58:a7947322db87
- Child:
- 60:b89ac376fba3
--- a/setup.cpp Tue Jun 01 07:45:09 2021 +0000 +++ b/setup.cpp Wed Jun 02 02:03:44 2021 +0000 @@ -52,8 +52,36 @@ agoffset[4] = gys; agoffset[5] = gzs; pc.printf("Gyrooffset : 0, 0, 0, %d, %d, %d \r\n",gxs,gys,gzs); + pc.printf("Press userbutton without initial Magbias value from EEPROM\r\n"); + int magBiasFromEEPROM = 1; + for(int i = 0;i<50 ;i++){ + wait(0.1); + if(userButton.read() == 1) + { + magBiasFromEEPROM = 0; + break; + } + }; + wait(1); + if(magBiasFromEEPROM==1){ + pc.printf("Read Initial Magbias from EEPROM\r\n"); + U read_bias; + readEEPROM(eeprom_address, eeprom_pointeraddress, read_bias.c, N_EEPROM*4); + wait(3); + for (int i = 0 ; i < N_EEPROM; i ++) + { + magbias[0] = float(read_bias.i[1])/1000.0f; + magbias[1] = float(read_bias.i[2])/1000.0f; + magbias[2] = float(read_bias.i[3])/1000.0f; + magbias[3] = float(read_bias.i[4])/1000.0f; + } + }else{ + pc.printf("Preset Initial Magbias \r\n"); + } + pc.printf("Initial Magbias : %f, %f, %f, %f\r\n", magbias[0], magbias[1], magbias[2], magbias[3]); pc.printf("Mag Calibration Start\r\n"); + float f = 0; while(1)