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@76:7fd3ac1afe3e, 2021-08-07 (annotated)

Committer:
NaotoMorita
Date:
Sat Aug 07 06:54:58 2021 +0000
Revision:
76:7fd3ac1afe3e
Parent:
75:a505b9896da1
Child:
77:2bf856e3eca4
async update

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 76:7fd3ac1afe3e 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 76:7fd3ac1afe3e 34 float Rscdyn = 263.980f;
NaotoMorita 76:7fd3ac1afe3e 35 float Qabdyn = 124.810f;
NaotoMorita 68:b9f6938fab9d 36 pc.serial.printf("%f \r\n",sigma_accnorm);
NaotoMorita 73:84ffa0166e6c 37 pc.Subscribe(0000, &(vp));
NaotoMorita 76:7fd3ac1afe3e 38 if(serialParamSource){
NaotoMorita 76:7fd3ac1afe3e 39 while(1){
NaotoMorita 76:7fd3ac1afe3e 40 pc.serial.attach(NULL, Serial::RxIrq);
NaotoMorita 76:7fd3ac1afe3e 41 pc.serial.printf("%d %d %d %d %d \r\n",checkParamSerial[0],checkParamSerial[1],checkParamSerial[2],checkParamSerial[3],checkParamSerial[4]);
NaotoMorita 76:7fd3ac1afe3e 42 pc.serial.attach(&pc, &UsaPack::Receive, Serial::RxIrq);
NaotoMorita 76:7fd3ac1afe3e 43 switch(vp.commandIndex){
NaotoMorita 76:7fd3ac1afe3e 44 case 1:
NaotoMorita 76:7fd3ac1afe3e 45 NVIC_SystemReset();
NaotoMorita 76:7fd3ac1afe3e 46 break;
NaotoMorita 76:7fd3ac1afe3e 47 case 10:
NaotoMorita 76:7fd3ac1afe3e 48 ekf.setQqerr(float(vp.commandVal));
NaotoMorita 76:7fd3ac1afe3e 49 checkParamSerial[0] = 1;
NaotoMorita 76:7fd3ac1afe3e 50 break;
NaotoMorita 76:7fd3ac1afe3e 51 case 11:
NaotoMorita 76:7fd3ac1afe3e 52 ekf.setQgbias(float(vp.commandVal));
NaotoMorita 76:7fd3ac1afe3e 53 checkParamSerial[1] = 1;
NaotoMorita 76:7fd3ac1afe3e 54 break;
NaotoMorita 76:7fd3ac1afe3e 55 case 12:
NaotoMorita 76:7fd3ac1afe3e 56 Qabdyn=float(vp.commandVal);
NaotoMorita 76:7fd3ac1afe3e 57 checkParamSerial[2] = 1;
NaotoMorita 76:7fd3ac1afe3e 58 break;
NaotoMorita 76:7fd3ac1afe3e 59 case 13:
NaotoMorita 76:7fd3ac1afe3e 60 Rscdyn=float(vp.commandVal);
NaotoMorita 76:7fd3ac1afe3e 61 checkParamSerial[3] = 1;
NaotoMorita 76:7fd3ac1afe3e 62 break;
NaotoMorita 76:7fd3ac1afe3e 63 case 14:
NaotoMorita 76:7fd3ac1afe3e 64
NaotoMorita 76:7fd3ac1afe3e 65 checkParamSerial[4] = 1;
NaotoMorita 76:7fd3ac1afe3e 66 break;
NaotoMorita 76:7fd3ac1afe3e 67 default :
NaotoMorita 76:7fd3ac1afe3e 68 break;
NaotoMorita 76:7fd3ac1afe3e 69 }
NaotoMorita 76:7fd3ac1afe3e 70 int cpsSum = 0;
NaotoMorita 76:7fd3ac1afe3e 71 for(int i = 0;i<5;i++){
NaotoMorita 76:7fd3ac1afe3e 72 cpsSum += checkParamSerial[i];
NaotoMorita 76:7fd3ac1afe3e 73 }
NaotoMorita 76:7fd3ac1afe3e 74 if(cpsSum == 5){
NaotoMorita 76:7fd3ac1afe3e 75 break;
NaotoMorita 76:7fd3ac1afe3e 76 }
NaotoMorita 76:7fd3ac1afe3e 77 wait(0.01);
NaotoMorita 76:7fd3ac1afe3e 78 }
NaotoMorita 76:7fd3ac1afe3e 79 }
cocorlow 56:888379912f81 80
cocorlow 56:888379912f81 81 LoopTicker PIDtick;
cocorlow 56:888379912f81 82 PIDtick.attach(calcServoOut,PID_dt);
cocorlow 56:888379912f81 83
NaotoMorita 68:b9f6938fab9d 84
cocorlow 56:888379912f81 85
cocorlow 56:888379912f81 86 while(1)
cocorlow 56:888379912f81 87 {
cocorlow 56:888379912f81 88 float tstart = _t.read();
cocorlow 56:888379912f81 89 //姿勢角を更新
NaotoMorita 73:84ffa0166e6c 90 if(hilFlag == true){
NaotoMorita 73:84ffa0166e6c 91 getHILval();
NaotoMorita 73:84ffa0166e6c 92 }else{
NaotoMorita 73:84ffa0166e6c 93 getIMUval();
NaotoMorita 73:84ffa0166e6c 94 }
NaotoMorita 74:f67062e7813e 95
NaotoMorita 68:b9f6938fab9d 96 ekf.updateQhat(gyro, att_dt);
NaotoMorita 68:b9f6938fab9d 97 ekf.updateErrState(gyro, att_dt);
NaotoMorita 76:7fd3ac1afe3e 98 ekf.updateAccMeasures(acc,accref);
NaotoMorita 76:7fd3ac1afe3e 99 ekf.updateGyroBiasConstraints(gyro);
NaotoMorita 76:7fd3ac1afe3e 100 ekf.updateVelocityConstraints();
NaotoMorita 76:7fd3ac1afe3e 101 /*
NaotoMorita 76:7fd3ac1afe3e 102 if(obsCount == 450){
NaotoMorita 76:7fd3ac1afe3e 103 if(ekf.determinDynStatus(acc,accref)==false){
NaotoMorita 76:7fd3ac1afe3e 104 ekf.setRsoftconst(1.0f,1.0f);
NaotoMorita 76:7fd3ac1afe3e 105 ekf.setQab(0.0f);
NaotoMorita 76:7fd3ac1afe3e 106 }else{
NaotoMorita 76:7fd3ac1afe3e 107 ekf.setRsoftconst(263.0f,500.0f);
NaotoMorita 76:7fd3ac1afe3e 108 ekf.setQab(Qabdyn);
NaotoMorita 76:7fd3ac1afe3e 109 }
NaotoMorita 68:b9f6938fab9d 110 ekf.updateAccMeasures(acc,accref);
NaotoMorita 76:7fd3ac1afe3e 111 obsCount = 0;
NaotoMorita 68:b9f6938fab9d 112 }else{
NaotoMorita 76:7fd3ac1afe3e 113 obsCount += 1;
NaotoMorita 76:7fd3ac1afe3e 114 if(ekf.determinDynStatus(acc,accref)==false){
NaotoMorita 76:7fd3ac1afe3e 115 ekf.updateStaticAccMeasures(acc,accref);
NaotoMorita 76:7fd3ac1afe3e 116 }
NaotoMorita 68:b9f6938fab9d 117 }
NaotoMorita 76:7fd3ac1afe3e 118 */
NaotoMorita 76:7fd3ac1afe3e 119 //ekf.updateMagMeasures(mag);
NaotoMorita 68:b9f6938fab9d 120 ekf.fuseErr2Qhat();
NaotoMorita 76:7fd3ac1afe3e 121 //ekf.resetBias();
NaotoMorita 68:b9f6938fab9d 122 ekf.computeAngles(rpy, rpy_align);
cocorlow 56:888379912f81 123 PIDtick.loop();
cocorlow 56:888379912f81 124
cocorlow 56:888379912f81 125 float tend = _t.read();
cocorlow 56:888379912f81 126 att_dt = (tend-tstart);
cocorlow 56:888379912f81 127 }
cocorlow 56:888379912f81 128 }