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
run.cpp@106:2d854e92cebb, 2021-11-12 (annotated)
- Committer:
- NaotoMorita
- Date:
- Fri Nov 12 09:03:41 2021 +0000
- Revision:
- 106:2d854e92cebb
- Parent:
- 104:20b8caa29185
- Child:
- 111:0fae4fbe2a80
mag mod
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 | } |
NaotoMorita | 99:98b892ca70ec | 11 | |
NaotoMorita | 93:b827f78a717a | 12 | int n_init = 1000; |
NaotoMorita | 93:b827f78a717a | 13 | for(int i = 0;i<n_init;i++){ |
NaotoMorita | 93:b827f78a717a | 14 | lsm.readAccel(); |
NaotoMorita | 93:b827f78a717a | 15 | lsm.readMag(); |
NaotoMorita | 93:b827f78a717a | 16 | lsm.readGyro(); |
NaotoMorita | 93:b827f78a717a | 17 | agoffset[0] += lsm.ax * 9.8f; |
NaotoMorita | 93:b827f78a717a | 18 | agoffset[1] += lsm.ay * 9.8f; |
NaotoMorita | 93:b827f78a717a | 19 | agoffset[2] += lsm.az * 9.8f-9.8f; |
NaotoMorita | 93:b827f78a717a | 20 | agoffset[3] +=(lsm.gx * M_PI / 180.0f); |
NaotoMorita | 93:b827f78a717a | 21 | agoffset[4] +=(lsm.gy * M_PI / 180.0f); |
NaotoMorita | 93:b827f78a717a | 22 | agoffset[5] +=(lsm.gz * M_PI / 180.0f); |
NaotoMorita | 99:98b892ca70ec | 23 | palt0 += lps.pressureToAltitudeMeters(lps.readPressureMillibars()); |
NaotoMorita | 103:fec71c2051c5 | 24 | magref.x += lsm.mx; |
NaotoMorita | 103:fec71c2051c5 | 25 | magref.y += lsm.my; |
NaotoMorita | 103:fec71c2051c5 | 26 | magref.z += lsm.mz; |
NaotoMorita | 93:b827f78a717a | 27 | } |
NaotoMorita | 93:b827f78a717a | 28 | for(int i = 0;i<6;i++){ |
NaotoMorita | 93:b827f78a717a | 29 | agoffset[i] /= float(n_init); |
NaotoMorita | 93:b827f78a717a | 30 | } |
NaotoMorita | 103:fec71c2051c5 | 31 | magref.x /= float(n_init); |
NaotoMorita | 103:fec71c2051c5 | 32 | magref.y /= float(n_init); |
NaotoMorita | 103:fec71c2051c5 | 33 | magref.z /= float(n_init); |
NaotoMorita | 103:fec71c2051c5 | 34 | if(hilFlag == false){ |
NaotoMorita | 103:fec71c2051c5 | 35 | eskf.setMagField(magref.x,magref.y,-magref.z); |
NaotoMorita | 103:fec71c2051c5 | 36 | }else{ |
NaotoMorita | 103:fec71c2051c5 | 37 | eskf.setMagField(magref.x,magref.y,magref.z); |
NaotoMorita | 102:1c77ff6e2a85 | 38 | } |
NaotoMorita | 98:bdaa6bbfb1d9 | 39 | palt0 /= float(n_init); |
NaotoMorita | 93:b827f78a717a | 40 | twelite.printf("Sensor offset : %f %f %f %f %f %f\r\n",agoffset[0],agoffset[1],agoffset[2],agoffset[3],agoffset[4],agoffset[5]); |
osaka | 87:89bbbcdb667b | 41 | //センサ正常性チェック |
osaka | 87:89bbbcdb667b | 42 | //usaPack通信開始 |
NaotoMorita | 77:2bf856e3eca4 | 43 | pc.Subscribe(0000, &(vp)); |
NaotoMorita | 77:2bf856e3eca4 | 44 | |
osaka | 87:89bbbcdb667b | 45 | //制御ループのアタッチ |
NaotoMorita | 77:2bf856e3eca4 | 46 | LoopTicker PIDtick; |
NaotoMorita | 77:2bf856e3eca4 | 47 | PIDtick.attach(calcServoOut,PID_dt); |
NaotoMorita | 77:2bf856e3eca4 | 48 | |
NaotoMorita | 99:98b892ca70ec | 49 | //ESKFの共分散設定 |
NaotoMorita | 93:b827f78a717a | 50 | eskf.setGravity(0.0f,0.0f,9.8f); |
NaotoMorita | 93:b827f78a717a | 51 | eskf.setPhatPosition(0.1f); |
NaotoMorita | 93:b827f78a717a | 52 | eskf.setPhatVelocity(0.1f); |
NaotoMorita | 100:7589b663d462 | 53 | eskf.setPhatAngleError(0.1f); |
NaotoMorita | 94:579e875a4244 | 54 | eskf.setPhatAccBias(0.001f); |
NaotoMorita | 94:579e875a4244 | 55 | eskf.setPhatGyroBias(0.001f); |
NaotoMorita | 94:579e875a4244 | 56 | eskf.setPhatGravity(0.0000001f); |
NaotoMorita | 103:fec71c2051c5 | 57 | eskf.setPhatMagField(0.00001f); |
NaotoMorita | 93:b827f78a717a | 58 | |
NaotoMorita | 93:b827f78a717a | 59 | eskf.setQVelocity(0.0025f); |
NaotoMorita | 93:b827f78a717a | 60 | eskf.setQAngleError(0.00025f); |
NaotoMorita | 94:579e875a4244 | 61 | eskf.setQAccBias(0.0000001f); |
NaotoMorita | 93:b827f78a717a | 62 | eskf.setQGyroBias(0.000001f); |
NaotoMorita | 103:fec71c2051c5 | 63 | eskf.setQMagField(0.001f); |
NaotoMorita | 93:b827f78a717a | 64 | |
NaotoMorita | 106:2d854e92cebb | 65 | Matrix Rgps(6,6); |
NaotoMorita | 100:7589b663d462 | 66 | setDiag(Rgps,2.0f); |
NaotoMorita | 106:2d854e92cebb | 67 | Rgps(6,6) = 100.0f; |
NaotoMorita | 106:2d854e92cebb | 68 | |
NaotoMorita | 106:2d854e92cebb | 69 | Matrix Rgpspos(3,3); |
NaotoMorita | 106:2d854e92cebb | 70 | setDiag(Rgpspos,2.0f); |
NaotoMorita | 106:2d854e92cebb | 71 | |
NaotoMorita | 106:2d854e92cebb | 72 | Matrix Rgpsvel(3,3); |
NaotoMorita | 106:2d854e92cebb | 73 | setDiag(Rgpsvel,2.0f); |
NaotoMorita | 106:2d854e92cebb | 74 | //Rgpsvel(3,3) = 10.0f; |
NaotoMorita | 99:98b892ca70ec | 75 | |
NaotoMorita | 103:fec71c2051c5 | 76 | Matrix RImuConst(5,5); |
NaotoMorita | 106:2d854e92cebb | 77 | setDiag(RImuConst,100.0f); |
NaotoMorita | 106:2d854e92cebb | 78 | RImuConst(4,4) = 10.0f; |
NaotoMorita | 104:20b8caa29185 | 79 | RImuConst(5,5) = 1.0f; |
NaotoMorita | 106:2d854e92cebb | 80 | |
NaotoMorita | 106:2d854e92cebb | 81 | Matrix Rmag(3,3); |
NaotoMorita | 106:2d854e92cebb | 82 | Rmag(1,1) = 10.0f; |
NaotoMorita | 106:2d854e92cebb | 83 | Rmag(2,2) = 1.0f; |
NaotoMorita | 106:2d854e92cebb | 84 | Rmag(3,3) = 2.0f; |
NaotoMorita | 106:2d854e92cebb | 85 | _t.start(); |
NaotoMorita | 93:b827f78a717a | 86 | float tgps = _t.read(); |
NaotoMorita | 77:2bf856e3eca4 | 87 | while(1) |
NaotoMorita | 77:2bf856e3eca4 | 88 | { |
NaotoMorita | 68:b9f6938fab9d | 89 | float tstart = _t.read(); |
osaka | 87:89bbbcdb667b | 90 | //センサの値を取得 |
NaotoMorita | 73:84ffa0166e6c | 91 | if(hilFlag == true){ |
NaotoMorita | 92:00460f6df439 | 92 | getHilIMUval(); |
NaotoMorita | 93:b827f78a717a | 93 | }else{ |
NaotoMorita | 92:00460f6df439 | 94 | getIMUval(); |
NaotoMorita | 73:84ffa0166e6c | 95 | } |
osaka | 87:89bbbcdb667b | 96 | //ekfの更新 |
NaotoMorita | 90:96c2b0ed4b96 | 97 | eskf.updateNominal(MatrixMath::Vector2mat(acc),MatrixMath::Vector2mat(gyro),att_dt); |
NaotoMorita | 90:96c2b0ed4b96 | 98 | eskf.updateErrState(MatrixMath::Vector2mat(acc),MatrixMath::Vector2mat(gyro),att_dt); |
NaotoMorita | 103:fec71c2051c5 | 99 | |
NaotoMorita | 100:7589b663d462 | 100 | if(hilFlag == true){ |
NaotoMorita | 100:7589b663d462 | 101 | if(tstart-tgps>0.2f){ |
NaotoMorita | 92:00460f6df439 | 102 | getHilGPSval(); |
NaotoMorita | 100:7589b663d462 | 103 | tgps = _t.read(); |
NaotoMorita | 100:7589b663d462 | 104 | gpsUpdateFlag = true; |
NaotoMorita | 92:00460f6df439 | 105 | }else{ |
NaotoMorita | 100:7589b663d462 | 106 | gpsUpdateFlag = false; |
NaotoMorita | 92:00460f6df439 | 107 | } |
NaotoMorita | 92:00460f6df439 | 108 | }else{ |
NaotoMorita | 103:fec71c2051c5 | 109 | getGPSval(); |
NaotoMorita | 92:00460f6df439 | 110 | } |
NaotoMorita | 100:7589b663d462 | 111 | if(gpsUpdateFlag == true){ |
NaotoMorita | 106:2d854e92cebb | 112 | eskf.updateGPS(MatrixMath::Vector2mat(pi),palt,MatrixMath::Vector2mat(vi),MatrixMath::Vector2mat(mag),Rgps); |
NaotoMorita | 106:2d854e92cebb | 113 | //eskf.updateGPSPosition(MatrixMath::Vector2mat(pi),palt,Rgpspos); |
NaotoMorita | 106:2d854e92cebb | 114 | //eskf.updateGPSVelocity(MatrixMath::Vector2mat(vi),MatrixMath::Vector2mat(mag),Rgpsvel); |
NaotoMorita | 103:fec71c2051c5 | 115 | |
NaotoMorita | 104:20b8caa29185 | 116 | }else{ |
NaotoMorita | 106:2d854e92cebb | 117 | //eskf.updateImuConstraints(MatrixMath::Vector2mat(acc),MatrixMath::Vector2mat(mag),RImuConst); |
NaotoMorita | 106:2d854e92cebb | 118 | //eskf.updateMag(MatrixMath::Vector2mat(mag),palt,Rmag); |
NaotoMorita | 100:7589b663d462 | 119 | } |
NaotoMorita | 77:2bf856e3eca4 | 120 | PIDtick.loop(); |
NaotoMorita | 77:2bf856e3eca4 | 121 | |
osaka | 87:89bbbcdb667b | 122 | //制御時間を計測 |
NaotoMorita | 68:b9f6938fab9d | 123 | float tend = _t.read(); |
NaotoMorita | 68:b9f6938fab9d | 124 | att_dt = (tend-tstart); |
cocorlow | 56:888379912f81 | 125 | } |
osaka | 87:89bbbcdb667b | 126 | } |