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:
- 84:ff48e01ea76b
- Parent:
- 83:e69ab831031c
- Child:
- 86:456f00d52974
diff -r e69ab831031c -r ff48e01ea76b run.cpp --- 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();