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@74:f67062e7813e, 2021-07-20 (annotated)

Committer:
NaotoMorita
Date:
Tue Jul 20 05:52:29 2021 +0000
Revision:
74:f67062e7813e
Parent:
73:84ffa0166e6c
Child:
75:a505b9896da1
errStateEKF bug fix

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 pc.serial.printf("\r\nrun Mode\r\n");
NaotoMorita 68:b9f6938fab9d 6 wait(0.5);
NaotoMorita 68:b9f6938fab9d 7 Timer _t;
NaotoMorita 68:b9f6938fab9d 8 _t.start();
NaotoMorita 68:b9f6938fab9d 9 magCalibrator.setExtremes(magbiasMin,magbiasMax);
NaotoMorita 68:b9f6938fab9d 10 ekf.defineQhat(rpy_align);
NaotoMorita 66:e5afad70fdd8 11 float sum2accnorm = 0;
NaotoMorita 66:e5afad70fdd8 12 float sumaccnorm = 0;
cocorlow 56:888379912f81 13 for(int i = 0; i < 1000; i++){
NaotoMorita 68:b9f6938fab9d 14 float tstart = _t.read();
NaotoMorita 73:84ffa0166e6c 15 if(hilFlag == true){
NaotoMorita 73:84ffa0166e6c 16 getHILval();
NaotoMorita 73:84ffa0166e6c 17 }else{
NaotoMorita 73:84ffa0166e6c 18 getIMUval();
NaotoMorita 73:84ffa0166e6c 19 }
NaotoMorita 73:84ffa0166e6c 20 //ekf.updateQhat(gyro, att_dt);
NaotoMorita 73:84ffa0166e6c 21 //ekf.updateErrState(gyro, att_dt);
NaotoMorita 68:b9f6938fab9d 22 ekf.updateStaticAccMeasures(acc,accref);
NaotoMorita 73:84ffa0166e6c 23 //ekf.fuseErr2Qhat();
NaotoMorita 73:84ffa0166e6c 24 //ekf.resetBias();
NaotoMorita 74:f67062e7813e 25 ekf.updateMagMeasures(mag);
NaotoMorita 68:b9f6938fab9d 26 ekf.computeAngles(rpy, rpy_align);
NaotoMorita 66:e5afad70fdd8 27 sumaccnorm += acc.Norm();
NaotoMorita 66:e5afad70fdd8 28 sum2accnorm += acc.Norm()*acc.Norm();
NaotoMorita 68:b9f6938fab9d 29 float tend = _t.read();
NaotoMorita 68:b9f6938fab9d 30 att_dt = (tend-tstart);
cocorlow 56:888379912f81 31 }
NaotoMorita 68:b9f6938fab9d 32 accref.z = sumaccnorm / 1000.0f;
NaotoMorita 66:e5afad70fdd8 33 float sigma_accnorm = sqrt(sum2accnorm/1000.0f-accref.z*accref.z);
NaotoMorita 68:b9f6938fab9d 34 pc.serial.printf("%f \r\n",sigma_accnorm);
NaotoMorita 73:84ffa0166e6c 35 pc.Subscribe(0000, &(vp));
cocorlow 56:888379912f81 36
cocorlow 56:888379912f81 37 LoopTicker PIDtick;
cocorlow 56:888379912f81 38 PIDtick.attach(calcServoOut,PID_dt);
cocorlow 56:888379912f81 39
NaotoMorita 68:b9f6938fab9d 40
cocorlow 56:888379912f81 41
cocorlow 56:888379912f81 42 while(1)
cocorlow 56:888379912f81 43 {
cocorlow 56:888379912f81 44 float tstart = _t.read();
cocorlow 56:888379912f81 45 //姿勢角を更新
NaotoMorita 73:84ffa0166e6c 46 if(hilFlag == true){
NaotoMorita 73:84ffa0166e6c 47 getHILval();
NaotoMorita 73:84ffa0166e6c 48 }else{
NaotoMorita 73:84ffa0166e6c 49 getIMUval();
NaotoMorita 73:84ffa0166e6c 50 }
NaotoMorita 74:f67062e7813e 51
NaotoMorita 68:b9f6938fab9d 52 ekf.updateQhat(gyro, att_dt);
NaotoMorita 68:b9f6938fab9d 53 ekf.updateErrState(gyro, att_dt);
NaotoMorita 74:f67062e7813e 54 //ekf.updateAllMeasures(acc,gyro,mag,accref,tstart,att_dt);
NaotoMorita 68:b9f6938fab9d 55 if(ekf.determinDynStatus(acc,accref)){
NaotoMorita 68:b9f6938fab9d 56 ekf.updateAccMeasures(acc,accref);
NaotoMorita 68:b9f6938fab9d 57 }else{
NaotoMorita 68:b9f6938fab9d 58 ekf.updateStaticAccMeasures(acc,accref);
NaotoMorita 68:b9f6938fab9d 59 }
NaotoMorita 74:f67062e7813e 60 ekf.updateGyroBiasMeasures(gyro,mag,tstart,att_dt);
NaotoMorita 68:b9f6938fab9d 61 ekf.fuseErr2Qhat();
NaotoMorita 68:b9f6938fab9d 62 ekf.resetBias();
NaotoMorita 73:84ffa0166e6c 63 //ekf.updateMagMeasures(mag);
NaotoMorita 68:b9f6938fab9d 64 ekf.computeAngles(rpy, rpy_align);
cocorlow 56:888379912f81 65 PIDtick.loop();
cocorlow 56:888379912f81 66
cocorlow 56:888379912f81 67 float tend = _t.read();
cocorlow 56:888379912f81 68 att_dt = (tend-tstart);
cocorlow 56:888379912f81 69 }
cocorlow 56:888379912f81 70 }