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@86:456f00d52974, 2021-10-18 (annotated)

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