solaESKF_EIGEN
Dependencies: mbed LPS25HB_I2C LSM9DS1 PIDcontroller LoopTicker GPSUBX_UART_Eigen SBUS_without_mainfile MedianFilter Eigen UsaPack solaESKF_Eigen Vector3 CalibrateMagneto FastPWM
Diff: run.cpp
- Revision:
- 86:456f00d52974
- Parent:
- 84:ff48e01ea76b
- Child:
- 87:89bbbcdb667b
diff -r bc03f862a316 -r 456f00d52974 run.cpp --- 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();