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?
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 | 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 | */ |