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:
- 90:96c2b0ed4b96
- Parent:
- 89:c9f64bd655d9
- Child:
- 92:00460f6df439
diff -r c9f64bd655d9 -r 96c2b0ed4b96 run.cpp --- a/run.cpp Mon Oct 25 05:39:34 2021 +0000 +++ b/run.cpp Tue Oct 26 05:36:18 2021 +0000 @@ -7,18 +7,13 @@ _t.start(); //センサの初期化・ジャイロバイアス・加速度スケールの取得 - float sumaccnorm = 0; int n_init = 1; if(hilFlag == false){ - ekf.defineQhat(rpy_align); - for(int i = 0; i < n_init; i++){ - sumaccnorm += acc.Norm(); - } - accref.z = sumaccnorm / float(n_init); + eskf.setQhat(rpy_align.x,rpy_align.y,rpy_align.z); } + eskf.setGravity(0.0f,0.0f,-9.8f); //センサ正常性チェック - //usaPack通信開始 pc.Subscribe(0000, &(vp)); @@ -32,36 +27,13 @@ //センサの値を取得 if(hilFlag == true){ getHILval(); - //getIMUval(); }else{ //getIMUval(); } - //ekfの更新 - 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.updateSinkRate(vi.z,acc,accref); - obsCount = 0; - }else{ - //ekf.updateGPSVelocity(vi.x,vi.y,vi.z,acc,accref); - //ekf.updateAccMeasures(acc,accref); - ekf.updateGPSVelocity(vi.x,vi.y,vi.z,acc,accref); - if(ekf.determinDynStatus(acc,accref)){ - //ekf.updateAccMeasures(acc,accref); - }else{ - //ekf.updateAccMeasures(acc,accref); - } - - obsCount += 1; - } - //ekf.updateSinkRate(vi.z,acc,accref); - //ekf.updateGyroBiasConstraints(gyro); - - ekf.fuseErr2Nominal(); - ekf.resetBias(); - ekf.computeAngles(rpy, rpy_align); + eskf.updateNominal(MatrixMath::Vector2mat(acc),MatrixMath::Vector2mat(gyro),att_dt); + eskf.updateErrState(MatrixMath::Vector2mat(acc),MatrixMath::Vector2mat(gyro),att_dt); + eskf.fuseErr2Nominal(); PIDtick.loop();