solaESKF_EIGEN
Dependencies: mbed LPS25HB_I2C LSM9DS1 PIDcontroller LoopTicker GPSUBX_UART_Eigen SBUS_without_mainfile MedianFilter Eigen UsaPack solaESKF_Eigen Vector3 CalibrateMagneto FastPWM
run.cpp@87:89bbbcdb667b, 2021-10-20 (annotated)
- Committer:
- osaka
- Date:
- Wed Oct 20 01:50:52 2021 +0000
- Revision:
- 87:89bbbcdb667b
- Parent:
- 86:456f00d52974
- Child:
- 88:be349faa1976
Pmod nav
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 | 68:b9f6938fab9d | 5 | wait(0.5); |
NaotoMorita | 68:b9f6938fab9d | 6 | Timer _t; |
NaotoMorita | 68:b9f6938fab9d | 7 | _t.start(); |
NaotoMorita | 77:2bf856e3eca4 | 8 | |
osaka | 87:89bbbcdb667b | 9 | //センサの初期化・ジャイロバイアス・加速度スケールの取得 |
NaotoMorita | 66:e5afad70fdd8 | 10 | float sumaccnorm = 0; |
NaotoMorita | 77:2bf856e3eca4 | 11 | int n_init = 1000; |
NaotoMorita | 77:2bf856e3eca4 | 12 | if(hilFlag == false){ |
NaotoMorita | 77:2bf856e3eca4 | 13 | ekf.defineQhat(rpy_align); |
NaotoMorita | 77:2bf856e3eca4 | 14 | for(int i = 0; i < n_init; i++){ |
NaotoMorita | 77:2bf856e3eca4 | 15 | sumaccnorm += acc.Norm(); |
NaotoMorita | 77:2bf856e3eca4 | 16 | } |
NaotoMorita | 77:2bf856e3eca4 | 17 | accref.z = sumaccnorm / float(n_init); |
NaotoMorita | 77:2bf856e3eca4 | 18 | } |
osaka | 87:89bbbcdb667b | 19 | |
osaka | 87:89bbbcdb667b | 20 | //センサ正常性チェック |
osaka | 87:89bbbcdb667b | 21 | |
osaka | 87:89bbbcdb667b | 22 | //usaPack通信開始 |
NaotoMorita | 77:2bf856e3eca4 | 23 | pc.Subscribe(0000, &(vp)); |
NaotoMorita | 77:2bf856e3eca4 | 24 | |
osaka | 87:89bbbcdb667b | 25 | //制御ループのアタッチ |
NaotoMorita | 77:2bf856e3eca4 | 26 | LoopTicker PIDtick; |
NaotoMorita | 77:2bf856e3eca4 | 27 | PIDtick.attach(calcServoOut,PID_dt); |
NaotoMorita | 77:2bf856e3eca4 | 28 | |
NaotoMorita | 77:2bf856e3eca4 | 29 | while(1) |
NaotoMorita | 77:2bf856e3eca4 | 30 | { |
NaotoMorita | 68:b9f6938fab9d | 31 | float tstart = _t.read(); |
osaka | 87:89bbbcdb667b | 32 | //センサの値を取得 |
NaotoMorita | 73:84ffa0166e6c | 33 | if(hilFlag == true){ |
NaotoMorita | 73:84ffa0166e6c | 34 | getHILval(); |
NaotoMorita | 73:84ffa0166e6c | 35 | }else{ |
osaka | 87:89bbbcdb667b | 36 | //getIMUval(); |
NaotoMorita | 73:84ffa0166e6c | 37 | } |
NaotoMorita | 77:2bf856e3eca4 | 38 | |
osaka | 87:89bbbcdb667b | 39 | //ekfの更新 |
NaotoMorita | 77:2bf856e3eca4 | 40 | ekf.updateNominal(gyro,acc,accref,att_dt); |
NaotoMorita | 77:2bf856e3eca4 | 41 | ekf.updateErrState(gyro,acc, att_dt); |
NaotoMorita | 77:2bf856e3eca4 | 42 | if(obsCount == 50){ |
osaka | 86:456f00d52974 | 43 | //ekf.updateGPSVelocity(vi.x,vi.y,vi.z,acc,accref); |
osaka | 86:456f00d52974 | 44 | //ekf.updateSinkRate(vi.z,acc,accref); |
NaotoMorita | 77:2bf856e3eca4 | 45 | obsCount = 0; |
NaotoMorita | 77:2bf856e3eca4 | 46 | }else{ |
osaka | 86:456f00d52974 | 47 | //ekf.updateGPSVelocity(vi.x,vi.y,vi.z,acc,accref); |
osaka | 86:456f00d52974 | 48 | //ekf.updateAccMeasures(acc,accref); |
osaka | 86:456f00d52974 | 49 | |
NaotoMorita | 77:2bf856e3eca4 | 50 | if(ekf.determinDynStatus(acc,accref)){ |
osaka | 86:456f00d52974 | 51 | ekf.updateAccMeasures(acc,accref); |
NaotoMorita | 77:2bf856e3eca4 | 52 | }else{ |
osaka | 86:456f00d52974 | 53 | ekf.updateStaticAccMeasures(acc,accref); |
NaotoMorita | 77:2bf856e3eca4 | 54 | } |
osaka | 86:456f00d52974 | 55 | |
NaotoMorita | 77:2bf856e3eca4 | 56 | obsCount += 1; |
NaotoMorita | 77:2bf856e3eca4 | 57 | } |
NaotoMorita | 84:ff48e01ea76b | 58 | //ekf.updateSinkRate(vi.z,acc,accref); |
osaka | 86:456f00d52974 | 59 | //ekf.updateGyroBiasConstraints(gyro); |
NaotoMorita | 83:e69ab831031c | 60 | |
NaotoMorita | 77:2bf856e3eca4 | 61 | ekf.fuseErr2Nominal(); |
NaotoMorita | 77:2bf856e3eca4 | 62 | ekf.resetBias(); |
NaotoMorita | 68:b9f6938fab9d | 63 | ekf.computeAngles(rpy, rpy_align); |
osaka | 87:89bbbcdb667b | 64 | |
NaotoMorita | 77:2bf856e3eca4 | 65 | PIDtick.loop(); |
NaotoMorita | 77:2bf856e3eca4 | 66 | |
osaka | 87:89bbbcdb667b | 67 | //制御時間を計測 |
NaotoMorita | 68:b9f6938fab9d | 68 | float tend = _t.read(); |
NaotoMorita | 68:b9f6938fab9d | 69 | att_dt = (tend-tstart); |
cocorlow | 56:888379912f81 | 70 | } |
osaka | 87:89bbbcdb667b | 71 | } |