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@88:be349faa1976, 2021-10-21 (annotated)

Committer:
osaka
Date:
Thu Oct 21 06:41:03 2021 +0000
Revision:
88:be349faa1976
Parent:
87:89bbbcdb667b
Child:
89:c9f64bd655d9
newer

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;
osaka 88:be349faa1976 11 int n_init = 1;
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();
osaka 88:be349faa1976 35 //getIMUval();
NaotoMorita 73:84ffa0166e6c 36 }else{
osaka 87:89bbbcdb667b 37 //getIMUval();
NaotoMorita 73:84ffa0166e6c 38 }
NaotoMorita 77:2bf856e3eca4 39
osaka 87:89bbbcdb667b 40 //ekfの更新
NaotoMorita 77:2bf856e3eca4 41 ekf.updateNominal(gyro,acc,accref,att_dt);
NaotoMorita 77:2bf856e3eca4 42 ekf.updateErrState(gyro,acc, att_dt);
NaotoMorita 77:2bf856e3eca4 43 if(obsCount == 50){
osaka 86:456f00d52974 44 //ekf.updateGPSVelocity(vi.x,vi.y,vi.z,acc,accref);
osaka 86:456f00d52974 45 //ekf.updateSinkRate(vi.z,acc,accref);
NaotoMorita 77:2bf856e3eca4 46 obsCount = 0;
NaotoMorita 77:2bf856e3eca4 47 }else{
osaka 86:456f00d52974 48 //ekf.updateGPSVelocity(vi.x,vi.y,vi.z,acc,accref);
osaka 86:456f00d52974 49 //ekf.updateAccMeasures(acc,accref);
osaka 86:456f00d52974 50
NaotoMorita 77:2bf856e3eca4 51 if(ekf.determinDynStatus(acc,accref)){
osaka 86:456f00d52974 52 ekf.updateAccMeasures(acc,accref);
NaotoMorita 77:2bf856e3eca4 53 }else{
osaka 88:be349faa1976 54 ekf.updateAccMeasures(acc,accref);
NaotoMorita 77:2bf856e3eca4 55 }
osaka 86:456f00d52974 56
NaotoMorita 77:2bf856e3eca4 57 obsCount += 1;
NaotoMorita 77:2bf856e3eca4 58 }
NaotoMorita 84:ff48e01ea76b 59 //ekf.updateSinkRate(vi.z,acc,accref);
osaka 86:456f00d52974 60 //ekf.updateGyroBiasConstraints(gyro);
NaotoMorita 83:e69ab831031c 61
NaotoMorita 77:2bf856e3eca4 62 ekf.fuseErr2Nominal();
NaotoMorita 77:2bf856e3eca4 63 ekf.resetBias();
NaotoMorita 68:b9f6938fab9d 64 ekf.computeAngles(rpy, rpy_align);
osaka 87:89bbbcdb667b 65
NaotoMorita 77:2bf856e3eca4 66 PIDtick.loop();
NaotoMorita 77:2bf856e3eca4 67
osaka 87:89bbbcdb667b 68 //制御時間を計測
NaotoMorita 68:b9f6938fab9d 69 float tend = _t.read();
NaotoMorita 68:b9f6938fab9d 70 att_dt = (tend-tstart);
cocorlow 56:888379912f81 71 }
osaka 87:89bbbcdb667b 72 }