Diff: run.cpp

Revision:

84:ff48e01ea76b

Parent:

83:e69ab831031c

Child:

86:456f00d52974

--- a/run.cpp	Wed Sep 22 09:30:45 2021 +0000
+++ b/run.cpp	Wed Sep 29 04:50:51 2021 +0000
@@ -53,29 +53,23 @@
         ekf.updateNominal(gyro,acc,accref,att_dt);
         ekf.updateErrState(gyro,acc, att_dt);
         if(obsCount == 50){
-            if(ekf.determinDynStatus(acc,accref)){
-                //ekf.updateMeasures(gyro,acc,accref);
-            }else{
-                //ekf.updateStaticMeasures(gyro,acc,accref);
-            }
-            //ekf.updateGPSVelocity(vi.x,vi.y,vi.z);
+            ekf.updateGPSVelocity(vi.x,vi.y,vi.z,acc,accref);
             obsCount = 0;
         }else{
             if(ekf.determinDynStatus(acc,accref)){
-                ekf.updateAccMeasures(acc,accref);
+                //ekf.updateAccMeasures(acc,accref);
             }else{   
-                ekf.updateStaticAccMeasures(acc,accref);
+                //ekf.updateStaticAccMeasures(acc,accref);
             }
             obsCount += 1;
         }
-        
+        //ekf.updateSinkRate(vi.z,acc,accref);
         ekf.updateGyroBiasConstraints(gyro);
-        //ekf.updateSinkRate(vi.z);
         
         ekf.fuseErr2Nominal();
         ekf.resetBias();
         ekf.computeAngles(rpy, rpy_align);
-        //ekf.computeVb(vb);
+        ekf.computeVb(vb);
         PIDtick.loop(); 
         
         float tend = _t.read();