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

Committer:
NaotoMorita
Date:
Tue Nov 30 12:04:58 2021 +0000
Revision:
135:49f8916588da
Parent:
134:d57c6b2a706b
Child:
139:b378528c05f2
preflight check

Who changed what in which revision?

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