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
transferData.cpp@133:346ce20b3950, 2021-11-30 (annotated)
- Committer:
- NaotoMorita
- Date:
- Tue Nov 30 07:58:46 2021 +0000
- Revision:
- 133:346ce20b3950
- Parent:
- 130:3a0ab2affb0f
- Parent:
- 132:896ad37b534b
- Child:
- 134:d57c6b2a706b
log usapack
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
osaka | 87:89bbbcdb667b | 1 | #include "global.hpp" |
osaka | 87:89bbbcdb667b | 2 | |
osaka | 87:89bbbcdb667b | 3 | void sendData2PC() |
osaka | 87:89bbbcdb667b | 4 | { |
osaka | 87:89bbbcdb667b | 5 | sp.da = da; |
osaka | 87:89bbbcdb667b | 6 | sp.de = de; |
osaka | 87:89bbbcdb667b | 7 | sp.dT = dT; |
osaka | 87:89bbbcdb667b | 8 | sp.rpy[0] = rpy.x*180.0f/M_PI; |
osaka | 87:89bbbcdb667b | 9 | sp.rpy[1] = rpy.y*180.0f/M_PI; |
osaka | 87:89bbbcdb667b | 10 | sp.rpy[2] = rpy.z*180.0f/M_PI; |
NaotoMorita | 93:b827f78a717a | 11 | Matrix vihat = eskf.getVihat(); |
NaotoMorita | 93:b827f78a717a | 12 | sp.vihat[0] = vihat(1,1); |
NaotoMorita | 93:b827f78a717a | 13 | sp.vihat[1] = vihat(2,1); |
NaotoMorita | 93:b827f78a717a | 14 | sp.vihat[2] = vihat(3,1); |
osaka | 87:89bbbcdb667b | 15 | pc.Send(0000, &(sp)); |
osaka | 87:89bbbcdb667b | 16 | } |
osaka | 87:89bbbcdb667b | 17 | |
osaka | 87:89bbbcdb667b | 18 | void sendTelemetry() |
osaka | 87:89bbbcdb667b | 19 | { |
NaotoMorita | 93:b827f78a717a | 20 | Matrix pihat = eskf.getPihat(); |
NaotoMorita | 93:b827f78a717a | 21 | Matrix vihat = eskf.getVihat(); |
NaotoMorita | 93:b827f78a717a | 22 | Matrix accBias = eskf.getAccBias(); |
NaotoMorita | 93:b827f78a717a | 23 | Matrix gyroBias = eskf.getGyroBias(); |
NaotoMorita | 93:b827f78a717a | 24 | Matrix gravity = eskf.getGravity(); |
NaotoMorita | 132:896ad37b534b | 25 | //magCalibrator.getExtremes(magbiasMin,magbiasMax); |
NaotoMorita | 119:a21e283730d1 | 26 | //twelite.printf("Magbias (Min) : %f, %f, %f\r\n", magbiasMin[0], magbiasMin[1], magbiasMin[2]); |
NaotoMorita | 119:a21e283730d1 | 27 | //twelite.printf("Magbias (Max) : %f, %f, %f\r\n", magbiasMax[0], magbiasMax[1], magbiasMax[2]); |
NaotoMorita | 93:b827f78a717a | 28 | |
osaka | 88:be349faa1976 | 29 | //twelite.printf("%f %f %f %f %f %f\r\n",da,de,dT,rpy.x*180.0f/M_PI,rpy.y*180.0f/M_PI,rpy.z*180.0f/M_PI); |
NaotoMorita | 94:579e875a4244 | 30 | //twelite.printf("%f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f\r\n",1.0f/att_dt,rpy.x*180.0f/M_PI,rpy.y*180.0f/M_PI,rpy.z*180.0f/M_PI, pihat(1,1),pihat(2,1),pihat(3,1),vihat(1,1),vihat(2,1),vihat(3,1),accBias(1,1),accBias(2,1),accBias(3,1),gyroBias(1,1),gyroBias(2,1),gyroBias(3,1),gravity(1,1),gravity(2,1),gravity(3,1)); |
NaotoMorita | 133:346ce20b3950 | 31 | twelite.Send(0000, &(sp)); |
NaotoMorita | 129:a76be8380bb2 | 32 | |
NaotoMorita | 133:346ce20b3950 | 33 | //twelite.printf("%d %f %f %f %f %f %f %f %f %f %f %f %f %f\r\n",gps.gpsFix ,_t.read(),rpy.x*180.0f/M_PI,rpy.y*180.0f/M_PI,rpy.z*180.0f/M_PI,pihat(1,1),pihat(2,1),pihat(3,1),vihat(1,1),vihat(2,1),vihat(3,1),sqrt(dynaccnorm2),gps.Longitude,gps.Latitude); |
NaotoMorita | 129:a76be8380bb2 | 34 | //twelite.printf("%d %f %f %f %f %f %f %f %f %f %f\r\n",gps.gpsFix ,1.0f/att_dt,rpy.x*180.0f/M_PI,rpy.y*180.0f/M_PI,rpy.z*180.0f/M_PI,pi.x,pi.y,pi.z,vi.x,vi.y,vi.z); |
NaotoMorita | 93:b827f78a717a | 35 | //twelite.printf("%f %f %f %f %f %f\r\n",accBias(1,1),accBias(2,1),accBias(3,1),gyroBias(1,1),gyroBias(2,1),gyroBias(3,1)); |
NaotoMorita | 92:00460f6df439 | 36 | //twelite.printf("%f %f %f %f %f %f\r\n",pi.x,pi.y,pi.z,vi.x,vi.y,vi.z); |
NaotoMorita | 102:1c77ff6e2a85 | 37 | //twelite.printf("%f %f %f %f %f %f %f \r\n", magGamma(7,1), magGamma(8,1), magGamma(9,1), mag.x, mag.y, mag.z,magres); |
osaka | 87:89bbbcdb667b | 38 | } |
osaka | 87:89bbbcdb667b | 39 | |
osaka | 87:89bbbcdb667b | 40 | void writeSDcard() |
osaka | 87:89bbbcdb667b | 41 | { |
NaotoMorita | 94:579e875a4244 | 42 | Matrix pihat = eskf.getPihat(); |
NaotoMorita | 94:579e875a4244 | 43 | Matrix vihat = eskf.getVihat(); |
NaotoMorita | 132:896ad37b534b | 44 | |
NaotoMorita | 132:896ad37b534b | 45 | lp.time = _t.read(); |
NaotoMorita | 132:896ad37b534b | 46 | lp.hertz = 1.0f/att_dt; |
NaotoMorita | 132:896ad37b534b | 47 | lp.da = da; |
NaotoMorita | 132:896ad37b534b | 48 | lp.de = de; |
NaotoMorita | 132:896ad37b534b | 49 | lp.dT = dT; |
NaotoMorita | 132:896ad37b534b | 50 | for(int i = 0;i<16;i++){ |
NaotoMorita | 132:896ad37b534b | 51 | lp.rc[i] = rc[i]; |
NaotoMorita | 132:896ad37b534b | 52 | } |
NaotoMorita | 132:896ad37b534b | 53 | for(int i = 0;i<3;i++){ |
NaotoMorita | 132:896ad37b534b | 54 | lp.rpy[i] = euler(i+1,1); |
NaotoMorita | 132:896ad37b534b | 55 | lp.pihat[i] = pihat(i+1,1); |
NaotoMorita | 132:896ad37b534b | 56 | lp.vihat[i] = vihat(i+1,1); |
NaotoMorita | 132:896ad37b534b | 57 | } |
NaotoMorita | 132:896ad37b534b | 58 | lp.pi[0] = pi.x; |
NaotoMorita | 132:896ad37b534b | 59 | lp.pi[1] = pi.y; |
NaotoMorita | 132:896ad37b534b | 60 | lp.pi[2] = pi.z; |
NaotoMorita | 132:896ad37b534b | 61 | lp.vi[0] = vi.x; |
NaotoMorita | 132:896ad37b534b | 62 | lp.vi[1] = vi.y; |
NaotoMorita | 132:896ad37b534b | 63 | lp.vi[2] = vi.z; |
NaotoMorita | 132:896ad37b534b | 64 | lp.acc[0] = acc.x; |
NaotoMorita | 132:896ad37b534b | 65 | lp.acc[1] = acc.y; |
NaotoMorita | 132:896ad37b534b | 66 | lp.acc[2] = acc.z; |
NaotoMorita | 132:896ad37b534b | 67 | lp.gyro[0] = gyro.x; |
NaotoMorita | 132:896ad37b534b | 68 | lp.gyro[1] = gyro.y; |
NaotoMorita | 132:896ad37b534b | 69 | lp.gyro[2] = gyro.z; |
NaotoMorita | 132:896ad37b534b | 70 | lp.mag[0] = mag.x; |
NaotoMorita | 132:896ad37b534b | 71 | lp.mag[1] = mag.y; |
NaotoMorita | 132:896ad37b534b | 72 | lp.mag[2] = mag.z; |
NaotoMorita | 132:896ad37b534b | 73 | lp.palt = palt; |
NaotoMorita | 94:579e875a4244 | 74 | |
osaka | 88:be349faa1976 | 75 | //sd.printf("%f %f %f %f %f %f\r\n",da,de,dT,rpy.x*180.0f/M_PI,rpy.y*180.0f/M_PI,rpy.z*180.0f/M_PI); |
NaotoMorita | 132:896ad37b534b | 76 | //sd.printf("%f %f %f %f %f %f %f %f %f %f %f %f %f %f %f\r\n",_t.read(),da,de,dT,rc[0],rc[1],rc[2],rpy.x*180.0f/M_PI,rpy.y*180.0f/M_PI,rpy.z*180.0f/M_PI, pihat(1,1),pihat(2,1),pihat(3,1),vihat(1,1),vihat(2,1),vihat(3,1)); |
NaotoMorita | 132:896ad37b534b | 77 | sd.Send(0000, &(lp)); |
NaotoMorita | 132:896ad37b534b | 78 | //sd.printf("%f %f %f %f %f %f\r\n",da,de,dT,rpy.x*180.0f/M_PI,rpy.y*180.0f/M_PI,rpy.z*180.0f/M_PI); |
osaka | 87:89bbbcdb667b | 79 | } |