Diff: setup.cpp

Revision:

61:c05353850017

Parent:

60:b89ac376fba3

Child:

65:ea184054e659

--- a/setup.cpp	Wed Jun 02 06:14:47 2021 +0000
+++ b/setup.cpp	Thu Jun 03 11:28:13 2021 +0000
@@ -70,40 +70,42 @@
         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;
-        }   
+            magbiasMin[0] = float(read_bias.i[1])/1000.0f;
+            magbiasMin[1] = float(read_bias.i[2])/1000.0f;
+            magbiasMin[2] = float(read_bias.i[3])/1000.0f;
+            magbiasMax[0] = float(read_bias.i[4])/1000.0f;
+            magbiasMax[1] = float(read_bias.i[5])/1000.0f;
+            magbiasMax[2] = float(read_bias.i[6])/1000.0f;
+        }
+        magCalibrator.setExtremes(magbiasMin,magbiasMax);
     }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]);
+    magCalibrator.getExtremes(magbiasMin,magbiasMax);
+    pc.printf("Initial Magbias (Min) : %f, %f, %f\r\n", magbiasMin[0], magbiasMin[1], magbiasMin[2]);
+    pc.printf("Initial Magbias (Max) : %f, %f, %f\r\n", magbiasMax[0], magbiasMax[1], magbiasMax[2]);
+    
     pc.printf("Mag Calibration Start\r\n");
     
-    
-    float f = 0;
-    
+    float inputMag[3];
+    float outputMag[3];
     while(1)
     {
         mag_sensor.getAxis(mdata); // flush the magnetmeter
-        magval[0] = (mdata.x - magbias[0]);
-        magval[1] = (mdata.y - magbias[1]);
-        magval[2] = (mdata.z - magbias[2]);
-        float mag_r = magval[0]*magval[0] + magval[1]*magval[1] + magval[2]*magval[2];
-        float lr = 0.000001f;
-        f = mag_r - magbias[3]*magbias[3];
-        magbias[0] = magbias[0] + 4 * lr * f * magval[0];
-        magbias[1] = magbias[1] + 4 * lr * f * magval[1];
-        magbias[2] = magbias[2] + 4 * lr * f * magval[2];
-        magbias[3] = magbias[3] + 4 * lr * f * magbias[3];   
+        inputMag[0] = mdata.x;
+        inputMag[1] = mdata.y;
+        inputMag[2] = mdata.z;
+        magCalibrator.run(inputMag,outputMag);
         if(userButton.read() == 1)
         {
             break;
         }
         wait(0.001);
     }
-    pc.printf("Magbias : %f, %f, %f, %f\r\n", magbias[0], magbias[1], magbias[2], magbias[3]);
+    magCalibrator.getExtremes(magbiasMin,magbiasMax);
+    pc.printf("Magbias (Min) : %f, %f, %f\r\n", magbiasMin[0], magbiasMin[1], magbiasMin[2]);
+    pc.printf("Magbias (Max) : %f, %f, %f\r\n", magbiasMax[0], magbiasMax[1], magbiasMax[2]);
+    magCalibrator.getExtremes(magbiasMin,magbiasMax);
     pc.printf("Determine Position of MBED\r\n");
     wait(1);
     pc.printf("Press the user button\r\n");
@@ -184,39 +186,49 @@
     
     U transfer_data;
     transfer_data.i[0] = pos_tail;
-    for (int i = 1; i < 5; i++)
+    for (int i = 0; i < 3; i++)
     {
-        if (!isnan(magbias[i - 1]))
-            transfer_data.i[i] = int(magbias[i - 1]*1000);  // intに丸めた値を送る。
+        if (!isnan(magbiasMax[i]))
+            transfer_data.i[i+1] = int(magbiasMin[i]*1000);  // intに丸めた値を送る。
         else
         {
             pc.printf("Mag bias is NOT transferred\n");
-            transfer_data.i[i] = 100;
+            transfer_data.i[i+1] = 100;
+        }
+    }
+    for (int i = 0; i < 3; i++)
+    {
+        if (!isnan(magbiasMax[i]))
+            transfer_data.i[i+4] = int(magbiasMax[i]*1000);  // intに丸めた値を送る。
+        else
+        {
+            pc.printf("Mag bias is NOT transferred\n");
+            transfer_data.i[i+4] = 100;
         }
     }
     // gxs,gys,gzsを送る
     int gxyzs[3] = {gxs, gys, gzs};
-    for (int i = 5; i < 8; i++)
+    for (int i = 0; i < 3; i++)
     {
-        if (!isnan(gxyzs[i - 5]))
-            transfer_data.i[i] = gxyzs[i - 5];
+        if (!isnan(gxyzs[i]))
+            transfer_data.i[i+7] = gxyzs[i];
         else
         {
             pc.printf("gxyzs is NOT transferred\n");
-            transfer_data.i[i] = 0;
+            transfer_data.i[i+7] = 0;
         }
     }
     
     // ave_pitch,ave_rollを送る
     int ave_pr[2] = {ave_pitch*100, ave_roll*100};
-    for (int i = 8; i < 10; i++)
+    for (int i = 0; i < 2; i++)
     {
-        if (!isnan(ave_pr[i - 8]))
-            transfer_data.i[i] = ave_pr[i - 8];
+        if (!isnan(ave_pr[i]))
+            transfer_data.i[i+10] = ave_pr[i];
         else
         {
           pc.printf("alignment data is NOT transferred\n");
-          transfer_data.i[i] = 0;
+          transfer_data.i[i+10] = 0;
         }
     }
 //    transfer_data.i[10] = tail_address[pos_tail];