Diff: run.cpp

Revision:

86:456f00d52974

Parent:

84:ff48e01ea76b

Child:

87:89bbbcdb667b

--- a/run.cpp	Fri Oct 08 08:11:25 2021 +0000
+++ b/run.cpp	Mon Oct 18 12:18:15 2021 +0000
@@ -15,13 +15,9 @@
         ekf.defineQhat(rpy_align);
         for(int i = 0; i < n_init; i++){
             float tstart = _t.read();
-            if(hilFlag == true){
-                getHILval();
-            }else{
-                getIMUval();
-            }
+            getIMUval();
             ekf.updateStaticAccMeasures(acc,accref);
-            ekf.updateGyroBiasConstraints(gyro);
+            //ekf.updateGyroBiasConstraints(gyro);
             ekf.fuseErr2Nominal();
             ekf.resetBias();
             //ekf.updateMagMeasures(mag);
@@ -53,18 +49,23 @@
         ekf.updateNominal(gyro,acc,accref,att_dt);
         ekf.updateErrState(gyro,acc, att_dt);
         if(obsCount == 50){
-            ekf.updateGPSVelocity(vi.x,vi.y,vi.z,acc,accref);
+            //ekf.updateGPSVelocity(vi.x,vi.y,vi.z,acc,accref);
+            //ekf.updateSinkRate(vi.z,acc,accref);
             obsCount = 0;
         }else{
+            //ekf.updateGPSVelocity(vi.x,vi.y,vi.z,acc,accref);
+            //ekf.updateAccMeasures(acc,accref);
+            
             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.updateGyroBiasConstraints(gyro);
         
         ekf.fuseErr2Nominal();
         ekf.resetBias();