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@141:725321fe2949, 2021-12-10 (annotated)
- Committer:
- cocorlow
- Date:
- Fri Dec 10 10:43:50 2021 +0000
- Revision:
- 141:725321fe2949
- Parent:
- 139:b378528c05f2
- Child:
- 143:53808e4e684c
Eigen completed
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 | { |
cocorlow | 141:725321fe2949 | 5 | wait(0.5); |
cocorlow | 141:725321fe2949 | 6 | |
cocorlow | 141:725321fe2949 | 7 | //センサの初期化・ジャイロバイアス・加速度スケールの取得 |
cocorlow | 141:725321fe2949 | 8 | int n_init = 1000; |
cocorlow | 141:725321fe2949 | 9 | for(int i = 0;i<n_init;i++){ |
cocorlow | 141:725321fe2949 | 10 | lsm.readAccel(); |
cocorlow | 141:725321fe2949 | 11 | lsm.readMag(); |
cocorlow | 141:725321fe2949 | 12 | lsm.readGyro(); |
cocorlow | 141:725321fe2949 | 13 | agoffset[0] += lsm.ax * 9.8f; |
cocorlow | 141:725321fe2949 | 14 | agoffset[1] += lsm.ay * 9.8f; |
cocorlow | 141:725321fe2949 | 15 | agoffset[2] += lsm.az * 9.8f-9.8f; |
cocorlow | 141:725321fe2949 | 16 | agoffset[3] +=(lsm.gx * M_PI_F / 180.0f); |
cocorlow | 141:725321fe2949 | 17 | agoffset[4] +=(lsm.gy * M_PI_F / 180.0f); |
cocorlow | 141:725321fe2949 | 18 | agoffset[5] +=(lsm.gz * M_PI_F / 180.0f); |
cocorlow | 141:725321fe2949 | 19 | palt0 += lps.pressureToAltitudeMeters(lps.readPressureMillibars()); |
cocorlow | 141:725321fe2949 | 20 | magref(0) += lsm.mx; |
cocorlow | 141:725321fe2949 | 21 | magref(1) += lsm.my; |
cocorlow | 141:725321fe2949 | 22 | magref(2) += lsm.mz; |
cocorlow | 141:725321fe2949 | 23 | } |
cocorlow | 141:725321fe2949 | 24 | for(int i = 0;i<6;i++){ |
cocorlow | 141:725321fe2949 | 25 | agoffset[i] /= float(n_init); |
cocorlow | 141:725321fe2949 | 26 | } |
cocorlow | 141:725321fe2949 | 27 | magref /= float(n_init); |
cocorlow | 141:725321fe2949 | 28 | palt0 /= float(n_init); |
cocorlow | 141:725321fe2949 | 29 | twelite.serial.printf("Sensor offset : %f %f %f %f %f %f\r\n",agoffset[0],agoffset[1],agoffset[2],agoffset[3],agoffset[4],agoffset[5]); |
cocorlow | 141:725321fe2949 | 30 | |
cocorlow | 141:725321fe2949 | 31 | //ESKFの共分散設定 |
cocorlow | 141:725321fe2949 | 32 | eskf.setGravity(0.0f,0.0f,9.8f); |
cocorlow | 141:725321fe2949 | 33 | eskf.setPhatPosition(0.1f); |
cocorlow | 141:725321fe2949 | 34 | eskf.setPhatVelocity(0.1f); |
cocorlow | 141:725321fe2949 | 35 | eskf.setPhatAngleError(0.5f); |
cocorlow | 141:725321fe2949 | 36 | eskf.setPhatAccBias(0.001f); |
cocorlow | 141:725321fe2949 | 37 | eskf.setPhatGyroBias(0.001f); |
cocorlow | 141:725321fe2949 | 38 | eskf.setPhatGravity(0.0000001f); |
cocorlow | 141:725321fe2949 | 39 | |
cocorlow | 141:725321fe2949 | 40 | eskf.setQVelocity(0.001f); |
cocorlow | 141:725321fe2949 | 41 | eskf.setQAngleError(0.0000025f); |
cocorlow | 141:725321fe2949 | 42 | eskf.setQAccBias(0.000001f); |
cocorlow | 141:725321fe2949 | 43 | eskf.setQGyroBias(0.000001f); |
cocorlow | 141:725321fe2949 | 44 | |
cocorlow | 141:725321fe2949 | 45 | Matrix3f Rgpspos; |
cocorlow | 141:725321fe2949 | 46 | setDiag(Rgpspos,1.0f); |
cocorlow | 141:725321fe2949 | 47 | |
cocorlow | 141:725321fe2949 | 48 | Matrix3f Rgpsvel; |
cocorlow | 141:725321fe2949 | 49 | Rgpsvel(0,0) = 0.01f; |
cocorlow | 141:725321fe2949 | 50 | Rgpsvel(1,1) = 0.01f; |
cocorlow | 141:725321fe2949 | 51 | Rgpsvel(2,2) = 0.1f; |
cocorlow | 141:725321fe2949 | 52 | |
cocorlow | 141:725321fe2949 | 53 | MatrixXf Rgps(5,5); |
cocorlow | 141:725321fe2949 | 54 | setDiag(Rgps,0.05f); |
cocorlow | 141:725321fe2949 | 55 | Rgps(3,3) = 0.1f; |
cocorlow | 141:725321fe2949 | 56 | Rgps(4,4) = 0.1f; |
cocorlow | 141:725321fe2949 | 57 | |
cocorlow | 141:725321fe2949 | 58 | float dynAccCriteria = 0.4f; |
cocorlow | 141:725321fe2949 | 59 | Matrix3f Racc; |
cocorlow | 141:725321fe2949 | 60 | setDiag(Racc,100.0f); |
cocorlow | 141:725321fe2949 | 61 | Matrix3f RaccDyn; |
cocorlow | 141:725321fe2949 | 62 | setDiag(RaccDyn,5000.0f); |
cocorlow | 141:725321fe2949 | 63 | |
cocorlow | 141:725321fe2949 | 64 | Matrix<float, 1, 1> Rheading; |
cocorlow | 141:725321fe2949 | 65 | Rheading(0, 0) = 0.01f; |
cocorlow | 141:725321fe2949 | 66 | |
cocorlow | 141:725321fe2949 | 67 | _t.start(); |
cocorlow | 141:725321fe2949 | 68 | //センサ正常性チェック |
cocorlow | 141:725321fe2949 | 69 | if(hilFlag == false){ |
cocorlow | 141:725321fe2949 | 70 | while(1){ |
cocorlow | 141:725321fe2949 | 71 | float tstart = _t.read(); |
cocorlow | 141:725321fe2949 | 72 | getIMUval(); |
cocorlow | 141:725321fe2949 | 73 | getGPSval(); |
cocorlow | 141:725321fe2949 | 74 | eskf.updateNominal(acc, gyro, att_dt); |
cocorlow | 141:725321fe2949 | 75 | eskf.updateErrState(acc, gyro, att_dt); |
cocorlow | 141:725321fe2949 | 76 | eskf.updateGPS(pi, palt, vi, Rgps); |
cocorlow | 141:725321fe2949 | 77 | float heading = std::atan2(-mag(2), mag(0)); |
cocorlow | 141:725321fe2949 | 78 | eskf.updateHeading(heading, Rheading); |
cocorlow | 141:725321fe2949 | 79 | Matrix3f Raccpreflight; |
cocorlow | 141:725321fe2949 | 80 | setDiag(Raccpreflight, 5.0f); |
cocorlow | 141:725321fe2949 | 81 | eskf.updateAcc(acc, Raccpreflight); |
cocorlow | 141:725321fe2949 | 82 | |
cocorlow | 141:725321fe2949 | 83 | bool preflightCheck = true; |
cocorlow | 141:725321fe2949 | 84 | Vector3f gyroBias = eskf.getGyroBias(); |
cocorlow | 141:725321fe2949 | 85 | if(fabsf(gyro(0)-gyroBias(0))>2.0f || fabsf(gyro(1)-gyroBias(1))>2.0f || fabsf(gyro(2)-gyroBias(2))>2.0f){ |
cocorlow | 141:725321fe2949 | 86 | preflightCheck = false; |
cocorlow | 141:725321fe2949 | 87 | twelite.serial.printf("PreFlight Check : high gyro value\r\n"); |
cocorlow | 141:725321fe2949 | 88 | } |
cocorlow | 141:725321fe2949 | 89 | Vector3f accBias = eskf.getAccBias(); |
cocorlow | 141:725321fe2949 | 90 | if(fabsf(acc(0)-accBias(0))>2.0f || fabsf(acc(1)-gyroBias(1))>2.0f){ |
cocorlow | 141:725321fe2949 | 91 | preflightCheck = false; |
cocorlow | 141:725321fe2949 | 92 | twelite.serial.printf("PreFlight Check : high acc value\r\n"); |
cocorlow | 141:725321fe2949 | 93 | } |
cocorlow | 141:725321fe2949 | 94 | Vector3f vihat = eskf.getVihat(); |
cocorlow | 141:725321fe2949 | 95 | if(fabsf(vihat(0))>2.0f || fabsf(vihat(1))>2.0f||fabsf(vihat(2))>2.0f){ |
cocorlow | 141:725321fe2949 | 96 | preflightCheck = false; |
cocorlow | 141:725321fe2949 | 97 | twelite.serial.printf("PreFlight Check : high velocity value\r\n"); |
cocorlow | 141:725321fe2949 | 98 | } |
cocorlow | 141:725321fe2949 | 99 | Vector3f pihat = eskf.getPihat(); |
cocorlow | 141:725321fe2949 | 100 | if(fabsf(pihat(0))>2.0f || fabsf(pihat(1))>2.0f||fabsf(pihat(2))>2.0f){ |
cocorlow | 141:725321fe2949 | 101 | preflightCheck = false; |
cocorlow | 141:725321fe2949 | 102 | twelite.serial.printf("PreFlight Check : not home position\r\n"); |
cocorlow | 141:725321fe2949 | 103 | } |
cocorlow | 141:725321fe2949 | 104 | if(sbus.failSafe){ |
cocorlow | 141:725321fe2949 | 105 | preflightCheck = false; |
cocorlow | 141:725321fe2949 | 106 | twelite.serial.printf("PreFlight Check : no RC\r\n"); |
cocorlow | 141:725321fe2949 | 107 | } |
cocorlow | 141:725321fe2949 | 108 | // sbusデータの読み込み |
cocorlow | 141:725321fe2949 | 109 | for (int i =0 ; i < 16;i ++){ |
cocorlow | 141:725321fe2949 | 110 | rc[i] = 0.65f * mapfloat(float(sbus.getData(i)),368,1680,-1,1) + (1.0f - 0.65f) * rc[i]; // mapped input |
cocorlow | 141:725321fe2949 | 111 | } |
cocorlow | 141:725321fe2949 | 112 | if (rc[4]>-0.3f && rc[6] < -0.3f){ |
cocorlow | 141:725321fe2949 | 113 | preflightCheck = false; |
cocorlow | 141:725321fe2949 | 114 | twelite.serial.printf("PreFlight Check : autoPilot enabled\r\n"); |
cocorlow | 141:725321fe2949 | 115 | } |
cocorlow | 141:725321fe2949 | 116 | if(!(gps.gpsFix == 0x02 || gps.gpsFix == 0x03)){ |
cocorlow | 141:725321fe2949 | 117 | preflightCheck = false; |
cocorlow | 141:725321fe2949 | 118 | twelite.serial.printf("PreFlight Check : no gps lock\r\n"); |
cocorlow | 141:725321fe2949 | 119 | } |
cocorlow | 141:725321fe2949 | 120 | |
cocorlow | 141:725321fe2949 | 121 | if(preflightCheck == true){ |
cocorlow | 141:725321fe2949 | 122 | break; |
cocorlow | 141:725321fe2949 | 123 | } |
cocorlow | 141:725321fe2949 | 124 | } |
cocorlow | 141:725321fe2949 | 125 | } |
cocorlow | 141:725321fe2949 | 126 | twelite.serial.printf("PreFlight Check Completed\r\n"); |
cocorlow | 141:725321fe2949 | 127 | //usaPack通信開始 |
cocorlow | 141:725321fe2949 | 128 | pc.Subscribe(0000, &(vp)); |
cocorlow | 141:725321fe2949 | 129 | |
cocorlow | 141:725321fe2949 | 130 | //制御ループのアタッチ |
cocorlow | 141:725321fe2949 | 131 | LoopTicker PIDtick; |
cocorlow | 141:725321fe2949 | 132 | PIDtick.attach(calcServoOut,PID_dt); |
cocorlow | 141:725321fe2949 | 133 | |
cocorlow | 141:725321fe2949 | 134 | float tgps = _t.read(); |
cocorlow | 141:725321fe2949 | 135 | float theading = _t.read(); |
cocorlow | 141:725321fe2949 | 136 | while(1) |
cocorlow | 141:725321fe2949 | 137 | { |
cocorlow | 141:725321fe2949 | 138 | float tstart = _t.read(); |
cocorlow | 141:725321fe2949 | 139 | //センサの値を取得 |
cocorlow | 141:725321fe2949 | 140 | if(hilFlag == true){ |
cocorlow | 141:725321fe2949 | 141 | getHilIMUval(); |
cocorlow | 141:725321fe2949 | 142 | }else{ |
cocorlow | 141:725321fe2949 | 143 | getIMUval(); |
cocorlow | 141:725321fe2949 | 144 | } |
cocorlow | 141:725321fe2949 | 145 | //ekfの更新 |
cocorlow | 141:725321fe2949 | 146 | eskf.updateNominal(acc, gyro, att_dt); |
cocorlow | 141:725321fe2949 | 147 | eskf.updateErrState(acc, gyro, att_dt); |
cocorlow | 141:725321fe2949 | 148 | |
cocorlow | 141:725321fe2949 | 149 | if(hilFlag == true){ |
cocorlow | 141:725321fe2949 | 150 | if(tstart-tgps>0.2f){ |
cocorlow | 141:725321fe2949 | 151 | getHilGPSval(); |
cocorlow | 141:725321fe2949 | 152 | tgps = _t.read(); |
cocorlow | 141:725321fe2949 | 153 | gpsUpdateFlag = true; |
cocorlow | 141:725321fe2949 | 154 | }else{ |
cocorlow | 141:725321fe2949 | 155 | gpsUpdateFlag = false; |
cocorlow | 141:725321fe2949 | 156 | } |
cocorlow | 141:725321fe2949 | 157 | }else{ |
cocorlow | 141:725321fe2949 | 158 | if(userButton.read()==1) |
cocorlow | 141:725321fe2949 | 159 | { |
cocorlow | 141:725321fe2949 | 160 | gpsLlh0Fixed = false; |
cocorlow | 141:725321fe2949 | 161 | }; |
cocorlow | 141:725321fe2949 | 162 | getGPSval(); |
cocorlow | 141:725321fe2949 | 163 | } |
cocorlow | 141:725321fe2949 | 164 | |
cocorlow | 141:725321fe2949 | 165 | headingUpdateFlag = false; |
cocorlow | 141:725321fe2949 | 166 | if(tstart-theading>0.05f){ |
cocorlow | 141:725321fe2949 | 167 | Vector3f euler = eskf.computeAngles(); |
cocorlow | 141:725321fe2949 | 168 | if(fabsf(euler(1))<30.0f*M_PI_F/180.0f){ |
cocorlow | 141:725321fe2949 | 169 | headingUpdateFlag = true; |
cocorlow | 141:725321fe2949 | 170 | theading = _t.read(); |
cocorlow | 141:725321fe2949 | 171 | } |
cocorlow | 141:725321fe2949 | 172 | } |
cocorlow | 141:725321fe2949 | 173 | |
cocorlow | 141:725321fe2949 | 174 | if(gpsUpdateFlag == true){ |
cocorlow | 141:725321fe2949 | 175 | eskf.updateGPS(pi, palt, vi, Rgps); |
cocorlow | 141:725321fe2949 | 176 | //eskf.updateGPSPosition(MatrixMath::Vector2mat(pi),palt,Rgpspos); |
cocorlow | 141:725321fe2949 | 177 | //eskf.updateGPSVelocity(MatrixMath::Vector2mat(vi),Rgpsvel); |
cocorlow | 141:725321fe2949 | 178 | }else if(headingUpdateFlag == true){ |
cocorlow | 141:725321fe2949 | 179 | float heading = std::atan2(-mag(1),mag(0)); |
cocorlow | 141:725321fe2949 | 180 | eskf.updateHeading(heading,Rheading); |
cocorlow | 141:725321fe2949 | 181 | }else{ |
cocorlow | 141:725321fe2949 | 182 | Vector3f dynacc = eskf.calcDynAcc(acc); |
cocorlow | 141:725321fe2949 | 183 | dynaccnorm2 = dynacc.squaredNorm(); |
cocorlow | 141:725321fe2949 | 184 | //twelite.serial.printf("%f\r\n",sqrt(dynaccnorm2)); |
cocorlow | 141:725321fe2949 | 185 | if(dynaccnorm2 > dynAccCriteria*dynAccCriteria){ |
cocorlow | 141:725321fe2949 | 186 | eskf.updateAcc(acc, RaccDyn); |
cocorlow | 141:725321fe2949 | 187 | }else{ |
cocorlow | 141:725321fe2949 | 188 | eskf.updateAcc(acc, Racc); |
cocorlow | 141:725321fe2949 | 189 | } |
cocorlow | 141:725321fe2949 | 190 | } |
cocorlow | 141:725321fe2949 | 191 | PIDtick.loop(); |
cocorlow | 141:725321fe2949 | 192 | |
cocorlow | 141:725321fe2949 | 193 | //制御時間を計測 |
cocorlow | 141:725321fe2949 | 194 | float tend = _t.read(); |
cocorlow | 141:725321fe2949 | 195 | att_dt = (tend-tstart); |
cocorlow | 141:725321fe2949 | 196 | } |
osaka | 87:89bbbcdb667b | 197 | } |