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)