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@87:89bbbcdb667b, 2021-10-20 (annotated)

Committer:
osaka
Date:
Wed Oct 20 01:50:52 2021 +0000
Revision:
87:89bbbcdb667b
Parent:
86:456f00d52974
Child:
88:be349faa1976
Pmod nav

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 //センサの初期化・ジャイロバイアス・加速度スケールの取得
NaotoMorita 66:e5afad70fdd8 10 float sumaccnorm = 0;
NaotoMorita 77:2bf856e3eca4 11 int n_init = 1000;
NaotoMorita 77:2bf856e3eca4 12 if(hilFlag == false){
NaotoMorita 77:2bf856e3eca4 13 ekf.defineQhat(rpy_align);
NaotoMorita 77:2bf856e3eca4 14 for(int i = 0; i < n_init; i++){
NaotoMorita 77:2bf856e3eca4 15 sumaccnorm += acc.Norm();
NaotoMorita 77:2bf856e3eca4 16 }
NaotoMorita 77:2bf856e3eca4 17 accref.z = sumaccnorm / float(n_init);
NaotoMorita 77:2bf856e3eca4 18 }
osaka 87:89bbbcdb667b 19
osaka 87:89bbbcdb667b 20 //センサ正常性チェック
osaka 87:89bbbcdb667b 21
osaka 87:89bbbcdb667b 22 //usaPack通信開始
NaotoMorita 77:2bf856e3eca4 23 pc.Subscribe(0000, &(vp));
NaotoMorita 77:2bf856e3eca4 24
osaka 87:89bbbcdb667b 25 //制御ループのアタッチ
NaotoMorita 77:2bf856e3eca4 26 LoopTicker PIDtick;
NaotoMorita 77:2bf856e3eca4 27 PIDtick.attach(calcServoOut,PID_dt);
NaotoMorita 77:2bf856e3eca4 28
NaotoMorita 77:2bf856e3eca4 29 while(1)
NaotoMorita 77:2bf856e3eca4 30 {
NaotoMorita 68:b9f6938fab9d 31 float tstart = _t.read();
osaka 87:89bbbcdb667b 32 //センサの値を取得
NaotoMorita 73:84ffa0166e6c 33 if(hilFlag == true){
NaotoMorita 73:84ffa0166e6c 34 getHILval();
NaotoMorita 73:84ffa0166e6c 35 }else{
osaka 87:89bbbcdb667b 36 //getIMUval();
NaotoMorita 73:84ffa0166e6c 37 }
NaotoMorita 77:2bf856e3eca4 38
osaka 87:89bbbcdb667b 39 //ekfの更新
NaotoMorita 77:2bf856e3eca4 40 ekf.updateNominal(gyro,acc,accref,att_dt);
NaotoMorita 77:2bf856e3eca4 41 ekf.updateErrState(gyro,acc, att_dt);
NaotoMorita 77:2bf856e3eca4 42 if(obsCount == 50){
osaka 86:456f00d52974 43 //ekf.updateGPSVelocity(vi.x,vi.y,vi.z,acc,accref);
osaka 86:456f00d52974 44 //ekf.updateSinkRate(vi.z,acc,accref);
NaotoMorita 77:2bf856e3eca4 45 obsCount = 0;
NaotoMorita 77:2bf856e3eca4 46 }else{
osaka 86:456f00d52974 47 //ekf.updateGPSVelocity(vi.x,vi.y,vi.z,acc,accref);
osaka 86:456f00d52974 48 //ekf.updateAccMeasures(acc,accref);
osaka 86:456f00d52974 49
NaotoMorita 77:2bf856e3eca4 50 if(ekf.determinDynStatus(acc,accref)){
osaka 86:456f00d52974 51 ekf.updateAccMeasures(acc,accref);
NaotoMorita 77:2bf856e3eca4 52 }else{
osaka 86:456f00d52974 53 ekf.updateStaticAccMeasures(acc,accref);
NaotoMorita 77:2bf856e3eca4 54 }
osaka 86:456f00d52974 55
NaotoMorita 77:2bf856e3eca4 56 obsCount += 1;
NaotoMorita 77:2bf856e3eca4 57 }
NaotoMorita 84:ff48e01ea76b 58 //ekf.updateSinkRate(vi.z,acc,accref);
osaka 86:456f00d52974 59 //ekf.updateGyroBiasConstraints(gyro);
NaotoMorita 83:e69ab831031c 60
NaotoMorita 77:2bf856e3eca4 61 ekf.fuseErr2Nominal();
NaotoMorita 77:2bf856e3eca4 62 ekf.resetBias();
NaotoMorita 68:b9f6938fab9d 63 ekf.computeAngles(rpy, rpy_align);
osaka 87:89bbbcdb667b 64
NaotoMorita 77:2bf856e3eca4 65 PIDtick.loop();
NaotoMorita 77:2bf856e3eca4 66
osaka 87:89bbbcdb667b 67 //制御時間を計測
NaotoMorita 68:b9f6938fab9d 68 float tend = _t.read();
NaotoMorita 68:b9f6938fab9d 69 att_dt = (tend-tstart);
cocorlow 56:888379912f81 70 }
osaka 87:89bbbcdb667b 71 }