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@143:53808e4e684c, 22 months ago (annotated)

Committer:
NaotoMorita
Date:
Fri Jun 24 05:44:34 2022 +0000
Revision:
143:53808e4e684c
Parent:
141:725321fe2949 
complete

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 143:53808e4e684c 5 preflightCalibration();
cocorlow 141:725321fe2949 6
NaotoMorita 143:53808e4e684c 7 setEskfCov();
cocorlow 141:725321fe2949 8
cocorlow 141:725321fe2949 9 MatrixXf Rgps(5,5);
NaotoMorita 143:53808e4e684c 10 setDiag(Rgps,1.5f);
NaotoMorita 143:53808e4e684c 11 Rgps(2,2) = 0.1f;
cocorlow 141:725321fe2949 12 Rgps(3,3) = 0.1f;
cocorlow 141:725321fe2949 13 Rgps(4,4) = 0.1f;
cocorlow 141:725321fe2949 14
NaotoMorita 143:53808e4e684c 15 MatrixXf Rwhole = MatrixXf::Zero(9,9);
NaotoMorita 143:53808e4e684c 16 Rwhole(0,0) = 1.5f;
NaotoMorita 143:53808e4e684c 17 Rwhole(1,1) = 1.5f;
NaotoMorita 143:53808e4e684c 18 Rwhole(2,2) = 0.1f;
NaotoMorita 143:53808e4e684c 19 Rwhole(3,3) = 0.1f;
NaotoMorita 143:53808e4e684c 20 Rwhole(4,4) = 0.1f;
NaotoMorita 143:53808e4e684c 21 Rwhole(5,5) = 5000.0f;
NaotoMorita 143:53808e4e684c 22 Rwhole(6,6) = 5000.0f;
NaotoMorita 143:53808e4e684c 23 Rwhole(7,7) = 5000.0f;
NaotoMorita 143:53808e4e684c 24 Rwhole(8,8) = 0.007f;
NaotoMorita 143:53808e4e684c 25
cocorlow 141:725321fe2949 26
cocorlow 141:725321fe2949 27 _t.start();
NaotoMorita 143:53808e4e684c 28
NaotoMorita 143:53808e4e684c 29 preflightCheck();
NaotoMorita 143:53808e4e684c 30 wait(1.0f);
NaotoMorita 143:53808e4e684c 31 //usaPack通信開始 制御ループのアタッチ
cocorlow 141:725321fe2949 32 pc.Subscribe(0000, &(vp));
cocorlow 141:725321fe2949 33 LoopTicker PIDtick;
cocorlow 141:725321fe2949 34 PIDtick.attach(calcServoOut,PID_dt);
cocorlow 141:725321fe2949 35
cocorlow 141:725321fe2949 36 float tgps = _t.read();
cocorlow 141:725321fe2949 37 float theading = _t.read();
NaotoMorita 143:53808e4e684c 38 float tstart = _t.read();
cocorlow 141:725321fe2949 39 while(1)
cocorlow 141:725321fe2949 40 {
cocorlow 141:725321fe2949 41 float tstart = _t.read();
cocorlow 141:725321fe2949 42 //センサの値を取得
cocorlow 141:725321fe2949 43 if(hilFlag == true){
cocorlow 141:725321fe2949 44 getHilIMUval();
cocorlow 141:725321fe2949 45 }else{
cocorlow 141:725321fe2949 46 getIMUval();
cocorlow 141:725321fe2949 47 }
cocorlow 141:725321fe2949 48 //ekfの更新
cocorlow 141:725321fe2949 49 eskf.updateNominal(acc, gyro, att_dt);
cocorlow 141:725321fe2949 50 eskf.updateErrState(acc, gyro, att_dt);
cocorlow 141:725321fe2949 51
cocorlow 141:725321fe2949 52 if(hilFlag == true){
cocorlow 141:725321fe2949 53 if(tstart-tgps>0.2f){
cocorlow 141:725321fe2949 54 getHilGPSval();
cocorlow 141:725321fe2949 55 tgps = _t.read();
cocorlow 141:725321fe2949 56 gpsUpdateFlag = true;
cocorlow 141:725321fe2949 57 }else{
cocorlow 141:725321fe2949 58 gpsUpdateFlag = false;
cocorlow 141:725321fe2949 59 }
cocorlow 141:725321fe2949 60 }else{
cocorlow 141:725321fe2949 61 if(userButton.read()==1)
cocorlow 141:725321fe2949 62 {
cocorlow 141:725321fe2949 63 gpsLlh0Fixed = false;
cocorlow 141:725321fe2949 64 };
cocorlow 141:725321fe2949 65 getGPSval();
cocorlow 141:725321fe2949 66 }
cocorlow 141:725321fe2949 67
NaotoMorita 143:53808e4e684c 68 if(gpsUpdateFlag == true){
NaotoMorita 143:53808e4e684c 69 float heading = std::atan2(-mag(1),mag(0));
NaotoMorita 143:53808e4e684c 70 Vector3f dynacc = eskf.calcDynAcc(acc);
NaotoMorita 143:53808e4e684c 71 dynaccnorm2 = dynacc.squaredNorm();
NaotoMorita 143:53808e4e684c 72 if(dynaccnorm2 > 0.16f){
NaotoMorita 143:53808e4e684c 73 Rwhole(5,5) = 100.0f;
NaotoMorita 143:53808e4e684c 74 Rwhole(6,6) = 100.0f;
NaotoMorita 143:53808e4e684c 75 Rwhole(7,7) = 100.0f;
NaotoMorita 143:53808e4e684c 76 }else{
NaotoMorita 143:53808e4e684c 77 Rwhole(5,5) = 5000.0f;
NaotoMorita 143:53808e4e684c 78 Rwhole(6,6) = 5000.0f;
NaotoMorita 143:53808e4e684c 79 Rwhole(7,7) = 5000.0f;
cocorlow 141:725321fe2949 80 }
NaotoMorita 143:53808e4e684c 81 eskf.updateWhole(pi, palt, vi,acc,heading, Rwhole);
cocorlow 141:725321fe2949 82 }
cocorlow 141:725321fe2949 83
cocorlow 141:725321fe2949 84 PIDtick.loop();
cocorlow 141:725321fe2949 85
cocorlow 141:725321fe2949 86 //制御時間を計測
cocorlow 141:725321fe2949 87 float tend = _t.read();
cocorlow 141:725321fe2949 88 att_dt = (tend-tstart);
cocorlow 141:725321fe2949 89 }
osaka 87:89bbbcdb667b 90 }