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@144:b3a713b4f1c4, 22 months ago (annotated)

Committer:
NaotoMorita
Date:
Wed Jun 29 07:57:10 2022 +0000
Revision:
144:b3a713b4f1c4
Parent:
143:53808e4e684c 
can fly

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