solaESKF_EIGEN
Dependencies: mbed LPS25HB_I2C LSM9DS1 PIDcontroller LoopTicker GPSUBX_UART_Eigen SBUS_without_mainfile MedianFilter Eigen UsaPack solaESKF_Eigen Vector3 CalibrateMagneto FastPWM
transferData.cpp@135:49f8916588da, 2021-11-30 (annotated)
- Committer:
- NaotoMorita
- Date:
- Tue Nov 30 12:04:58 2021 +0000
- Revision:
- 135:49f8916588da
- Parent:
- 134:d57c6b2a706b
- Child:
- 139:b378528c05f2
preflight check
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 | 134:d57c6b2a706b | 22 | tp.time=_t.read(); |
NaotoMorita | 134:d57c6b2a706b | 23 | tp.hertz = 1.0f/att_dt; |
NaotoMorita | 134:d57c6b2a706b | 24 | tp.gpsFix = float(gps.gpsFix); |
NaotoMorita | 134:d57c6b2a706b | 25 | for(int i = 0;i<3;i++){ |
NaotoMorita | 134:d57c6b2a706b | 26 | tp.rpy[i] = euler(i+1,1)*180.0f/M_PI; |
NaotoMorita | 134:d57c6b2a706b | 27 | tp.pihat[i] = pihat(i+1,1); |
NaotoMorita | 134:d57c6b2a706b | 28 | tp.vihat[i] = vihat(i+1,1); |
NaotoMorita | 134:d57c6b2a706b | 29 | } |
NaotoMorita | 134:d57c6b2a706b | 30 | tp.dynaccNorm = sqrt(dynaccnorm2); |
NaotoMorita | 93:b827f78a717a | 31 | |
NaotoMorita | 134:d57c6b2a706b | 32 | twelite.Send(0000, &(tp)); |
NaotoMorita | 129:a76be8380bb2 | 33 | |
osaka | 87:89bbbcdb667b | 34 | } |
osaka | 87:89bbbcdb667b | 35 | |
osaka | 87:89bbbcdb667b | 36 | void writeSDcard() |
osaka | 87:89bbbcdb667b | 37 | { |
NaotoMorita | 94:579e875a4244 | 38 | Matrix pihat = eskf.getPihat(); |
NaotoMorita | 94:579e875a4244 | 39 | Matrix vihat = eskf.getVihat(); |
NaotoMorita | 132:896ad37b534b | 40 | |
NaotoMorita | 132:896ad37b534b | 41 | lp.time = _t.read(); |
NaotoMorita | 132:896ad37b534b | 42 | lp.hertz = 1.0f/att_dt; |
NaotoMorita | 135:49f8916588da | 43 | lp.gpsFix = float(gps.gpsFix); |
NaotoMorita | 132:896ad37b534b | 44 | lp.da = da; |
NaotoMorita | 132:896ad37b534b | 45 | lp.de = de; |
NaotoMorita | 132:896ad37b534b | 46 | lp.dT = dT; |
NaotoMorita | 132:896ad37b534b | 47 | for(int i = 0;i<16;i++){ |
NaotoMorita | 132:896ad37b534b | 48 | lp.rc[i] = rc[i]; |
NaotoMorita | 132:896ad37b534b | 49 | } |
NaotoMorita | 132:896ad37b534b | 50 | for(int i = 0;i<3;i++){ |
NaotoMorita | 132:896ad37b534b | 51 | lp.rpy[i] = euler(i+1,1); |
NaotoMorita | 132:896ad37b534b | 52 | lp.pihat[i] = pihat(i+1,1); |
NaotoMorita | 132:896ad37b534b | 53 | lp.vihat[i] = vihat(i+1,1); |
NaotoMorita | 132:896ad37b534b | 54 | } |
NaotoMorita | 132:896ad37b534b | 55 | lp.pi[0] = pi.x; |
NaotoMorita | 132:896ad37b534b | 56 | lp.pi[1] = pi.y; |
NaotoMorita | 132:896ad37b534b | 57 | lp.pi[2] = pi.z; |
NaotoMorita | 132:896ad37b534b | 58 | lp.vi[0] = vi.x; |
NaotoMorita | 132:896ad37b534b | 59 | lp.vi[1] = vi.y; |
NaotoMorita | 132:896ad37b534b | 60 | lp.vi[2] = vi.z; |
NaotoMorita | 132:896ad37b534b | 61 | lp.acc[0] = acc.x; |
NaotoMorita | 132:896ad37b534b | 62 | lp.acc[1] = acc.y; |
NaotoMorita | 132:896ad37b534b | 63 | lp.acc[2] = acc.z; |
NaotoMorita | 132:896ad37b534b | 64 | lp.gyro[0] = gyro.x; |
NaotoMorita | 132:896ad37b534b | 65 | lp.gyro[1] = gyro.y; |
NaotoMorita | 132:896ad37b534b | 66 | lp.gyro[2] = gyro.z; |
NaotoMorita | 132:896ad37b534b | 67 | lp.mag[0] = mag.x; |
NaotoMorita | 132:896ad37b534b | 68 | lp.mag[1] = mag.y; |
NaotoMorita | 132:896ad37b534b | 69 | lp.mag[2] = mag.z; |
NaotoMorita | 132:896ad37b534b | 70 | lp.palt = palt; |
NaotoMorita | 94:579e875a4244 | 71 | |
osaka | 88:be349faa1976 | 72 | //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 | 73 | //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 | 74 | sd.Send(0000, &(lp)); |
NaotoMorita | 132:896ad37b534b | 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); |
osaka | 87:89bbbcdb667b | 76 | } |