HAPSRG / Mbed 2 deprecated optWingforHAPS_Eigen

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@118:962f226bdf64, 2021-11-18 (annotated)

Committer:
NaotoMorita
Date:
Thu Nov 18 00:50:47 2021 +0000
Revision:
118:962f226bdf64
Parent:
116:663f5889dd17 
bias mod

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 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 98:bdaa6bbfb1d9 34 palt0 /= float(n_init);
NaotoMorita 93:b827f78a717a 35 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 36 //センサ正常性チェック
osaka 87:89bbbcdb667b 37 //usaPack通信開始
NaotoMorita 77:2bf856e3eca4 38 pc.Subscribe(0000, &(vp));
NaotoMorita 77:2bf856e3eca4 39
osaka 87:89bbbcdb667b 40 //制御ループのアタッチ
NaotoMorita 77:2bf856e3eca4 41 LoopTicker PIDtick;
NaotoMorita 77:2bf856e3eca4 42 PIDtick.attach(calcServoOut,PID_dt);
NaotoMorita 77:2bf856e3eca4 43
NaotoMorita 99:98b892ca70ec 44 //ESKFの共分散設定
NaotoMorita 93:b827f78a717a 45 eskf.setGravity(0.0f,0.0f,9.8f);
NaotoMorita 93:b827f78a717a 46 eskf.setPhatPosition(0.1f);
NaotoMorita 93:b827f78a717a 47 eskf.setPhatVelocity(0.1f);
NaotoMorita 113:3e47d9881529 48 eskf.setPhatAngleError(0.5f);
NaotoMorita 94:579e875a4244 49 eskf.setPhatAccBias(0.001f);
NaotoMorita 94:579e875a4244 50 eskf.setPhatGyroBias(0.001f);
NaotoMorita 94:579e875a4244 51 eskf.setPhatGravity(0.0000001f);
NaotoMorita 118:962f226bdf64 52 eskf.setPhatMagBias(0.001f);
NaotoMorita 118:962f226bdf64 53 eskf.setPhatMagRadius(0.001f);
NaotoMorita 93:b827f78a717a 54
NaotoMorita 93:b827f78a717a 55 eskf.setQVelocity(0.0025f);
NaotoMorita 93:b827f78a717a 56 eskf.setQAngleError(0.00025f);
NaotoMorita 94:579e875a4244 57 eskf.setQAccBias(0.0000001f);
NaotoMorita 93:b827f78a717a 58 eskf.setQGyroBias(0.000001f);
NaotoMorita 115:1199d19cb10f 59 eskf.setQMagBias(0.000001f);
NaotoMorita 115:1199d19cb10f 60 eskf.setQMagRadius(0.000001f);
NaotoMorita 93:b827f78a717a 61
NaotoMorita 111:0fae4fbe2a80 62 Matrix Rgps(5,5);
NaotoMorita 100:7589b663d462 63 setDiag(Rgps,2.0f);
NaotoMorita 111:0fae4fbe2a80 64 //Rgps(6,6) = 100.0f;
NaotoMorita 106:2d854e92cebb 65
NaotoMorita 106:2d854e92cebb 66 Matrix Rgpspos(3,3);
NaotoMorita 106:2d854e92cebb 67 setDiag(Rgpspos,2.0f);
NaotoMorita 106:2d854e92cebb 68
NaotoMorita 106:2d854e92cebb 69 Matrix Rgpsvel(3,3);
NaotoMorita 106:2d854e92cebb 70 setDiag(Rgpsvel,2.0f);
NaotoMorita 106:2d854e92cebb 71 //Rgpsvel(3,3) = 10.0f;
NaotoMorita 99:98b892ca70ec 72
NaotoMorita 103:fec71c2051c5 73 Matrix RImuConst(5,5);
NaotoMorita 111:0fae4fbe2a80 74 setDiag(RImuConst,50.0f);
NaotoMorita 112:6a33ea9f6fed 75 RImuConst(4,4) = 0.01f;
NaotoMorita 112:6a33ea9f6fed 76 RImuConst(5,5) = 0.01f;
NaotoMorita 106:2d854e92cebb 77
NaotoMorita 113:3e47d9881529 78
NaotoMorita 113:3e47d9881529 79 Matrix Racc(3,3);
NaotoMorita 113:3e47d9881529 80 setDiag(Racc,50.0f);
NaotoMorita 113:3e47d9881529 81
NaotoMorita 118:962f226bdf64 82 Matrix Rother(2,2);
NaotoMorita 118:962f226bdf64 83 Rother(1,1) = 1.0f;
NaotoMorita 118:962f226bdf64 84 Rother(2,2) = 0.01f;
NaotoMorita 118:962f226bdf64 85
NaotoMorita 106:2d854e92cebb 86 Matrix Rmag(3,3);
NaotoMorita 118:962f226bdf64 87 Rmag(1,1) = 0.1f;
NaotoMorita 118:962f226bdf64 88 Rmag(2,2) = 0.1f;
NaotoMorita 118:962f226bdf64 89 Rmag(3,3) = 0.1f;
NaotoMorita 116:663f5889dd17 90 //Rmag(4,4) = 0.01f;
NaotoMorita 106:2d854e92cebb 91 _t.start();
NaotoMorita 93:b827f78a717a 92 float tgps = _t.read();
NaotoMorita 77:2bf856e3eca4 93 while(1)
NaotoMorita 77:2bf856e3eca4 94 {
NaotoMorita 68:b9f6938fab9d 95 float tstart = _t.read();
osaka 87:89bbbcdb667b 96 //センサの値を取得
NaotoMorita 73:84ffa0166e6c 97 if(hilFlag == true){
NaotoMorita 92:00460f6df439 98 getHilIMUval();
NaotoMorita 93:b827f78a717a 99 }else{
NaotoMorita 92:00460f6df439 100 getIMUval();
NaotoMorita 73:84ffa0166e6c 101 }
osaka 87:89bbbcdb667b 102 //ekfの更新
NaotoMorita 90:96c2b0ed4b96 103 eskf.updateNominal(MatrixMath::Vector2mat(acc),MatrixMath::Vector2mat(gyro),att_dt);
NaotoMorita 90:96c2b0ed4b96 104 eskf.updateErrState(MatrixMath::Vector2mat(acc),MatrixMath::Vector2mat(gyro),att_dt);
NaotoMorita 103:fec71c2051c5 105
NaotoMorita 100:7589b663d462 106 if(hilFlag == true){
NaotoMorita 100:7589b663d462 107 if(tstart-tgps>0.2f){
NaotoMorita 92:00460f6df439 108 getHilGPSval();
NaotoMorita 100:7589b663d462 109 tgps = _t.read();
NaotoMorita 100:7589b663d462 110 gpsUpdateFlag = true;
NaotoMorita 92:00460f6df439 111 }else{
NaotoMorita 100:7589b663d462 112 gpsUpdateFlag = false;
NaotoMorita 92:00460f6df439 113 }
NaotoMorita 92:00460f6df439 114 }else{
NaotoMorita 103:fec71c2051c5 115 getGPSval();
NaotoMorita 92:00460f6df439 116 }
NaotoMorita 100:7589b663d462 117 if(gpsUpdateFlag == true){
NaotoMorita 111:0fae4fbe2a80 118 eskf.updateGPS(MatrixMath::Vector2mat(pi),palt,MatrixMath::Vector2mat(vi),Rgps);
NaotoMorita 106:2d854e92cebb 119 //eskf.updateGPSPosition(MatrixMath::Vector2mat(pi),palt,Rgpspos);
NaotoMorita 106:2d854e92cebb 120 //eskf.updateGPSVelocity(MatrixMath::Vector2mat(vi),MatrixMath::Vector2mat(mag),Rgpsvel);
NaotoMorita 103:fec71c2051c5 121
NaotoMorita 104:20b8caa29185 122 }else{
NaotoMorita 118:962f226bdf64 123 eskf.updateImuConstraints(MatrixMath::Vector2mat(acc),MatrixMath::Vector2mat(mag),RImuConst);
NaotoMorita 118:962f226bdf64 124 //eskf.updateOtherConstraints(MatrixMath::Vector2mat(mag),palt,Rother);
NaotoMorita 118:962f226bdf64 125 //eskf.updateMag(MatrixMath::Vector2mat(mag),Rmag);
NaotoMorita 118:962f226bdf64 126 //eskf.updateAcc(MatrixMath::Vector2mat(acc),Racc);
NaotoMorita 100:7589b663d462 127 }
NaotoMorita 77:2bf856e3eca4 128 PIDtick.loop();
NaotoMorita 77:2bf856e3eca4 129
osaka 87:89bbbcdb667b 130 //制御時間を計測
NaotoMorita 68:b9f6938fab9d 131 float tend = _t.read();
NaotoMorita 68:b9f6938fab9d 132 att_dt = (tend-tstart);
cocorlow 56:888379912f81 133 }
osaka 87:89bbbcdb667b 134 }