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:
- 73:84ffa0166e6c
- Parent:
- 70:99f974d8960e
- Child:
- 74:f67062e7813e
--- a/run.cpp Wed Jun 30 01:16:21 2021 +0000 +++ b/run.cpp Thu Jul 15 05:26:15 2021 +0000 @@ -7,22 +7,22 @@ Timer _t; _t.start(); magCalibrator.setExtremes(magbiasMin,magbiasMax); - for(int i = 0; i < 1000; i++){ - getIMUval(); - } - //ekf.triad(acc/acc.Norm(), accref/accref.Norm(), mag/mag.Norm(), magref/magref.Norm()); ekf.defineQhat(rpy_align); float sum2accnorm = 0; float sumaccnorm = 0; for(int i = 0; i < 1000; i++){ float tstart = _t.read(); - getIMUval(); - ekf.updateQhat(gyro, att_dt); - ekf.updateErrState(gyro, att_dt); + if(hilFlag == true){ + getHILval(); + }else{ + getIMUval(); + } + //ekf.updateQhat(gyro, att_dt); + //ekf.updateErrState(gyro, att_dt); ekf.updateStaticAccMeasures(acc,accref); - ekf.fuseErr2Qhat(); - //ekf.resetGyroBias(gyroBias); - ekf.updateMagMeasures(mag); + //ekf.fuseErr2Qhat(); + //ekf.resetBias(); + //ekf.updateMagMeasures(mag); ekf.computeAngles(rpy, rpy_align); sumaccnorm += acc.Norm(); sum2accnorm += acc.Norm()*acc.Norm(); @@ -32,7 +32,7 @@ accref.z = sumaccnorm / 1000.0f; float sigma_accnorm = sqrt(sum2accnorm/1000.0f-accref.z*accref.z); pc.serial.printf("%f \r\n",sigma_accnorm); - pc.Subscribe(0000, &(posValues)); + pc.Subscribe(0000, &(vp)); LoopTicker PIDtick; PIDtick.attach(calcServoOut,PID_dt); @@ -43,7 +43,11 @@ { float tstart = _t.read(); //姿勢角を更新 - getIMUval(); + if(hilFlag == true){ + getHILval(); + }else{ + getIMUval(); + } ekf.updateQhat(gyro, att_dt); ekf.updateErrState(gyro, att_dt); if(ekf.determinDynStatus(acc,accref)){ @@ -53,7 +57,8 @@ } ekf.fuseErr2Qhat(); ekf.resetBias(); - ekf.updateMagMeasures(mag); + //ekf.updateMagMeasures(mag); + //ekf.gyroBiasEstimation(mag,gyro,att_dt); ekf.computeAngles(rpy, rpy_align); PIDtick.loop();