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@92:00460f6df439, 2021-10-28 (annotated)

Committer:
NaotoMorita
Date:
Thu Oct 28 09:44:47 2021 +0000
Revision:
92:00460f6df439
Parent:
90:96c2b0ed4b96
Child:
93:b827f78a717a
for PmodNAV

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 }
osaka 87:89bbbcdb667b 11
osaka 87:89bbbcdb667b 12 //センサ正常性チェック
osaka 87:89bbbcdb667b 13 //usaPack通信開始
NaotoMorita 77:2bf856e3eca4 14 pc.Subscribe(0000, &(vp));
NaotoMorita 77:2bf856e3eca4 15
osaka 87:89bbbcdb667b 16 //制御ループのアタッチ
NaotoMorita 77:2bf856e3eca4 17 LoopTicker PIDtick;
NaotoMorita 77:2bf856e3eca4 18 PIDtick.attach(calcServoOut,PID_dt);
NaotoMorita 77:2bf856e3eca4 19
NaotoMorita 92:00460f6df439 20 Timer _t;
NaotoMorita 92:00460f6df439 21 _t.start();
NaotoMorita 92:00460f6df439 22
NaotoMorita 92:00460f6df439 23 float tgps = _t.read();
NaotoMorita 92:00460f6df439 24 Matrix Rgpspos(3,3);
NaotoMorita 92:00460f6df439 25 Rgpspos(1,1) = 5000.0f;
NaotoMorita 92:00460f6df439 26 Rgpspos(2,2) = 5000.0f;
NaotoMorita 92:00460f6df439 27 Rgpspos(3,3) = 5000.0f;
NaotoMorita 92:00460f6df439 28 Matrix Rgpsvel(3,3);
NaotoMorita 92:00460f6df439 29 Rgpsvel(1,1) = 5000.0f;
NaotoMorita 92:00460f6df439 30 Rgpsvel(2,2) = 5000.0f;
NaotoMorita 92:00460f6df439 31 Rgpsvel(3,3) = 5000.0f;
NaotoMorita 92:00460f6df439 32 Matrix RaccConst(3,3);
NaotoMorita 92:00460f6df439 33 RaccConst(1,1) = 980000.0f;
NaotoMorita 92:00460f6df439 34 RaccConst(2,2) = 980000.0f;
NaotoMorita 92:00460f6df439 35 RaccConst(3,3) = 980000.0f;
NaotoMorita 92:00460f6df439 36 Matrix RgyroConst(2,2);
NaotoMorita 92:00460f6df439 37 RgyroConst(1,1) = 100000.0f;
NaotoMorita 92:00460f6df439 38 RgyroConst(2,2) = 100000.0f;
NaotoMorita 77:2bf856e3eca4 39 while(1)
NaotoMorita 77:2bf856e3eca4 40 {
NaotoMorita 68:b9f6938fab9d 41 float tstart = _t.read();
osaka 87:89bbbcdb667b 42 //センサの値を取得
NaotoMorita 73:84ffa0166e6c 43 if(hilFlag == true){
NaotoMorita 92:00460f6df439 44 getHilIMUval();
NaotoMorita 92:00460f6df439 45 getIMUval();
NaotoMorita 73:84ffa0166e6c 46 }else{
osaka 87:89bbbcdb667b 47 //getIMUval();
NaotoMorita 73:84ffa0166e6c 48 }
osaka 87:89bbbcdb667b 49 //ekfの更新
NaotoMorita 90:96c2b0ed4b96 50 eskf.updateNominal(MatrixMath::Vector2mat(acc),MatrixMath::Vector2mat(gyro),att_dt);
NaotoMorita 90:96c2b0ed4b96 51 eskf.updateErrState(MatrixMath::Vector2mat(acc),MatrixMath::Vector2mat(gyro),att_dt);
NaotoMorita 92:00460f6df439 52 if(tstart-tgps>0.2f){
NaotoMorita 92:00460f6df439 53 if(hilFlag == true){
NaotoMorita 92:00460f6df439 54 getHilGPSval();
NaotoMorita 92:00460f6df439 55 }else{
NaotoMorita 92:00460f6df439 56 //getGPSval();
NaotoMorita 92:00460f6df439 57 }
NaotoMorita 92:00460f6df439 58 eskf.updateGPSPosition(MatrixMath::Vector2mat(pi),Rgpspos);
NaotoMorita 92:00460f6df439 59 eskf.updateGPSVelocity(MatrixMath::Vector2mat(vi),Rgpsvel);
NaotoMorita 92:00460f6df439 60 tgps = _t.read();
NaotoMorita 92:00460f6df439 61 }else{
NaotoMorita 92:00460f6df439 62 //eskf.updateAccConstraints(MatrixMath::Vector2mat(acc),RaccConst);
NaotoMorita 92:00460f6df439 63 //eskf.updateGyroConstraints(MatrixMath::Vector2mat(gyro),RgyroConst);
NaotoMorita 92:00460f6df439 64 eskf.updateGPSPosition(MatrixMath::Vector2mat(pi),Rgpspos);
NaotoMorita 92:00460f6df439 65 eskf.updateGPSVelocity(MatrixMath::Vector2mat(vi),Rgpsvel);
NaotoMorita 92:00460f6df439 66 }
NaotoMorita 90:96c2b0ed4b96 67 eskf.fuseErr2Nominal();
osaka 87:89bbbcdb667b 68
NaotoMorita 77:2bf856e3eca4 69 PIDtick.loop();
NaotoMorita 77:2bf856e3eca4 70
osaka 87:89bbbcdb667b 71 //制御時間を計測
NaotoMorita 68:b9f6938fab9d 72 float tend = _t.read();
NaotoMorita 68:b9f6938fab9d 73 att_dt = (tend-tstart);
cocorlow 56:888379912f81 74 }
osaka 87:89bbbcdb667b 75 }