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@127:d73a6233ee4b, 2021-11-22 (annotated)

Committer:
NaotoMorita
Date:
Mon Nov 22 09:51:51 2021 +0000
Revision:
127:d73a6233ee4b
Parent:
126:03da3a8c8870
Child:
129:a76be8380bb2
MAG Fuse OK

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 93:b827f78a717a 31 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 32 //センサ正常性チェック
osaka 87:89bbbcdb667b 33 //usaPack通信開始
NaotoMorita 77:2bf856e3eca4 34 pc.Subscribe(0000, &(vp));
NaotoMorita 126:03da3a8c8870 35
osaka 87:89bbbcdb667b 36 //制御ループのアタッチ
NaotoMorita 77:2bf856e3eca4 37 LoopTicker PIDtick;
NaotoMorita 77:2bf856e3eca4 38 PIDtick.attach(calcServoOut,PID_dt);
NaotoMorita 77:2bf856e3eca4 39
NaotoMorita 99:98b892ca70ec 40 //ESKFの共分散設定
NaotoMorita 93:b827f78a717a 41 eskf.setGravity(0.0f,0.0f,9.8f);
NaotoMorita 93:b827f78a717a 42 eskf.setPhatPosition(0.1f);
NaotoMorita 93:b827f78a717a 43 eskf.setPhatVelocity(0.1f);
NaotoMorita 113:3e47d9881529 44 eskf.setPhatAngleError(0.5f);
NaotoMorita 94:579e875a4244 45 eskf.setPhatAccBias(0.001f);
NaotoMorita 94:579e875a4244 46 eskf.setPhatGyroBias(0.001f);
NaotoMorita 94:579e875a4244 47 eskf.setPhatGravity(0.0000001f);
NaotoMorita 93:b827f78a717a 48
NaotoMorita 127:d73a6233ee4b 49 eskf.setQVelocity(0.000025f);
NaotoMorita 93:b827f78a717a 50 eskf.setQAngleError(0.00025f);
NaotoMorita 94:579e875a4244 51 eskf.setQAccBias(0.0000001f);
NaotoMorita 93:b827f78a717a 52 eskf.setQGyroBias(0.000001f);
NaotoMorita 93:b827f78a717a 53
NaotoMorita 127:d73a6233ee4b 54 Matrix Rgps(6,6);
NaotoMorita 127:d73a6233ee4b 55 setDiag(Rgps,1.0f);
NaotoMorita 127:d73a6233ee4b 56 //Rgps(4,4) = 0.1f;
NaotoMorita 127:d73a6233ee4b 57 //Rgps(5,5) = 0.1f;
NaotoMorita 113:3e47d9881529 58
NaotoMorita 113:3e47d9881529 59 Matrix Racc(3,3);
NaotoMorita 113:3e47d9881529 60 setDiag(Racc,50.0f);
NaotoMorita 127:d73a6233ee4b 61
NaotoMorita 126:03da3a8c8870 62 Matrix Rheading(1,1);
NaotoMorita 127:d73a6233ee4b 63 Rheading(1,1) = 0.01f;
NaotoMorita 126:03da3a8c8870 64 _t.start();
NaotoMorita 93:b827f78a717a 65 float tgps = _t.read();
NaotoMorita 127:d73a6233ee4b 66 float theading = _t.read();
NaotoMorita 77:2bf856e3eca4 67 while(1)
NaotoMorita 77:2bf856e3eca4 68 {
NaotoMorita 68:b9f6938fab9d 69 float tstart = _t.read();
osaka 87:89bbbcdb667b 70 //センサの値を取得
NaotoMorita 73:84ffa0166e6c 71 if(hilFlag == true){
NaotoMorita 92:00460f6df439 72 getHilIMUval();
NaotoMorita 93:b827f78a717a 73 }else{
NaotoMorita 92:00460f6df439 74 getIMUval();
NaotoMorita 73:84ffa0166e6c 75 }
osaka 87:89bbbcdb667b 76 //ekfの更新
NaotoMorita 90:96c2b0ed4b96 77 eskf.updateNominal(MatrixMath::Vector2mat(acc),MatrixMath::Vector2mat(gyro),att_dt);
NaotoMorita 90:96c2b0ed4b96 78 eskf.updateErrState(MatrixMath::Vector2mat(acc),MatrixMath::Vector2mat(gyro),att_dt);
NaotoMorita 103:fec71c2051c5 79
NaotoMorita 100:7589b663d462 80 if(hilFlag == true){
NaotoMorita 100:7589b663d462 81 if(tstart-tgps>0.2f){
NaotoMorita 92:00460f6df439 82 getHilGPSval();
NaotoMorita 100:7589b663d462 83 tgps = _t.read();
NaotoMorita 100:7589b663d462 84 gpsUpdateFlag = true;
NaotoMorita 92:00460f6df439 85 }else{
NaotoMorita 100:7589b663d462 86 gpsUpdateFlag = false;
NaotoMorita 92:00460f6df439 87 }
NaotoMorita 92:00460f6df439 88 }else{
NaotoMorita 103:fec71c2051c5 89 getGPSval();
NaotoMorita 92:00460f6df439 90 }
NaotoMorita 127:d73a6233ee4b 91
NaotoMorita 127:d73a6233ee4b 92 headingUpdateFlag = false;
NaotoMorita 127:d73a6233ee4b 93 if(tstart-theading>0.05f){
NaotoMorita 127:d73a6233ee4b 94 Matrix euler = eskf.computeAngles();
NaotoMorita 127:d73a6233ee4b 95 if(fabsf(euler(2,1))<30.0f*M_PI/180.0f){
NaotoMorita 127:d73a6233ee4b 96 headingUpdateFlag = true;
NaotoMorita 127:d73a6233ee4b 97 theading = _t.read();
NaotoMorita 127:d73a6233ee4b 98 }
NaotoMorita 127:d73a6233ee4b 99 }
NaotoMorita 127:d73a6233ee4b 100
NaotoMorita 100:7589b663d462 101 if(gpsUpdateFlag == true){
NaotoMorita 111:0fae4fbe2a80 102 eskf.updateGPS(MatrixMath::Vector2mat(pi),palt,MatrixMath::Vector2mat(vi),Rgps);
NaotoMorita 127:d73a6233ee4b 103 //eskf.updateGPSPosition(MatrixMath::Vector2mat(pi),palt,Rgps);
NaotoMorita 127:d73a6233ee4b 104 }else if(headingUpdateFlag == true){
NaotoMorita 127:d73a6233ee4b 105 float heading = atan2f(-mag.y,mag.x);
NaotoMorita 127:d73a6233ee4b 106 eskf.updateHeading(heading,Rheading);
NaotoMorita 104:20b8caa29185 107 }else{
NaotoMorita 124:7d6b1b62483b 108 eskf.updateAcc(MatrixMath::Vector2mat(acc),Racc);
NaotoMorita 100:7589b663d462 109 }
NaotoMorita 77:2bf856e3eca4 110 PIDtick.loop();
NaotoMorita 77:2bf856e3eca4 111
osaka 87:89bbbcdb667b 112 //制御時間を計測
NaotoMorita 68:b9f6938fab9d 113 float tend = _t.read();
NaotoMorita 68:b9f6938fab9d 114 att_dt = (tend-tstart);
cocorlow 56:888379912f81 115 }
osaka 87:89bbbcdb667b 116 }