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@84:ff48e01ea76b, 2021-09-29 (annotated)

Committer:
NaotoMorita
Date:
Wed Sep 29 04:50:51 2021 +0000
Revision:
84:ff48e01ea76b
Parent:
83:e69ab831031c
Child:
86:456f00d52974
newer;

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 84:ff48e01ea76b 56 ekf.updateGPSVelocity(vi.x,vi.y,vi.z,acc,accref);
NaotoMorita 77:2bf856e3eca4 57 obsCount = 0;
NaotoMorita 77:2bf856e3eca4 58 }else{
NaotoMorita 77:2bf856e3eca4 59 if(ekf.determinDynStatus(acc,accref)){
NaotoMorita 84:ff48e01ea76b 60 //ekf.updateAccMeasures(acc,accref);
NaotoMorita 77:2bf856e3eca4 61 }else{
NaotoMorita 84:ff48e01ea76b 62 //ekf.updateStaticAccMeasures(acc,accref);
NaotoMorita 77:2bf856e3eca4 63 }
NaotoMorita 77:2bf856e3eca4 64 obsCount += 1;
NaotoMorita 77:2bf856e3eca4 65 }
NaotoMorita 84:ff48e01ea76b 66 //ekf.updateSinkRate(vi.z,acc,accref);
NaotoMorita 77:2bf856e3eca4 67 ekf.updateGyroBiasConstraints(gyro);
NaotoMorita 83:e69ab831031c 68
NaotoMorita 77:2bf856e3eca4 69 ekf.fuseErr2Nominal();
NaotoMorita 77:2bf856e3eca4 70 ekf.resetBias();
NaotoMorita 68:b9f6938fab9d 71 ekf.computeAngles(rpy, rpy_align);
NaotoMorita 84:ff48e01ea76b 72 ekf.computeVb(vb);
NaotoMorita 77:2bf856e3eca4 73 PIDtick.loop();
NaotoMorita 77:2bf856e3eca4 74
NaotoMorita 68:b9f6938fab9d 75 float tend = _t.read();
NaotoMorita 68:b9f6938fab9d 76 att_dt = (tend-tstart);
cocorlow 56:888379912f81 77 }
NaotoMorita 77:2bf856e3eca4 78 }
NaotoMorita 77:2bf856e3eca4 79
NaotoMorita 77:2bf856e3eca4 80 /*
NaotoMorita 76:7fd3ac1afe3e 81 if(serialParamSource){
NaotoMorita 76:7fd3ac1afe3e 82 while(1){
NaotoMorita 76:7fd3ac1afe3e 83 pc.serial.attach(NULL, Serial::RxIrq);
NaotoMorita 76:7fd3ac1afe3e 84 pc.serial.printf("%d %d %d %d %d \r\n",checkParamSerial[0],checkParamSerial[1],checkParamSerial[2],checkParamSerial[3],checkParamSerial[4]);
NaotoMorita 76:7fd3ac1afe3e 85 pc.serial.attach(&pc, &UsaPack::Receive, Serial::RxIrq);
NaotoMorita 76:7fd3ac1afe3e 86 switch(vp.commandIndex){
NaotoMorita 76:7fd3ac1afe3e 87 case 1:
NaotoMorita 76:7fd3ac1afe3e 88 NVIC_SystemReset();
NaotoMorita 76:7fd3ac1afe3e 89 break;
NaotoMorita 76:7fd3ac1afe3e 90 case 10:
NaotoMorita 76:7fd3ac1afe3e 91 ekf.setQqerr(float(vp.commandVal));
NaotoMorita 76:7fd3ac1afe3e 92 checkParamSerial[0] = 1;
NaotoMorita 76:7fd3ac1afe3e 93 break;
NaotoMorita 76:7fd3ac1afe3e 94 case 11:
NaotoMorita 76:7fd3ac1afe3e 95 ekf.setQgbias(float(vp.commandVal));
NaotoMorita 76:7fd3ac1afe3e 96 checkParamSerial[1] = 1;
NaotoMorita 76:7fd3ac1afe3e 97 break;
NaotoMorita 76:7fd3ac1afe3e 98 case 12:
NaotoMorita 76:7fd3ac1afe3e 99 Qabdyn=float(vp.commandVal);
NaotoMorita 76:7fd3ac1afe3e 100 checkParamSerial[2] = 1;
NaotoMorita 76:7fd3ac1afe3e 101 break;
NaotoMorita 76:7fd3ac1afe3e 102 case 13:
NaotoMorita 76:7fd3ac1afe3e 103 Rscdyn=float(vp.commandVal);
NaotoMorita 76:7fd3ac1afe3e 104 checkParamSerial[3] = 1;
NaotoMorita 76:7fd3ac1afe3e 105 break;
NaotoMorita 76:7fd3ac1afe3e 106 case 14:
NaotoMorita 76:7fd3ac1afe3e 107
NaotoMorita 76:7fd3ac1afe3e 108 checkParamSerial[4] = 1;
NaotoMorita 76:7fd3ac1afe3e 109 break;
NaotoMorita 76:7fd3ac1afe3e 110 default :
NaotoMorita 76:7fd3ac1afe3e 111 break;
NaotoMorita 76:7fd3ac1afe3e 112 }
NaotoMorita 76:7fd3ac1afe3e 113 int cpsSum = 0;
NaotoMorita 76:7fd3ac1afe3e 114 for(int i = 0;i<5;i++){
NaotoMorita 76:7fd3ac1afe3e 115 cpsSum += checkParamSerial[i];
NaotoMorita 76:7fd3ac1afe3e 116 }
NaotoMorita 76:7fd3ac1afe3e 117 if(cpsSum == 5){
NaotoMorita 76:7fd3ac1afe3e 118 break;
NaotoMorita 76:7fd3ac1afe3e 119 }
NaotoMorita 76:7fd3ac1afe3e 120 wait(0.01);
NaotoMorita 76:7fd3ac1afe3e 121 }
NaotoMorita 76:7fd3ac1afe3e 122 }
NaotoMorita 77:2bf856e3eca4 123 */