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@90:96c2b0ed4b96, 2021-10-26 (annotated)

Committer:
NaotoMorita
Date:
Tue Oct 26 05:36:18 2021 +0000
Revision:
90:96c2b0ed4b96
Parent:
89:c9f64bd655d9
Child:
92:00460f6df439
solaeskf compiled

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 68:b9f6938fab9d 6 Timer _t;
NaotoMorita 68:b9f6938fab9d 7 _t.start();
NaotoMorita 77:2bf856e3eca4 8
osaka 87:89bbbcdb667b 9 //センサの初期化・ジャイロバイアス・加速度スケールの取得
osaka 88:be349faa1976 10 int n_init = 1;
NaotoMorita 77:2bf856e3eca4 11 if(hilFlag == false){
NaotoMorita 90:96c2b0ed4b96 12 eskf.setQhat(rpy_align.x,rpy_align.y,rpy_align.z);
NaotoMorita 77:2bf856e3eca4 13 }
NaotoMorita 90:96c2b0ed4b96 14 eskf.setGravity(0.0f,0.0f,-9.8f);
osaka 87:89bbbcdb667b 15
osaka 87:89bbbcdb667b 16 //センサ正常性チェック
osaka 87:89bbbcdb667b 17 //usaPack通信開始
NaotoMorita 77:2bf856e3eca4 18 pc.Subscribe(0000, &(vp));
NaotoMorita 77:2bf856e3eca4 19
osaka 87:89bbbcdb667b 20 //制御ループのアタッチ
NaotoMorita 77:2bf856e3eca4 21 LoopTicker PIDtick;
NaotoMorita 77:2bf856e3eca4 22 PIDtick.attach(calcServoOut,PID_dt);
NaotoMorita 77:2bf856e3eca4 23
NaotoMorita 77:2bf856e3eca4 24 while(1)
NaotoMorita 77:2bf856e3eca4 25 {
NaotoMorita 68:b9f6938fab9d 26 float tstart = _t.read();
osaka 87:89bbbcdb667b 27 //センサの値を取得
NaotoMorita 73:84ffa0166e6c 28 if(hilFlag == true){
NaotoMorita 73:84ffa0166e6c 29 getHILval();
NaotoMorita 73:84ffa0166e6c 30 }else{
osaka 87:89bbbcdb667b 31 //getIMUval();
NaotoMorita 73:84ffa0166e6c 32 }
osaka 87:89bbbcdb667b 33 //ekfの更新
NaotoMorita 90:96c2b0ed4b96 34 eskf.updateNominal(MatrixMath::Vector2mat(acc),MatrixMath::Vector2mat(gyro),att_dt);
NaotoMorita 90:96c2b0ed4b96 35 eskf.updateErrState(MatrixMath::Vector2mat(acc),MatrixMath::Vector2mat(gyro),att_dt);
NaotoMorita 90:96c2b0ed4b96 36 eskf.fuseErr2Nominal();
osaka 87:89bbbcdb667b 37
NaotoMorita 77:2bf856e3eca4 38 PIDtick.loop();
NaotoMorita 77:2bf856e3eca4 39
osaka 87:89bbbcdb667b 40 //制御時間を計測
NaotoMorita 68:b9f6938fab9d 41 float tend = _t.read();
NaotoMorita 68:b9f6938fab9d 42 att_dt = (tend-tstart);
cocorlow 56:888379912f81 43 }
osaka 87:89bbbcdb667b 44 }