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@143:53808e4e684c, 23 months ago (annotated)
- Committer:
- NaotoMorita
- Date:
- Fri Jun 24 05:44:34 2022 +0000
- Revision:
- 143:53808e4e684c
- Parent:
- 141:725321fe2949
- Child:
- 144:b3a713b4f1c4
complete
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 | 143:53808e4e684c | 5 | preflightCalibration(); |
cocorlow | 141:725321fe2949 | 6 | |
NaotoMorita | 143:53808e4e684c | 7 | setEskfCov(); |
cocorlow | 141:725321fe2949 | 8 | |
cocorlow | 141:725321fe2949 | 9 | MatrixXf Rgps(5,5); |
NaotoMorita | 143:53808e4e684c | 10 | setDiag(Rgps,1.5f); |
NaotoMorita | 143:53808e4e684c | 11 | Rgps(2,2) = 0.1f; |
cocorlow | 141:725321fe2949 | 12 | Rgps(3,3) = 0.1f; |
cocorlow | 141:725321fe2949 | 13 | Rgps(4,4) = 0.1f; |
cocorlow | 141:725321fe2949 | 14 | |
NaotoMorita | 143:53808e4e684c | 15 | MatrixXf Rwhole = MatrixXf::Zero(9,9); |
NaotoMorita | 143:53808e4e684c | 16 | Rwhole(0,0) = 1.5f; |
NaotoMorita | 143:53808e4e684c | 17 | Rwhole(1,1) = 1.5f; |
NaotoMorita | 143:53808e4e684c | 18 | Rwhole(2,2) = 0.1f; |
NaotoMorita | 143:53808e4e684c | 19 | Rwhole(3,3) = 0.1f; |
NaotoMorita | 143:53808e4e684c | 20 | Rwhole(4,4) = 0.1f; |
NaotoMorita | 143:53808e4e684c | 21 | Rwhole(5,5) = 5000.0f; |
NaotoMorita | 143:53808e4e684c | 22 | Rwhole(6,6) = 5000.0f; |
NaotoMorita | 143:53808e4e684c | 23 | Rwhole(7,7) = 5000.0f; |
NaotoMorita | 143:53808e4e684c | 24 | Rwhole(8,8) = 0.007f; |
NaotoMorita | 143:53808e4e684c | 25 | |
cocorlow | 141:725321fe2949 | 26 | |
cocorlow | 141:725321fe2949 | 27 | _t.start(); |
NaotoMorita | 143:53808e4e684c | 28 | |
NaotoMorita | 143:53808e4e684c | 29 | preflightCheck(); |
NaotoMorita | 143:53808e4e684c | 30 | wait(1.0f); |
NaotoMorita | 143:53808e4e684c | 31 | //usaPack通信開始 制御ループのアタッチ |
cocorlow | 141:725321fe2949 | 32 | pc.Subscribe(0000, &(vp)); |
cocorlow | 141:725321fe2949 | 33 | LoopTicker PIDtick; |
cocorlow | 141:725321fe2949 | 34 | PIDtick.attach(calcServoOut,PID_dt); |
cocorlow | 141:725321fe2949 | 35 | |
cocorlow | 141:725321fe2949 | 36 | float tgps = _t.read(); |
cocorlow | 141:725321fe2949 | 37 | float theading = _t.read(); |
NaotoMorita | 143:53808e4e684c | 38 | float tstart = _t.read(); |
cocorlow | 141:725321fe2949 | 39 | while(1) |
cocorlow | 141:725321fe2949 | 40 | { |
cocorlow | 141:725321fe2949 | 41 | float tstart = _t.read(); |
cocorlow | 141:725321fe2949 | 42 | //センサの値を取得 |
cocorlow | 141:725321fe2949 | 43 | if(hilFlag == true){ |
cocorlow | 141:725321fe2949 | 44 | getHilIMUval(); |
cocorlow | 141:725321fe2949 | 45 | }else{ |
cocorlow | 141:725321fe2949 | 46 | getIMUval(); |
cocorlow | 141:725321fe2949 | 47 | } |
cocorlow | 141:725321fe2949 | 48 | //ekfの更新 |
cocorlow | 141:725321fe2949 | 49 | eskf.updateNominal(acc, gyro, att_dt); |
cocorlow | 141:725321fe2949 | 50 | eskf.updateErrState(acc, gyro, att_dt); |
cocorlow | 141:725321fe2949 | 51 | |
cocorlow | 141:725321fe2949 | 52 | if(hilFlag == true){ |
cocorlow | 141:725321fe2949 | 53 | if(tstart-tgps>0.2f){ |
cocorlow | 141:725321fe2949 | 54 | getHilGPSval(); |
cocorlow | 141:725321fe2949 | 55 | tgps = _t.read(); |
cocorlow | 141:725321fe2949 | 56 | gpsUpdateFlag = true; |
cocorlow | 141:725321fe2949 | 57 | }else{ |
cocorlow | 141:725321fe2949 | 58 | gpsUpdateFlag = false; |
cocorlow | 141:725321fe2949 | 59 | } |
cocorlow | 141:725321fe2949 | 60 | }else{ |
cocorlow | 141:725321fe2949 | 61 | if(userButton.read()==1) |
cocorlow | 141:725321fe2949 | 62 | { |
cocorlow | 141:725321fe2949 | 63 | gpsLlh0Fixed = false; |
cocorlow | 141:725321fe2949 | 64 | }; |
cocorlow | 141:725321fe2949 | 65 | getGPSval(); |
cocorlow | 141:725321fe2949 | 66 | } |
cocorlow | 141:725321fe2949 | 67 | |
NaotoMorita | 143:53808e4e684c | 68 | if(gpsUpdateFlag == true){ |
NaotoMorita | 143:53808e4e684c | 69 | float heading = std::atan2(-mag(1),mag(0)); |
NaotoMorita | 143:53808e4e684c | 70 | Vector3f dynacc = eskf.calcDynAcc(acc); |
NaotoMorita | 143:53808e4e684c | 71 | dynaccnorm2 = dynacc.squaredNorm(); |
NaotoMorita | 143:53808e4e684c | 72 | if(dynaccnorm2 > 0.16f){ |
NaotoMorita | 143:53808e4e684c | 73 | Rwhole(5,5) = 100.0f; |
NaotoMorita | 143:53808e4e684c | 74 | Rwhole(6,6) = 100.0f; |
NaotoMorita | 143:53808e4e684c | 75 | Rwhole(7,7) = 100.0f; |
NaotoMorita | 143:53808e4e684c | 76 | }else{ |
NaotoMorita | 143:53808e4e684c | 77 | Rwhole(5,5) = 5000.0f; |
NaotoMorita | 143:53808e4e684c | 78 | Rwhole(6,6) = 5000.0f; |
NaotoMorita | 143:53808e4e684c | 79 | Rwhole(7,7) = 5000.0f; |
cocorlow | 141:725321fe2949 | 80 | } |
NaotoMorita | 143:53808e4e684c | 81 | eskf.updateWhole(pi, palt, vi,acc,heading, Rwhole); |
cocorlow | 141:725321fe2949 | 82 | } |
cocorlow | 141:725321fe2949 | 83 | |
cocorlow | 141:725321fe2949 | 84 | PIDtick.loop(); |
cocorlow | 141:725321fe2949 | 85 | |
cocorlow | 141:725321fe2949 | 86 | //制御時間を計測 |
cocorlow | 141:725321fe2949 | 87 | float tend = _t.read(); |
cocorlow | 141:725321fe2949 | 88 | att_dt = (tend-tstart); |
cocorlow | 141:725321fe2949 | 89 | } |
osaka | 87:89bbbcdb667b | 90 | } |