Diff: hil.cpp

Revision:

122:68efdee114fa

Parent:

111:0fae4fbe2a80

--- a/hil.cpp	Thu Nov 18 10:10:18 2021 +0000
+++ b/hil.cpp	Fri Nov 19 05:44:23 2021 +0000
@@ -37,18 +37,19 @@
     gyro.z = float(vp.gyroData[2]) / 131.0f * 0.0174533f + gyromz;
     
     Matrix magraw(3,1);
-    magraw(1,1) = float(vp.magData[0])/1000.0f + (lsm.mx-magref.x);
-    magraw(2,1) = float(vp.magData[1])/1000.0f + (lsm.my-magref.y);
-    magraw(3,1) = float(vp.magData[2])/1000.0f + (lsm.mz-magref.z);
+    magraw(1,1) = float(vp.magData[0])/1000.0f +(lsm.mx-magref.x);
+    magraw(2,1) = float(vp.magData[1])/1000.0f +(lsm.my-magref.y);
+    magraw(3,1) = float(vp.magData[2])/1000.0f +(lsm.mz-magref.z);
 
-    //magres = magCalibrator.calcResidial(magraw);
-    //if(magres > magresThreshold){
-    //    magCalibrator.updateParams(magraw,0.001f);
-    //};
-    Matrix magmod = magraw;
-    mag.x = magmod(1,1);
-    mag.y = magmod(2,1);
-    mag.z = magmod(3,1);
+    //float inputMag[3];
+    //float outputMag[3];
+    //inputMag[0] = magraw(1,1)*1000.0f;
+    //inputMag[1] = magraw(2,1)*1000.0f;
+    //inputMag[2] = magraw(3,1)*1000.0f;
+    //magCalibrator.run(inputMag,outputMag);
+    mag.x = magraw(1,1);
+    mag.y = magraw(2,1);
+    mag.z = magraw(3,1);
     
     palt =  float(vp.piData[2])/1.0f -(lps.pressureToAltitudeMeters(lps.readPressureMillibars())-palt0);;