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@68:b9f6938fab9d, 2021-06-28 (annotated)

Committer:
NaotoMorita
Date:
Mon Jun 28 01:40:26 2021 +0000
Revision:
68:b9f6938fab9d
Parent:
66:e5afad70fdd8
Child:
70:99f974d8960e
commit

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