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@74:f67062e7813e, 2021-07-20 (annotated)
- Committer:
- NaotoMorita
- Date:
- Tue Jul 20 05:52:29 2021 +0000
- Revision:
- 74:f67062e7813e
- Parent:
- 73:84ffa0166e6c
- Child:
- 75:a505b9896da1
errStateEKF bug fix
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 | 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 | 68:b9f6938fab9d | 10 | ekf.defineQhat(rpy_align); |
NaotoMorita | 66:e5afad70fdd8 | 11 | float sum2accnorm = 0; |
NaotoMorita | 66:e5afad70fdd8 | 12 | float sumaccnorm = 0; |
cocorlow | 56:888379912f81 | 13 | for(int i = 0; i < 1000; i++){ |
NaotoMorita | 68:b9f6938fab9d | 14 | float tstart = _t.read(); |
NaotoMorita | 73:84ffa0166e6c | 15 | if(hilFlag == true){ |
NaotoMorita | 73:84ffa0166e6c | 16 | getHILval(); |
NaotoMorita | 73:84ffa0166e6c | 17 | }else{ |
NaotoMorita | 73:84ffa0166e6c | 18 | getIMUval(); |
NaotoMorita | 73:84ffa0166e6c | 19 | } |
NaotoMorita | 73:84ffa0166e6c | 20 | //ekf.updateQhat(gyro, att_dt); |
NaotoMorita | 73:84ffa0166e6c | 21 | //ekf.updateErrState(gyro, att_dt); |
NaotoMorita | 68:b9f6938fab9d | 22 | ekf.updateStaticAccMeasures(acc,accref); |
NaotoMorita | 73:84ffa0166e6c | 23 | //ekf.fuseErr2Qhat(); |
NaotoMorita | 73:84ffa0166e6c | 24 | //ekf.resetBias(); |
NaotoMorita | 74:f67062e7813e | 25 | ekf.updateMagMeasures(mag); |
NaotoMorita | 68:b9f6938fab9d | 26 | ekf.computeAngles(rpy, rpy_align); |
NaotoMorita | 66:e5afad70fdd8 | 27 | sumaccnorm += acc.Norm(); |
NaotoMorita | 66:e5afad70fdd8 | 28 | sum2accnorm += acc.Norm()*acc.Norm(); |
NaotoMorita | 68:b9f6938fab9d | 29 | float tend = _t.read(); |
NaotoMorita | 68:b9f6938fab9d | 30 | att_dt = (tend-tstart); |
cocorlow | 56:888379912f81 | 31 | } |
NaotoMorita | 68:b9f6938fab9d | 32 | accref.z = sumaccnorm / 1000.0f; |
NaotoMorita | 66:e5afad70fdd8 | 33 | float sigma_accnorm = sqrt(sum2accnorm/1000.0f-accref.z*accref.z); |
NaotoMorita | 68:b9f6938fab9d | 34 | pc.serial.printf("%f \r\n",sigma_accnorm); |
NaotoMorita | 73:84ffa0166e6c | 35 | pc.Subscribe(0000, &(vp)); |
cocorlow | 56:888379912f81 | 36 | |
cocorlow | 56:888379912f81 | 37 | LoopTicker PIDtick; |
cocorlow | 56:888379912f81 | 38 | PIDtick.attach(calcServoOut,PID_dt); |
cocorlow | 56:888379912f81 | 39 | |
NaotoMorita | 68:b9f6938fab9d | 40 | |
cocorlow | 56:888379912f81 | 41 | |
cocorlow | 56:888379912f81 | 42 | while(1) |
cocorlow | 56:888379912f81 | 43 | { |
cocorlow | 56:888379912f81 | 44 | float tstart = _t.read(); |
cocorlow | 56:888379912f81 | 45 | //姿勢角を更新 |
NaotoMorita | 73:84ffa0166e6c | 46 | if(hilFlag == true){ |
NaotoMorita | 73:84ffa0166e6c | 47 | getHILval(); |
NaotoMorita | 73:84ffa0166e6c | 48 | }else{ |
NaotoMorita | 73:84ffa0166e6c | 49 | getIMUval(); |
NaotoMorita | 73:84ffa0166e6c | 50 | } |
NaotoMorita | 74:f67062e7813e | 51 | |
NaotoMorita | 68:b9f6938fab9d | 52 | ekf.updateQhat(gyro, att_dt); |
NaotoMorita | 68:b9f6938fab9d | 53 | ekf.updateErrState(gyro, att_dt); |
NaotoMorita | 74:f67062e7813e | 54 | //ekf.updateAllMeasures(acc,gyro,mag,accref,tstart,att_dt); |
NaotoMorita | 68:b9f6938fab9d | 55 | if(ekf.determinDynStatus(acc,accref)){ |
NaotoMorita | 68:b9f6938fab9d | 56 | ekf.updateAccMeasures(acc,accref); |
NaotoMorita | 68:b9f6938fab9d | 57 | }else{ |
NaotoMorita | 68:b9f6938fab9d | 58 | ekf.updateStaticAccMeasures(acc,accref); |
NaotoMorita | 68:b9f6938fab9d | 59 | } |
NaotoMorita | 74:f67062e7813e | 60 | ekf.updateGyroBiasMeasures(gyro,mag,tstart,att_dt); |
NaotoMorita | 68:b9f6938fab9d | 61 | ekf.fuseErr2Qhat(); |
NaotoMorita | 68:b9f6938fab9d | 62 | ekf.resetBias(); |
NaotoMorita | 73:84ffa0166e6c | 63 | //ekf.updateMagMeasures(mag); |
NaotoMorita | 68:b9f6938fab9d | 64 | ekf.computeAngles(rpy, rpy_align); |
cocorlow | 56:888379912f81 | 65 | PIDtick.loop(); |
cocorlow | 56:888379912f81 | 66 | |
cocorlow | 56:888379912f81 | 67 | float tend = _t.read(); |
cocorlow | 56:888379912f81 | 68 | att_dt = (tend-tstart); |
cocorlow | 56:888379912f81 | 69 | } |
cocorlow | 56:888379912f81 | 70 | } |