Diff: setup.cpp

Revision:

76:7fd3ac1afe3e

Parent:

73:84ffa0166e6c

Child:

77:2bf856e3eca4

--- a/setup.cpp	Tue Jul 20 11:57:05 2021 +0000
+++ b/setup.cpp	Sat Aug 07 06:54:58 2021 +0000
@@ -26,6 +26,8 @@
     servoLeft.pulsewidth_us(1500.0); 
     servoThrust.pulsewidth_us(1100.0);
     
+    sd.baud(57600);
+    sd.printf("\r\nFlight Start\r\n");
     accelgyro.initialize();
     //加速度計のフルスケールレンジを設定
     accelgyro.setFullScaleAccelRange(ACCEL_FSR);
@@ -94,10 +96,11 @@
     
     pc.serial.printf("Gyrooffset : 0, 0, 0, %d, %d, %d \r\n",gxs,gys,gzs);
     
-    
+    /*
     pc.serial.printf("Initial Magbias (Min) : %f, %f, %f\r\n", magbiasMin[0], magbiasMin[1], magbiasMin[2]);
     pc.serial.printf("Initial Magbias (Max) : %f, %f, %f\r\n", magbiasMax[0], magbiasMax[1], magbiasMax[2]);
-    
+    */
+    pc.serial.printf("Initial Magbias  : %f, %f, %f, %f \r\n", magbias[0], magbias[1], magbias[2], magbias[3]);
     pc.serial.printf("Acc Scale and Mag Calibration Start\r\n");
     for(int i = 0;i<3;i++){
         accMin[i] = -1.0f;
@@ -106,16 +109,18 @@
     
     accMax[2] = accScaleCalibrate(5);
     accMin[0] = accScaleCalibrate(1);
-    accMax[0] =accScaleCalibrate(2);
-    accMin[1] =accScaleCalibrate(3);
-    accMax[1] =accScaleCalibrate(4);
-    accMin[2] =accScaleCalibrate(6);
+    accMax[0] = accScaleCalibrate(2);
+    accMin[1] = accScaleCalibrate(3);
+    accMax[1] = accScaleCalibrate(4);
+    accMin[2] = accScaleCalibrate(6);
     
+    /*
     magCalibrator.getExtremes(magbiasMin,magbiasMax);
     pc.serial.printf("Magbias (Min) : %f, %f, %f\r\n", magbiasMin[0], magbiasMin[1], magbiasMin[2]);
     pc.serial.printf("Magbias (Max) : %f, %f, %f\r\n", magbiasMax[0], magbiasMax[1], magbiasMax[2]);
     magCalibrator.setExtremes(magbiasMin,magbiasMax);
-    
+    */
+    pc.serial.printf("Magbias  : %f, %f, %f, %f \r\n", magbias[0], magbias[1], magbias[2], magbias[3]);
     pc.serial.printf("accMin : %f, %f, %f\r\n", accMin[0], accMin[1], accMin[2]);
     pc.serial.printf("accMax : %f, %f, %f\r\n", accMax[0], accMax[1], accMax[2]);
     
@@ -128,14 +133,10 @@
     rpy_align.x = 0.0f*M_PI/180.0f;
     float ave_pitch = 0.0f;
     float ave_roll = 0.0f;
-    ekf.Q(4,4) = 0.00001f;
-    ekf.Q(5,5) = 0.00001f;
-    ekf.Q(6,6) = 0.00001f;
-    ekf.Qab(1,1) = 0.0f;
-    ekf.Qab(2,2) = 0.0f;
-    ekf.Qab(3,3) = 0.0f;
+    ekf.setQgbias(0.0f);
+    ekf.setQab(0.0f);
     getIMUval();
-    ekf.triad(acc/acc.Norm(), accref/accref.Norm(), mag/mag.Norm(), magref/magref.Norm());
+    //ekf.triad(acc/acc.Norm(), accref/accref.Norm(), mag/mag.Norm(), magref/magref.Norm());
     Timer _t;
     _t.start();
     for (int i = 0 ; i < 2200; i++)
@@ -256,6 +257,7 @@
     double accx = 0.0;
     double accy = 0.0;
     double accz = 0.0;
+    float magval[3] = {0,0,0};
     for(int i = 0;i<iter_n ;i++)
     {
         accelgyro.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
@@ -263,14 +265,17 @@
         accy += ay/ ACCEL_SSF;
         accz += az/ ACCEL_SSF;
         
-        float inputMag[3];
-        float outputMag[3];
         mag_sensor.getAxis(mdata); // flush the magnetmeter
-        inputMag[0] = mdata.x;
-        inputMag[1] = mdata.y;
-        inputMag[2] = mdata.z;
-        magCalibrator.run(inputMag,outputMag);
-        
+        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.00001f;
+        float 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];
         wait(0.001);
     }
     float returnval = 0.0f;