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?
User | Revision | Line number | New 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 | */ |