Diff: run.cpp

Revision:

61:c05353850017

Parent:

56:888379912f81

Child:

66:e5afad70fdd8

--- a/run.cpp	Wed Jun 02 06:14:47 2021 +0000
+++ b/run.cpp	Thu Jun 03 11:28:13 2021 +0000
@@ -8,15 +8,18 @@
     readEEPROM(eeprom_address, eeprom_pointeraddress, read_calib.c, N_EEPROM*4);
     wait(3);
     pos_tail = (int)read_calib.i[0];
-    agoffset[3] = float(read_calib.i[5]);
-    agoffset[4] = float(read_calib.i[6]);
-    agoffset[5] = float(read_calib.i[7]);
-    magbias[0] = float(read_calib.i[1])/1000.0f;
-    magbias[1] = float(read_calib.i[2])/1000.0f;
-    magbias[2] = float(read_calib.i[3])/1000.0f;
-    magbias[3] = float(read_calib.i[4])/1000.0f;
-    rpy_align.y = float(read_calib.i[8])/200000.0f;
-    rpy_align.x = float(read_calib.i[9])/200000.0f;
+    agoffset[3] = float(read_calib.i[7]);
+    agoffset[4] = float(read_calib.i[8]);
+    agoffset[5] = float(read_calib.i[9]);
+    magbiasMin[0] = float(read_calib.i[1])/1000.0f;
+    magbiasMin[1] = float(read_calib.i[2])/1000.0f;
+    magbiasMin[2] = float(read_calib.i[3])/1000.0f;
+    magbiasMax[0] = float(read_calib.i[4])/1000.0f;
+    magbiasMax[1] = float(read_calib.i[5])/1000.0f;
+    magbiasMax[2] = float(read_calib.i[6])/1000.0f;
+    rpy_align.y = float(read_calib.i[10])/200000.0f;
+    rpy_align.x = float(read_calib.i[11])/200000.0f;
+    magCalibrator.setExtremes(magbiasMin,magbiasMax);
 //    tail_address[pos_tail] = (int)read_calib.i[10];
     
     switch(pos_tail){
@@ -37,15 +40,21 @@
     pc.printf("Alignment values are %f(pitch deg) %f(roll deg)\r\n",rpy_align.y*180.0f/M_PI,rpy_align.x*180.0f/M_PI);
     getIMUval();
     ekf.triad(acc/acc.Norm(), accref/accref.Norm(), mag/mag.Norm(), magref/magref.Norm());
+    float val2_thmg = 0;
+    float val2_accnorm = 0;
     float sumLPaccnorm = 0;
     for(int i = 0; i < 1000; i++){
         getIMUval();
         val_thmg += acos((mag % acc)/mag.Norm()/acc.Norm());
+        val2_thmg += (acos((mag % acc)/mag.Norm()/acc.Norm()))*(acos((mag % acc)/mag.Norm()/acc.Norm()));
         sumLPaccnorm += LPacc.Norm();
+        val2_accnorm += LPacc.Norm()*LPacc.Norm();
     }
     accref.z = -sumLPaccnorm / 1000;
     val_thmg /= 1000;
-    
+    sigma_thmg = sqrt(val2_thmg/1000-val_thmg*val_thmg);
+    sigma_accnorm = sqrt(val2_accnorm/1000-accref.z*accref.z);
+    pc.printf("sigma: %f %f \r\n",sigma_thmg,sigma_accnorm);
     for (int i = 0; i < 3; i++)
     {
         if (i == pos_tail)