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@89:c9f64bd655d9, 2021-10-25 (annotated)
- Committer:
- NaotoMorita
- Date:
- Mon Oct 25 05:39:34 2021 +0000
- Revision:
- 89:c9f64bd655d9
- Parent:
- 88:be349faa1976
- Child:
- 90:96c2b0ed4b96
12state only gps
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; |
osaka | 87:89bbbcdb667b | 11 | sp.vi[0] = vi.x; |
osaka | 87:89bbbcdb667b | 12 | sp.vi[1] = vi.y; |
osaka | 87:89bbbcdb667b | 13 | sp.vi[2] = vi.z; |
osaka | 87:89bbbcdb667b | 14 | sp.vihat[0] = ekf.vihat(1,1); |
osaka | 87:89bbbcdb667b | 15 | sp.vihat[1] = ekf.vihat(2,1); |
osaka | 87:89bbbcdb667b | 16 | sp.vihat[2] = ekf.vihat(3,1); |
osaka | 87:89bbbcdb667b | 17 | sp.accBias[0] = ekf.accBias.x; |
osaka | 87:89bbbcdb667b | 18 | sp.accBias[1] = ekf.accBias.y; |
osaka | 87:89bbbcdb667b | 19 | sp.accBias[2] = ekf.accBias.z; |
osaka | 87:89bbbcdb667b | 20 | pc.Send(0000, &(sp)); |
osaka | 87:89bbbcdb667b | 21 | } |
osaka | 87:89bbbcdb667b | 22 | |
osaka | 87:89bbbcdb667b | 23 | void sendTelemetry() |
osaka | 87:89bbbcdb667b | 24 | { |
osaka | 88:be349faa1976 | 25 | //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 | 89:c9f64bd655d9 | 26 | twelite.printf("%f %f %f %f %f %f %f %f %f %f %f %f\r\n",ekf.accned(1,1),ekf.accned(2,1),ekf.accned(3,1),ekf.vihat(1,1), ekf.vihat(2,1), ekf.vihat(3,1),ekf.errState(10,1),ekf.errState(11,1),ekf.errState(12,1),rpy.x*180.0f/M_PI,rpy.y*180.0f/M_PI,rpy.z*180.0f/M_PI); |
NaotoMorita | 89:c9f64bd655d9 | 27 | //twelite.printf("%f %f %f %f %f %f %f %f %f\r\n", lsm.ax, lsm.ay, lsm.az, lsm.gx, lsm.gy, lsm.gz, lsm.mx, lsm.my, lsm.mz); |
osaka | 87:89bbbcdb667b | 28 | } |
osaka | 87:89bbbcdb667b | 29 | |
osaka | 87:89bbbcdb667b | 30 | void writeSDcard() |
osaka | 87:89bbbcdb667b | 31 | { |
osaka | 88:be349faa1976 | 32 | //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 | 33 | } |