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