Diff: run.cpp

Revision:

74:f67062e7813e

Parent:

73:84ffa0166e6c

Child:

75:a505b9896da1

--- a/run.cpp	Thu Jul 15 05:26:15 2021 +0000
+++ b/run.cpp	Tue Jul 20 05:52:29 2021 +0000
@@ -22,7 +22,7 @@
         ekf.updateStaticAccMeasures(acc,accref);
         //ekf.fuseErr2Qhat();
         //ekf.resetBias();
-        //ekf.updateMagMeasures(mag);
+        ekf.updateMagMeasures(mag);
         ekf.computeAngles(rpy, rpy_align);
         sumaccnorm += acc.Norm();
         sum2accnorm += acc.Norm()*acc.Norm();
@@ -48,17 +48,19 @@
         }else{
             getIMUval();
         }
+
         ekf.updateQhat(gyro, att_dt);
         ekf.updateErrState(gyro, att_dt);
+        //ekf.updateAllMeasures(acc,gyro,mag,accref,tstart,att_dt);
         if(ekf.determinDynStatus(acc,accref)){
             ekf.updateAccMeasures(acc,accref);
         }else{
             ekf.updateStaticAccMeasures(acc,accref);
         }
+        ekf.updateGyroBiasMeasures(gyro,mag,tstart,att_dt);
         ekf.fuseErr2Qhat();
         ekf.resetBias();
         //ekf.updateMagMeasures(mag);
-        //ekf.gyroBiasEstimation(mag,gyro,att_dt);
         ekf.computeAngles(rpy, rpy_align);
         PIDtick.loop();