solaESKF_EIGEN
Dependencies: mbed LPS25HB_I2C LSM9DS1 PIDcontroller LoopTicker GPSUBX_UART_Eigen SBUS_without_mainfile MedianFilter Eigen UsaPack solaESKF_Eigen Vector3 CalibrateMagneto FastPWM
run.cpp@92:00460f6df439, 2021-10-28 (annotated)
- Committer:
- NaotoMorita
- Date:
- Thu Oct 28 09:44:47 2021 +0000
- Revision:
- 92:00460f6df439
- Parent:
- 90:96c2b0ed4b96
- Child:
- 93:b827f78a717a
for PmodNAV
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
cocorlow | 56:888379912f81 | 1 | #include "global.hpp" |
cocorlow | 56:888379912f81 | 2 | |
cocorlow | 56:888379912f81 | 3 | void run() |
cocorlow | 56:888379912f81 | 4 | { |
NaotoMorita | 68:b9f6938fab9d | 5 | wait(0.5); |
NaotoMorita | 77:2bf856e3eca4 | 6 | |
osaka | 87:89bbbcdb667b | 7 | //センサの初期化・ジャイロバイアス・加速度スケールの取得 |
NaotoMorita | 77:2bf856e3eca4 | 8 | if(hilFlag == false){ |
NaotoMorita | 90:96c2b0ed4b96 | 9 | eskf.setQhat(rpy_align.x,rpy_align.y,rpy_align.z); |
NaotoMorita | 77:2bf856e3eca4 | 10 | } |
osaka | 87:89bbbcdb667b | 11 | |
osaka | 87:89bbbcdb667b | 12 | //センサ正常性チェック |
osaka | 87:89bbbcdb667b | 13 | //usaPack通信開始 |
NaotoMorita | 77:2bf856e3eca4 | 14 | pc.Subscribe(0000, &(vp)); |
NaotoMorita | 77:2bf856e3eca4 | 15 | |
osaka | 87:89bbbcdb667b | 16 | //制御ループのアタッチ |
NaotoMorita | 77:2bf856e3eca4 | 17 | LoopTicker PIDtick; |
NaotoMorita | 77:2bf856e3eca4 | 18 | PIDtick.attach(calcServoOut,PID_dt); |
NaotoMorita | 77:2bf856e3eca4 | 19 | |
NaotoMorita | 92:00460f6df439 | 20 | Timer _t; |
NaotoMorita | 92:00460f6df439 | 21 | _t.start(); |
NaotoMorita | 92:00460f6df439 | 22 | |
NaotoMorita | 92:00460f6df439 | 23 | float tgps = _t.read(); |
NaotoMorita | 92:00460f6df439 | 24 | Matrix Rgpspos(3,3); |
NaotoMorita | 92:00460f6df439 | 25 | Rgpspos(1,1) = 5000.0f; |
NaotoMorita | 92:00460f6df439 | 26 | Rgpspos(2,2) = 5000.0f; |
NaotoMorita | 92:00460f6df439 | 27 | Rgpspos(3,3) = 5000.0f; |
NaotoMorita | 92:00460f6df439 | 28 | Matrix Rgpsvel(3,3); |
NaotoMorita | 92:00460f6df439 | 29 | Rgpsvel(1,1) = 5000.0f; |
NaotoMorita | 92:00460f6df439 | 30 | Rgpsvel(2,2) = 5000.0f; |
NaotoMorita | 92:00460f6df439 | 31 | Rgpsvel(3,3) = 5000.0f; |
NaotoMorita | 92:00460f6df439 | 32 | Matrix RaccConst(3,3); |
NaotoMorita | 92:00460f6df439 | 33 | RaccConst(1,1) = 980000.0f; |
NaotoMorita | 92:00460f6df439 | 34 | RaccConst(2,2) = 980000.0f; |
NaotoMorita | 92:00460f6df439 | 35 | RaccConst(3,3) = 980000.0f; |
NaotoMorita | 92:00460f6df439 | 36 | Matrix RgyroConst(2,2); |
NaotoMorita | 92:00460f6df439 | 37 | RgyroConst(1,1) = 100000.0f; |
NaotoMorita | 92:00460f6df439 | 38 | RgyroConst(2,2) = 100000.0f; |
NaotoMorita | 77:2bf856e3eca4 | 39 | while(1) |
NaotoMorita | 77:2bf856e3eca4 | 40 | { |
NaotoMorita | 68:b9f6938fab9d | 41 | float tstart = _t.read(); |
osaka | 87:89bbbcdb667b | 42 | //センサの値を取得 |
NaotoMorita | 73:84ffa0166e6c | 43 | if(hilFlag == true){ |
NaotoMorita | 92:00460f6df439 | 44 | getHilIMUval(); |
NaotoMorita | 92:00460f6df439 | 45 | getIMUval(); |
NaotoMorita | 73:84ffa0166e6c | 46 | }else{ |
osaka | 87:89bbbcdb667b | 47 | //getIMUval(); |
NaotoMorita | 73:84ffa0166e6c | 48 | } |
osaka | 87:89bbbcdb667b | 49 | //ekfの更新 |
NaotoMorita | 90:96c2b0ed4b96 | 50 | eskf.updateNominal(MatrixMath::Vector2mat(acc),MatrixMath::Vector2mat(gyro),att_dt); |
NaotoMorita | 90:96c2b0ed4b96 | 51 | eskf.updateErrState(MatrixMath::Vector2mat(acc),MatrixMath::Vector2mat(gyro),att_dt); |
NaotoMorita | 92:00460f6df439 | 52 | if(tstart-tgps>0.2f){ |
NaotoMorita | 92:00460f6df439 | 53 | if(hilFlag == true){ |
NaotoMorita | 92:00460f6df439 | 54 | getHilGPSval(); |
NaotoMorita | 92:00460f6df439 | 55 | }else{ |
NaotoMorita | 92:00460f6df439 | 56 | //getGPSval(); |
NaotoMorita | 92:00460f6df439 | 57 | } |
NaotoMorita | 92:00460f6df439 | 58 | eskf.updateGPSPosition(MatrixMath::Vector2mat(pi),Rgpspos); |
NaotoMorita | 92:00460f6df439 | 59 | eskf.updateGPSVelocity(MatrixMath::Vector2mat(vi),Rgpsvel); |
NaotoMorita | 92:00460f6df439 | 60 | tgps = _t.read(); |
NaotoMorita | 92:00460f6df439 | 61 | }else{ |
NaotoMorita | 92:00460f6df439 | 62 | //eskf.updateAccConstraints(MatrixMath::Vector2mat(acc),RaccConst); |
NaotoMorita | 92:00460f6df439 | 63 | //eskf.updateGyroConstraints(MatrixMath::Vector2mat(gyro),RgyroConst); |
NaotoMorita | 92:00460f6df439 | 64 | eskf.updateGPSPosition(MatrixMath::Vector2mat(pi),Rgpspos); |
NaotoMorita | 92:00460f6df439 | 65 | eskf.updateGPSVelocity(MatrixMath::Vector2mat(vi),Rgpsvel); |
NaotoMorita | 92:00460f6df439 | 66 | } |
NaotoMorita | 90:96c2b0ed4b96 | 67 | eskf.fuseErr2Nominal(); |
osaka | 87:89bbbcdb667b | 68 | |
NaotoMorita | 77:2bf856e3eca4 | 69 | PIDtick.loop(); |
NaotoMorita | 77:2bf856e3eca4 | 70 | |
osaka | 87:89bbbcdb667b | 71 | //制御時間を計測 |
NaotoMorita | 68:b9f6938fab9d | 72 | float tend = _t.read(); |
NaotoMorita | 68:b9f6938fab9d | 73 | att_dt = (tend-tstart); |
cocorlow | 56:888379912f81 | 74 | } |
osaka | 87:89bbbcdb667b | 75 | } |