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@70:99f974d8960e, 2021-06-29 (annotated)

Committer:
NaotoMorita
Date:
Tue Jun 29 08:07:55 2021 +0000
Revision:
70:99f974d8960e
Parent:
68:b9f6938fab9d
Child:
73:84ffa0166e6c
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);
NaotoMorita 70:99f974d8960e 10 for(int i = 0; i < 1000; i++){
NaotoMorita 70:99f974d8960e 11 getIMUval();
NaotoMorita 70:99f974d8960e 12 }
NaotoMorita 68:b9f6938fab9d 13 //ekf.triad(acc/acc.Norm(), accref/accref.Norm(), mag/mag.Norm(), magref/magref.Norm());
NaotoMorita 68:b9f6938fab9d 14 ekf.defineQhat(rpy_align);
NaotoMorita 66:e5afad70fdd8 15 float sum2accnorm = 0;
NaotoMorita 66:e5afad70fdd8 16 float sumaccnorm = 0;
cocorlow 56:888379912f81 17 for(int i = 0; i < 1000; i++){
NaotoMorita 68:b9f6938fab9d 18 float tstart = _t.read();
cocorlow 56:888379912f81 19 getIMUval();
NaotoMorita 68:b9f6938fab9d 20 ekf.updateQhat(gyro, att_dt);
NaotoMorita 68:b9f6938fab9d 21 ekf.updateErrState(gyro, att_dt);
NaotoMorita 68:b9f6938fab9d 22 ekf.updateStaticAccMeasures(acc,accref);
NaotoMorita 68:b9f6938fab9d 23 ekf.fuseErr2Qhat();
NaotoMorita 68:b9f6938fab9d 24 //ekf.resetGyroBias(gyroBias);
NaotoMorita 68:b9f6938fab9d 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 66:e5afad70fdd8 35 pc.Subscribe(0000, &(posValues));
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 //姿勢角を更新
cocorlow 56:888379912f81 46 getIMUval();
NaotoMorita 68:b9f6938fab9d 47 ekf.updateQhat(gyro, att_dt);
NaotoMorita 68:b9f6938fab9d 48 ekf.updateErrState(gyro, att_dt);
NaotoMorita 68:b9f6938fab9d 49 if(ekf.determinDynStatus(acc,accref)){
NaotoMorita 68:b9f6938fab9d 50 ekf.updateAccMeasures(acc,accref);
NaotoMorita 68:b9f6938fab9d 51 }else{
NaotoMorita 68:b9f6938fab9d 52 ekf.updateStaticAccMeasures(acc,accref);
NaotoMorita 68:b9f6938fab9d 53 }
NaotoMorita 68:b9f6938fab9d 54 ekf.fuseErr2Qhat();
NaotoMorita 68:b9f6938fab9d 55 ekf.resetBias();
NaotoMorita 68:b9f6938fab9d 56 ekf.updateMagMeasures(mag);
NaotoMorita 68:b9f6938fab9d 57 ekf.computeAngles(rpy, rpy_align);
cocorlow 56:888379912f81 58 PIDtick.loop();
cocorlow 56:888379912f81 59
cocorlow 56:888379912f81 60 float tend = _t.read();
cocorlow 56:888379912f81 61 att_dt = (tend-tstart);
cocorlow 56:888379912f81 62 }
cocorlow 56:888379912f81 63 }