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@82:c183c29d2427, 2021-09-16 (annotated)

Committer:
NaotoMorita
Date:
Thu Sep 16 09:30:21 2021 +0000
Revision:
82:c183c29d2427
Parent:
77:2bf856e3eca4
Child:
83:e69ab831031c
velocity fuse

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 77:2bf856e3eca4 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 77:2bf856e3eca4 10
NaotoMorita 66:e5afad70fdd8 11 float sum2accnorm = 0;
NaotoMorita 66:e5afad70fdd8 12 float sumaccnorm = 0;
NaotoMorita 77:2bf856e3eca4 13 int n_init = 1000;
NaotoMorita 77:2bf856e3eca4 14 if(hilFlag == false){
NaotoMorita 77:2bf856e3eca4 15 ekf.defineQhat(rpy_align);
NaotoMorita 77:2bf856e3eca4 16 for(int i = 0; i < n_init; i++){
NaotoMorita 77:2bf856e3eca4 17 float tstart = _t.read();
NaotoMorita 77:2bf856e3eca4 18 if(hilFlag == true){
NaotoMorita 77:2bf856e3eca4 19 getHILval();
NaotoMorita 77:2bf856e3eca4 20 }else{
NaotoMorita 77:2bf856e3eca4 21 getIMUval();
NaotoMorita 77:2bf856e3eca4 22 }
NaotoMorita 77:2bf856e3eca4 23 ekf.updateStaticAccMeasures(acc,accref);
NaotoMorita 77:2bf856e3eca4 24 ekf.updateGyroBiasConstraints(gyro);
NaotoMorita 77:2bf856e3eca4 25 ekf.fuseErr2Nominal();
NaotoMorita 77:2bf856e3eca4 26 ekf.resetBias();
NaotoMorita 77:2bf856e3eca4 27 //ekf.updateMagMeasures(mag);
NaotoMorita 77:2bf856e3eca4 28 ekf.computeAngles(rpy, rpy_align);
NaotoMorita 77:2bf856e3eca4 29 sumaccnorm += acc.Norm();
NaotoMorita 77:2bf856e3eca4 30 sum2accnorm += acc.Norm()*acc.Norm();
NaotoMorita 77:2bf856e3eca4 31 float tend = _t.read();
NaotoMorita 77:2bf856e3eca4 32 att_dt = (tend-tstart);
NaotoMorita 77:2bf856e3eca4 33 }
NaotoMorita 77:2bf856e3eca4 34 accref.z = sumaccnorm / float(n_init);
NaotoMorita 77:2bf856e3eca4 35 }
NaotoMorita 77:2bf856e3eca4 36 pc.Subscribe(0000, &(vp));
NaotoMorita 77:2bf856e3eca4 37
NaotoMorita 77:2bf856e3eca4 38 LoopTicker PIDtick;
NaotoMorita 77:2bf856e3eca4 39 PIDtick.attach(calcServoOut,PID_dt);
NaotoMorita 77:2bf856e3eca4 40
NaotoMorita 77:2bf856e3eca4 41
NaotoMorita 77:2bf856e3eca4 42
NaotoMorita 77:2bf856e3eca4 43 while(1)
NaotoMorita 77:2bf856e3eca4 44 {
NaotoMorita 68:b9f6938fab9d 45 float tstart = _t.read();
NaotoMorita 77:2bf856e3eca4 46 //姿勢角を更新
NaotoMorita 73:84ffa0166e6c 47 if(hilFlag == true){
NaotoMorita 73:84ffa0166e6c 48 getHILval();
NaotoMorita 73:84ffa0166e6c 49 }else{
NaotoMorita 73:84ffa0166e6c 50 getIMUval();
NaotoMorita 73:84ffa0166e6c 51 }
NaotoMorita 77:2bf856e3eca4 52
NaotoMorita 77:2bf856e3eca4 53 ekf.updateNominal(gyro,acc,accref,att_dt);
NaotoMorita 77:2bf856e3eca4 54 ekf.updateErrState(gyro,acc, att_dt);
NaotoMorita 77:2bf856e3eca4 55 if(obsCount == 50){
NaotoMorita 77:2bf856e3eca4 56 if(ekf.determinDynStatus(acc,accref)){
NaotoMorita 77:2bf856e3eca4 57 ekf.updateMeasures(gyro,acc,accref);
NaotoMorita 77:2bf856e3eca4 58 }else{
NaotoMorita 77:2bf856e3eca4 59 ekf.updateStaticMeasures(gyro,acc,accref);
NaotoMorita 77:2bf856e3eca4 60 }
NaotoMorita 77:2bf856e3eca4 61 obsCount = 0;
NaotoMorita 77:2bf856e3eca4 62 }else{
NaotoMorita 77:2bf856e3eca4 63 if(ekf.determinDynStatus(acc,accref)){
NaotoMorita 77:2bf856e3eca4 64 ekf.updateAccMeasures(acc,accref);
NaotoMorita 77:2bf856e3eca4 65 }else{
NaotoMorita 77:2bf856e3eca4 66 ekf.updateStaticAccMeasures(acc,accref);
NaotoMorita 77:2bf856e3eca4 67 }
NaotoMorita 77:2bf856e3eca4 68 obsCount += 1;
NaotoMorita 77:2bf856e3eca4 69 }
NaotoMorita 77:2bf856e3eca4 70
NaotoMorita 77:2bf856e3eca4 71 ekf.updateGyroBiasConstraints(gyro);
NaotoMorita 82:c183c29d2427 72 ekf.updateVelocity(vi.y,vi.x,vi.z);
NaotoMorita 77:2bf856e3eca4 73 //ekf.updateVelocityConstraints();
NaotoMorita 76:7fd3ac1afe3e 74 //ekf.updateMagMeasures(mag);
NaotoMorita 77:2bf856e3eca4 75 ekf.fuseErr2Nominal();
NaotoMorita 77:2bf856e3eca4 76 ekf.resetBias();
NaotoMorita 68:b9f6938fab9d 77 ekf.computeAngles(rpy, rpy_align);
NaotoMorita 77:2bf856e3eca4 78 ekf.computeVb(vb);
NaotoMorita 77:2bf856e3eca4 79 PIDtick.loop();
NaotoMorita 77:2bf856e3eca4 80
NaotoMorita 68:b9f6938fab9d 81 float tend = _t.read();
NaotoMorita 68:b9f6938fab9d 82 att_dt = (tend-tstart);
cocorlow 56:888379912f81 83 }
NaotoMorita 77:2bf856e3eca4 84 }
NaotoMorita 77:2bf856e3eca4 85
NaotoMorita 77:2bf856e3eca4 86 /*
NaotoMorita 76:7fd3ac1afe3e 87 if(serialParamSource){
NaotoMorita 76:7fd3ac1afe3e 88 while(1){
NaotoMorita 76:7fd3ac1afe3e 89 pc.serial.attach(NULL, Serial::RxIrq);
NaotoMorita 76:7fd3ac1afe3e 90 pc.serial.printf("%d %d %d %d %d \r\n",checkParamSerial[0],checkParamSerial[1],checkParamSerial[2],checkParamSerial[3],checkParamSerial[4]);
NaotoMorita 76:7fd3ac1afe3e 91 pc.serial.attach(&pc, &UsaPack::Receive, Serial::RxIrq);
NaotoMorita 76:7fd3ac1afe3e 92 switch(vp.commandIndex){
NaotoMorita 76:7fd3ac1afe3e 93 case 1:
NaotoMorita 76:7fd3ac1afe3e 94 NVIC_SystemReset();
NaotoMorita 76:7fd3ac1afe3e 95 break;
NaotoMorita 76:7fd3ac1afe3e 96 case 10:
NaotoMorita 76:7fd3ac1afe3e 97 ekf.setQqerr(float(vp.commandVal));
NaotoMorita 76:7fd3ac1afe3e 98 checkParamSerial[0] = 1;
NaotoMorita 76:7fd3ac1afe3e 99 break;
NaotoMorita 76:7fd3ac1afe3e 100 case 11:
NaotoMorita 76:7fd3ac1afe3e 101 ekf.setQgbias(float(vp.commandVal));
NaotoMorita 76:7fd3ac1afe3e 102 checkParamSerial[1] = 1;
NaotoMorita 76:7fd3ac1afe3e 103 break;
NaotoMorita 76:7fd3ac1afe3e 104 case 12:
NaotoMorita 76:7fd3ac1afe3e 105 Qabdyn=float(vp.commandVal);
NaotoMorita 76:7fd3ac1afe3e 106 checkParamSerial[2] = 1;
NaotoMorita 76:7fd3ac1afe3e 107 break;
NaotoMorita 76:7fd3ac1afe3e 108 case 13:
NaotoMorita 76:7fd3ac1afe3e 109 Rscdyn=float(vp.commandVal);
NaotoMorita 76:7fd3ac1afe3e 110 checkParamSerial[3] = 1;
NaotoMorita 76:7fd3ac1afe3e 111 break;
NaotoMorita 76:7fd3ac1afe3e 112 case 14:
NaotoMorita 76:7fd3ac1afe3e 113
NaotoMorita 76:7fd3ac1afe3e 114 checkParamSerial[4] = 1;
NaotoMorita 76:7fd3ac1afe3e 115 break;
NaotoMorita 76:7fd3ac1afe3e 116 default :
NaotoMorita 76:7fd3ac1afe3e 117 break;
NaotoMorita 76:7fd3ac1afe3e 118 }
NaotoMorita 76:7fd3ac1afe3e 119 int cpsSum = 0;
NaotoMorita 76:7fd3ac1afe3e 120 for(int i = 0;i<5;i++){
NaotoMorita 76:7fd3ac1afe3e 121 cpsSum += checkParamSerial[i];
NaotoMorita 76:7fd3ac1afe3e 122 }
NaotoMorita 76:7fd3ac1afe3e 123 if(cpsSum == 5){
NaotoMorita 76:7fd3ac1afe3e 124 break;
NaotoMorita 76:7fd3ac1afe3e 125 }
NaotoMorita 76:7fd3ac1afe3e 126 wait(0.01);
NaotoMorita 76:7fd3ac1afe3e 127 }
NaotoMorita 76:7fd3ac1afe3e 128 }
NaotoMorita 77:2bf856e3eca4 129 */