HAPSRG / Mbed 2 deprecated hexaTest_Eigen

solaESKF_EIGEN

Dependencies:   mbed LPS25HB_I2C LSM9DS1 PIDcontroller LoopTicker GPSUBX_UART_Eigen SBUS_without_mainfile MedianFilter Eigen UsaPack solaESKF_Eigen Vector3 CalibrateMagneto FastPWM

run.cpp@98:bdaa6bbfb1d9, 2021-11-03 (annotated)

Committer:
NaotoMorita
Date:
Wed Nov 03 14:59:26 2021 +0000
Revision:
98:bdaa6bbfb1d9
Parent:
96:3645f8f9bdd3
Child:
99:98b892ca70ec
slightly modified

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 93:b827f78a717a 11 int n_init = 1000;
NaotoMorita 93:b827f78a717a 12 for(int i = 0;i<n_init;i++){
NaotoMorita 93:b827f78a717a 13 lsm.readAccel();
NaotoMorita 93:b827f78a717a 14 lsm.readMag();
NaotoMorita 93:b827f78a717a 15 lsm.readGyro();
NaotoMorita 93:b827f78a717a 16 agoffset[0] += lsm.ax * 9.8f;
NaotoMorita 93:b827f78a717a 17 agoffset[1] += lsm.ay * 9.8f;
NaotoMorita 93:b827f78a717a 18 agoffset[2] += lsm.az * 9.8f-9.8f;
NaotoMorita 93:b827f78a717a 19 agoffset[3] +=(lsm.gx * M_PI / 180.0f);
NaotoMorita 93:b827f78a717a 20 agoffset[4] +=(lsm.gy * M_PI / 180.0f);
NaotoMorita 93:b827f78a717a 21 agoffset[5] +=(lsm.gz * M_PI / 180.0f);
NaotoMorita 98:bdaa6bbfb1d9 22 palt0 = lps.pressureToAltitudeMeters(lps.readPressureMillibars());
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 98:bdaa6bbfb1d9 27 palt0 /= float(n_init);
NaotoMorita 93:b827f78a717a 28 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 29 //センサ正常性チェック
osaka 87:89bbbcdb667b 30 //usaPack通信開始
NaotoMorita 77:2bf856e3eca4 31 pc.Subscribe(0000, &(vp));
NaotoMorita 77:2bf856e3eca4 32
osaka 87:89bbbcdb667b 33 //制御ループのアタッチ
NaotoMorita 77:2bf856e3eca4 34 LoopTicker PIDtick;
NaotoMorita 77:2bf856e3eca4 35 PIDtick.attach(calcServoOut,PID_dt);
NaotoMorita 77:2bf856e3eca4 36
NaotoMorita 93:b827f78a717a 37 //EKFの共分散設定
NaotoMorita 93:b827f78a717a 38 eskf.setGravity(0.0f,0.0f,9.8f);
NaotoMorita 93:b827f78a717a 39 eskf.setPhatPosition(0.1f);
NaotoMorita 93:b827f78a717a 40 eskf.setPhatVelocity(0.1f);
NaotoMorita 93:b827f78a717a 41 eskf.setPhatAngleError(0.1f);
NaotoMorita 94:579e875a4244 42 eskf.setPhatAccBias(0.001f);
NaotoMorita 94:579e875a4244 43 eskf.setPhatGyroBias(0.001f);
NaotoMorita 94:579e875a4244 44 eskf.setPhatGravity(0.0000001f);
NaotoMorita 93:b827f78a717a 45
NaotoMorita 93:b827f78a717a 46 eskf.setQVelocity(0.0025f);
NaotoMorita 93:b827f78a717a 47 eskf.setQAngleError(0.00025f);
NaotoMorita 94:579e875a4244 48 eskf.setQAccBias(0.0000001f);
NaotoMorita 93:b827f78a717a 49 eskf.setQGyroBias(0.000001f);
NaotoMorita 93:b827f78a717a 50
NaotoMorita 92:00460f6df439 51 Matrix Rgpspos(3,3);
NaotoMorita 93:b827f78a717a 52 setDiag(Rgpspos,0.9f);
NaotoMorita 92:00460f6df439 53 Matrix Rgpsvel(3,3);
NaotoMorita 93:b827f78a717a 54 setDiag(Rgpsvel,0.1f);
NaotoMorita 93:b827f78a717a 55
NaotoMorita 94:579e875a4244 56 Matrix RaccConst(4,4);
NaotoMorita 94:579e875a4244 57 setDiag(RaccConst,50.0f);
NaotoMorita 94:579e875a4244 58 RaccConst(4,4) = 1.0f;
NaotoMorita 93:b827f78a717a 59 //Matrix RgyroConst(2,2);
NaotoMorita 93:b827f78a717a 60 //setDiag(RgyroConst,100);
NaotoMorita 94:579e875a4244 61 _t.start();
NaotoMorita 93:b827f78a717a 62 float tgps = _t.read();
NaotoMorita 77:2bf856e3eca4 63 while(1)
NaotoMorita 77:2bf856e3eca4 64 {
NaotoMorita 68:b9f6938fab9d 65 float tstart = _t.read();
osaka 87:89bbbcdb667b 66 //センサの値を取得
NaotoMorita 73:84ffa0166e6c 67 if(hilFlag == true){
NaotoMorita 92:00460f6df439 68 getHilIMUval();
NaotoMorita 93:b827f78a717a 69 }else{
NaotoMorita 92:00460f6df439 70 getIMUval();
NaotoMorita 73:84ffa0166e6c 71 }
osaka 87:89bbbcdb667b 72 //ekfの更新
NaotoMorita 90:96c2b0ed4b96 73 eskf.updateNominal(MatrixMath::Vector2mat(acc),MatrixMath::Vector2mat(gyro),att_dt);
NaotoMorita 90:96c2b0ed4b96 74 eskf.updateErrState(MatrixMath::Vector2mat(acc),MatrixMath::Vector2mat(gyro),att_dt);
NaotoMorita 92:00460f6df439 75 if(tstart-tgps>0.2f){
NaotoMorita 92:00460f6df439 76 if(hilFlag == true){
NaotoMorita 92:00460f6df439 77 getHilGPSval();
NaotoMorita 92:00460f6df439 78 }else{
osaka 96:3645f8f9bdd3 79 getGPSval();
NaotoMorita 92:00460f6df439 80 }
NaotoMorita 92:00460f6df439 81 eskf.updateGPSPosition(MatrixMath::Vector2mat(pi),Rgpspos);
NaotoMorita 92:00460f6df439 82 eskf.updateGPSVelocity(MatrixMath::Vector2mat(vi),Rgpsvel);
NaotoMorita 92:00460f6df439 83 tgps = _t.read();
NaotoMorita 92:00460f6df439 84 }else{
NaotoMorita 94:579e875a4244 85 eskf.updateAccConstraints(MatrixMath::Vector2mat(acc),palt,RaccConst);
NaotoMorita 92:00460f6df439 86 //eskf.updateGyroConstraints(MatrixMath::Vector2mat(gyro),RgyroConst);
NaotoMorita 92:00460f6df439 87 }
NaotoMorita 90:96c2b0ed4b96 88 eskf.fuseErr2Nominal();
osaka 87:89bbbcdb667b 89
NaotoMorita 77:2bf856e3eca4 90 PIDtick.loop();
NaotoMorita 77:2bf856e3eca4 91
osaka 87:89bbbcdb667b 92 //制御時間を計測
NaotoMorita 68:b9f6938fab9d 93 float tend = _t.read();
NaotoMorita 68:b9f6938fab9d 94 att_dt = (tend-tstart);
cocorlow 56:888379912f81 95 }
osaka 87:89bbbcdb667b 96 }