Diff: servo.cpp

Revision:

69:0caaad87cf1d

Parent:

68:11ffadc7adb1

Child:

71:62eb45ecffe9

diff -r 11ffadc7adb1 -r 0caaad87cf1d servo.cpp
--- a/servo.cpp	Tue Jun 22 12:00:40 2021 +0000
+++ b/servo.cpp	Mon Jun 28 03:30:54 2021 +0000
@@ -70,28 +70,6 @@
     
     servo.pulsewidth_us(servoOut[0]);
      
-    //観測アップデート
-    dynacc = ekf.calcDynAcc(LPacc, accref);
-    th_mg = abs(acos(LPmag/LPmag.Norm() % LPacc/LPacc.Norm())-val_thmg);
-    
-    accnormerr = abs(LPacc.Norm()-accref.Norm());
-    //静止時100個の平均　0.01877522 0.00514146  0.00477393
-
-    //int ang_th  = th_mg < 0.01877522;
-    //int dyn_th = dynaccnorm < 0.00514146;
-    //int norm_th =  accnormerr< 0.00477393;
-    int ang_th  = th_mg < sigma_thmg/200.0;
-    int dyn_th = dynacc.Norm() < sigma_accnorm/50.0;
-    int norm_th =  accnormerr< sigma_accnorm/50.0;
-    if(dyn_th+ang_th+norm_th>0)
-    {
-    //if(dyn_th+ang_th+norm_th>-1){
-            obs_count += 1;
-            ekf.updateAcrossMeasures(LPmag/LPmag.Norm(), magref/magref.Norm(), ekf.Rmag);
-            ekf.updateAcrossMeasures(LPacc/LPacc.Norm(), accref/accref.Norm(), ekf.Racc);
-    } 
-     
-//    if(loop_count >= 10) 
     if(loop_count >= 3)
     {
         writeSdcard();