Diff: hil.cpp

Revision:

143:53808e4e684c

Parent:

141:725321fe2949

--- a/hil.cpp	Fri Dec 10 11:20:13 2021 +0000
+++ b/hil.cpp	Fri Jun 24 05:44:34 2022 +0000
@@ -6,12 +6,12 @@
         case 1:
             NVIC_SystemReset();
             break;
+        case 2:
+            hilDataOut = vp.commandVal;
+            break;
         default : 
             break;
         }
-    rpy_align(0) = 0.0f;
-    rpy_align(1) = 0.0f;
-    accref(2) =  1.0f;
     
     
     lsm.readAccel();
@@ -41,10 +41,6 @@
     magraw(1) = float(vp.magData[1])/1000.0f + (lsm.my-magref(1));
     magraw(2) = float(vp.magData[2])/1000.0f + (lsm.mz-magref(2));
 
-    //magres = magCalibrator.calcResidial(magraw);
-    //if(magres > magresThreshold){
-    //    magCalibrator.updateParams(magraw,0.001f);
-    //};
     Vector3f magmod = magraw;
     mag(0) = magmod(0);
     mag(1) = magmod(1);