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@61:c05353850017, 2021-06-03 (annotated)
- Committer:
- NaotoMorita
- Date:
- Thu Jun 03 11:28:13 2021 +0000
- Revision:
- 61:c05353850017
- Parent:
- 56:888379912f81
- Child:
- 66:e5afad70fdd8
modify calibrate magneto
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 | { |
cocorlow | 56:888379912f81 | 5 | pc.printf("reading calibration value\r\n"); |
cocorlow | 56:888379912f81 | 6 | //キャリブレーション値を取得 |
cocorlow | 56:888379912f81 | 7 | U read_calib; |
cocorlow | 56:888379912f81 | 8 | readEEPROM(eeprom_address, eeprom_pointeraddress, read_calib.c, N_EEPROM*4); |
cocorlow | 56:888379912f81 | 9 | wait(3); |
cocorlow | 56:888379912f81 | 10 | pos_tail = (int)read_calib.i[0]; |
NaotoMorita | 61:c05353850017 | 11 | agoffset[3] = float(read_calib.i[7]); |
NaotoMorita | 61:c05353850017 | 12 | agoffset[4] = float(read_calib.i[8]); |
NaotoMorita | 61:c05353850017 | 13 | agoffset[5] = float(read_calib.i[9]); |
NaotoMorita | 61:c05353850017 | 14 | magbiasMin[0] = float(read_calib.i[1])/1000.0f; |
NaotoMorita | 61:c05353850017 | 15 | magbiasMin[1] = float(read_calib.i[2])/1000.0f; |
NaotoMorita | 61:c05353850017 | 16 | magbiasMin[2] = float(read_calib.i[3])/1000.0f; |
NaotoMorita | 61:c05353850017 | 17 | magbiasMax[0] = float(read_calib.i[4])/1000.0f; |
NaotoMorita | 61:c05353850017 | 18 | magbiasMax[1] = float(read_calib.i[5])/1000.0f; |
NaotoMorita | 61:c05353850017 | 19 | magbiasMax[2] = float(read_calib.i[6])/1000.0f; |
NaotoMorita | 61:c05353850017 | 20 | rpy_align.y = float(read_calib.i[10])/200000.0f; |
NaotoMorita | 61:c05353850017 | 21 | rpy_align.x = float(read_calib.i[11])/200000.0f; |
NaotoMorita | 61:c05353850017 | 22 | magCalibrator.setExtremes(magbiasMin,magbiasMax); |
cocorlow | 56:888379912f81 | 23 | // tail_address[pos_tail] = (int)read_calib.i[10]; |
cocorlow | 56:888379912f81 | 24 | |
cocorlow | 56:888379912f81 | 25 | switch(pos_tail){ |
cocorlow | 56:888379912f81 | 26 | case 0: |
cocorlow | 56:888379912f81 | 27 | pc.printf("This MBED is Located at Left \r\n"); |
cocorlow | 56:888379912f81 | 28 | break; |
cocorlow | 56:888379912f81 | 29 | case 1: |
cocorlow | 56:888379912f81 | 30 | pc.printf("This MBED is Located at Center \r\n"); |
cocorlow | 56:888379912f81 | 31 | break; |
cocorlow | 56:888379912f81 | 32 | case 2: |
cocorlow | 56:888379912f81 | 33 | pc.printf("This MBED is Located at Right \r\n"); |
cocorlow | 56:888379912f81 | 34 | break; |
cocorlow | 56:888379912f81 | 35 | default: // error situation |
cocorlow | 56:888379912f81 | 36 | pc.printf("error\r\n"); |
cocorlow | 56:888379912f81 | 37 | break; |
cocorlow | 56:888379912f81 | 38 | } |
cocorlow | 56:888379912f81 | 39 | pc.printf("tail_address : %d\r\n", tail_address[pos_tail]); |
cocorlow | 56:888379912f81 | 40 | pc.printf("Alignment values are %f(pitch deg) %f(roll deg)\r\n",rpy_align.y*180.0f/M_PI,rpy_align.x*180.0f/M_PI); |
cocorlow | 56:888379912f81 | 41 | getIMUval(); |
cocorlow | 56:888379912f81 | 42 | ekf.triad(acc/acc.Norm(), accref/accref.Norm(), mag/mag.Norm(), magref/magref.Norm()); |
NaotoMorita | 61:c05353850017 | 43 | float val2_thmg = 0; |
NaotoMorita | 61:c05353850017 | 44 | float val2_accnorm = 0; |
cocorlow | 56:888379912f81 | 45 | float sumLPaccnorm = 0; |
cocorlow | 56:888379912f81 | 46 | for(int i = 0; i < 1000; i++){ |
cocorlow | 56:888379912f81 | 47 | getIMUval(); |
cocorlow | 56:888379912f81 | 48 | val_thmg += acos((mag % acc)/mag.Norm()/acc.Norm()); |
NaotoMorita | 61:c05353850017 | 49 | val2_thmg += (acos((mag % acc)/mag.Norm()/acc.Norm()))*(acos((mag % acc)/mag.Norm()/acc.Norm())); |
cocorlow | 56:888379912f81 | 50 | sumLPaccnorm += LPacc.Norm(); |
NaotoMorita | 61:c05353850017 | 51 | val2_accnorm += LPacc.Norm()*LPacc.Norm(); |
cocorlow | 56:888379912f81 | 52 | } |
cocorlow | 56:888379912f81 | 53 | accref.z = -sumLPaccnorm / 1000; |
cocorlow | 56:888379912f81 | 54 | val_thmg /= 1000; |
NaotoMorita | 61:c05353850017 | 55 | sigma_thmg = sqrt(val2_thmg/1000-val_thmg*val_thmg); |
NaotoMorita | 61:c05353850017 | 56 | sigma_accnorm = sqrt(val2_accnorm/1000-accref.z*accref.z); |
NaotoMorita | 61:c05353850017 | 57 | pc.printf("sigma: %f %f \r\n",sigma_thmg,sigma_accnorm); |
cocorlow | 56:888379912f81 | 58 | for (int i = 0; i < 3; i++) |
cocorlow | 56:888379912f81 | 59 | { |
cocorlow | 56:888379912f81 | 60 | if (i == pos_tail) |
cocorlow | 56:888379912f81 | 61 | { |
cocorlow | 56:888379912f81 | 62 | break; |
cocorlow | 56:888379912f81 | 63 | } |
cocorlow | 56:888379912f81 | 64 | else |
cocorlow | 56:888379912f81 | 65 | { |
cocorlow | 56:888379912f81 | 66 | tail.Subscribe(tail_address[i], &(posValues[i])); |
cocorlow | 56:888379912f81 | 67 | } |
cocorlow | 56:888379912f81 | 68 | } |
cocorlow | 56:888379912f81 | 69 | |
cocorlow | 56:888379912f81 | 70 | LoopTicker PIDtick; |
cocorlow | 56:888379912f81 | 71 | PIDtick.attach(calcServoOut,PID_dt); |
cocorlow | 56:888379912f81 | 72 | |
cocorlow | 56:888379912f81 | 73 | Timer _t; |
cocorlow | 56:888379912f81 | 74 | _t.start(); |
cocorlow | 56:888379912f81 | 75 | |
cocorlow | 56:888379912f81 | 76 | while(1) |
cocorlow | 56:888379912f81 | 77 | { |
cocorlow | 56:888379912f81 | 78 | float tstart = _t.read(); |
cocorlow | 56:888379912f81 | 79 | //姿勢角を更新 |
cocorlow | 56:888379912f81 | 80 | getIMUval(); |
cocorlow | 56:888379912f81 | 81 | ekf.updateBetweenMeasures(gyro, att_dt); |
cocorlow | 56:888379912f81 | 82 | ekf.computeAngles(rpy, rpy_g, rpy_align); |
cocorlow | 56:888379912f81 | 83 | PIDtick.loop(); |
cocorlow | 56:888379912f81 | 84 | |
cocorlow | 56:888379912f81 | 85 | float tend = _t.read(); |
cocorlow | 56:888379912f81 | 86 | att_dt = (tend-tstart); |
cocorlow | 56:888379912f81 | 87 | } |
cocorlow | 56:888379912f81 | 88 | } |