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:
- 100:7589b663d462
- Parent:
- 99:98b892ca70ec
- Child:
- 102:1c77ff6e2a85
diff -r 98b892ca70ec -r 7589b663d462 run.cpp --- a/run.cpp Thu Nov 04 09:42:19 2021 +0000 +++ b/run.cpp Fri Nov 05 13:48:22 2021 +0000 @@ -41,7 +41,7 @@ eskf.setGravity(0.0f,0.0f,9.8f); eskf.setPhatPosition(0.1f); eskf.setPhatVelocity(0.1f); - eskf.setPhatAngleError(10.0f); + eskf.setPhatAngleError(0.1f); eskf.setPhatAccBias(0.001f); eskf.setPhatGyroBias(0.001f); eskf.setPhatGravity(0.0000001f); @@ -56,6 +56,8 @@ //Rgpspos(3,3) = 5.0f; Matrix Rgpsvel(3,3); setDiag(Rgpsvel,2.0f); + Matrix Rgps(5,5); + setDiag(Rgps,2.0f); Matrix RaccConst(4,4); @@ -77,19 +79,23 @@ //ekfの更新 eskf.updateNominal(MatrixMath::Vector2mat(acc),MatrixMath::Vector2mat(gyro),att_dt); eskf.updateErrState(MatrixMath::Vector2mat(acc),MatrixMath::Vector2mat(gyro),att_dt); - if(tstart-tgps>0.2f){ - if(hilFlag == true){ + if(hilFlag == true){ + if(tstart-tgps>0.2f){ getHilGPSval(); + tgps = _t.read(); + gpsUpdateFlag = true; }else{ - getGPSval(); + gpsUpdateFlag = false; } - eskf.updateGPSPosition(MatrixMath::Vector2mat(pi),Rgpspos); - eskf.updateGPSVelocity(MatrixMath::Vector2mat(vi),Rgpsvel); - tgps = _t.read(); }else{ - eskf.updateAccConstraints(MatrixMath::Vector2mat(acc),palt,RaccConst); - //eskf.updateGyroConstraints(MatrixMath::Vector2mat(gyro),RgyroConst); + getGPSval; } + if(gpsUpdateFlag == true){ + //eskf.updateGPSPosition(MatrixMath::Vector2mat(pi),Rgpspos); + //eskf.updateGPSVelocity(MatrixMath::Vector2mat(vi),Rgpsvel); + eskf.updateGPS(MatrixMath::Vector2mat(pi),MatrixMath::Vector2mat(vi),Rgps); + } + eskf.updateAccConstraints(MatrixMath::Vector2mat(acc),palt,RaccConst); eskf.fuseErr2Nominal(); PIDtick.loop();