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?
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(); |
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 | */ |