Eigen Revision
Dependencies: mbed LPS25HB_I2C LSM9DS1 PIDcontroller Autopilot_Eigen LoopTicker GPSUBX_UART_Eigen SBUS_without_mainfile MedianFilter Eigen UsaPack solaESKF_Eigen Vector3 CalibrateMagneto FastPWM
Diff: run.cpp
- Revision:
- 74:f67062e7813e
- Parent:
- 73:84ffa0166e6c
- Child:
- 75:a505b9896da1
diff -r 84ffa0166e6c -r f67062e7813e run.cpp --- 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();