Diff: imu.cpp

Revision:

61:c05353850017

Parent:

56:888379912f81

Child:

66:e5afad70fdd8

--- a/imu.cpp	Wed Jun 02 06:14:47 2021 +0000
+++ b/imu.cpp	Thu Jun 03 11:28:13 2021 +0000
@@ -19,18 +19,24 @@
     gyro.y = float(gy) / GYRO_SSF * 0.0174533f;
     gyro.z = float(gz) / GYRO_SSF * 0.0174533f;
     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]);
-    mag.x = -magval[0]/magbias[3];
-    mag.y = -magval[1]/magbias[3];
-    mag.z = -magval[2]/magbias[3];
+    float inputMag[3];
+    float outputMag[3];
+    inputMag[0] = mdata.x;
+    inputMag[1] = mdata.y;
+    inputMag[2] = mdata.z;
+    magCalibrator.run(inputMag,outputMag);
+    mag.x = -outputMag[0];
+    mag.y = -outputMag[1];
+    mag.z = -outputMag[2];
     
-    float lpc_acc = 0.15f;
+    float lpc_acc = 0.95f;
     LPacc = lpc_acc*acc + (1.0f-lpc_acc)*LPacc;
 
-    float lpc_mag = 0.15f;
+    float lpc_mag = 0.45f;
     LPmag = lpc_mag*mag + (1.0f-lpc_mag)*LPmag;
     
+    val_thmg = 0.99f*val_thmg+0.01f*acos((mag % acc)/mag.Norm()/acc.Norm());
+    accref.z = 0.99f*accref.z + 0.01f*(-acc.Norm());
+    
     magref = ekf.calcMagRef(LPmag/LPmag.Norm());
 }
\ No newline at end of file