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@141:725321fe2949, 2021-12-10 (annotated)
- Committer:
- cocorlow
- Date:
- Fri Dec 10 10:43:50 2021 +0000
- Revision:
- 141:725321fe2949
- Parent:
- 140:53dbdb207542
- Child:
- 143:53808e4e684c
Eigen completed
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
cocorlow | 140:53dbdb207542 | 1 | #include "global.hpp" |
cocorlow | 140:53dbdb207542 | 2 | |
cocorlow | 140:53dbdb207542 | 3 | void sendData2PC() |
cocorlow | 140:53dbdb207542 | 4 | { |
cocorlow | 140:53dbdb207542 | 5 | sp.da = da; |
cocorlow | 140:53dbdb207542 | 6 | sp.de = de; |
cocorlow | 140:53dbdb207542 | 7 | sp.dT = dT; |
cocorlow | 140:53dbdb207542 | 8 | sp.rpy[0] = rpy(0)*180.0f/M_PI_F; |
cocorlow | 140:53dbdb207542 | 9 | sp.rpy[1] = rpy(1)*180.0f/M_PI_F; |
cocorlow | 140:53dbdb207542 | 10 | sp.rpy[2] = rpy(2)*180.0f/M_PI_F; |
cocorlow | 140:53dbdb207542 | 11 | Vector3f vihat = eskf.getVihat(); |
cocorlow | 140:53dbdb207542 | 12 | sp.vihat[0] = vihat(0); |
cocorlow | 140:53dbdb207542 | 13 | sp.vihat[1] = vihat(1); |
cocorlow | 140:53dbdb207542 | 14 | sp.vihat[2] = vihat(2); |
cocorlow | 140:53dbdb207542 | 15 | pc.Send(0000, &(sp)); |
cocorlow | 140:53dbdb207542 | 16 | } |
cocorlow | 140:53dbdb207542 | 17 | |
cocorlow | 140:53dbdb207542 | 18 | void sendTelemetry() |
cocorlow | 140:53dbdb207542 | 19 | { |
cocorlow | 141:725321fe2949 | 20 | // Vector3f pihat = eskf.getPihat(); |
cocorlow | 141:725321fe2949 | 21 | // Vector3f vihat = eskf.getVihat(); |
cocorlow | 141:725321fe2949 | 22 | // tp.time=_t.read(); |
cocorlow | 141:725321fe2949 | 23 | // tp.hertz = 1.0f/att_dt; |
cocorlow | 141:725321fe2949 | 24 | // tp.gpsFix = float(gps.gpsFix); |
cocorlow | 141:725321fe2949 | 25 | // for(int i = 0;i<3;i++){ |
cocorlow | 141:725321fe2949 | 26 | // tp.rpy[i] = euler(i*180.0f/M_PI_F); |
cocorlow | 141:725321fe2949 | 27 | // tp.pihat[i] = pihat(i); |
cocorlow | 141:725321fe2949 | 28 | // tp.vihat[i] = vihat(i); |
cocorlow | 141:725321fe2949 | 29 | // } |
cocorlow | 141:725321fe2949 | 30 | // tp.dynaccNorm = std::sqrt(dynaccnorm2); |
cocorlow | 141:725321fe2949 | 31 | // |
cocorlow | 141:725321fe2949 | 32 | // twelite.Send(0000, &(tp)); |
cocorlow | 140:53dbdb207542 | 33 | |
cocorlow | 140:53dbdb207542 | 34 | } |
cocorlow | 140:53dbdb207542 | 35 | |
cocorlow | 140:53dbdb207542 | 36 | void writeSDcard() |
cocorlow | 140:53dbdb207542 | 37 | { |
cocorlow | 140:53dbdb207542 | 38 | Vector3f pihat = eskf.getPihat(); |
cocorlow | 140:53dbdb207542 | 39 | Vector3f vihat = eskf.getVihat(); |
cocorlow | 140:53dbdb207542 | 40 | |
cocorlow | 140:53dbdb207542 | 41 | lp.time = _t.read(); |
cocorlow | 140:53dbdb207542 | 42 | lp.hertz = 1.0f/att_dt; |
cocorlow | 141:725321fe2949 | 43 | // lp.gpsFix = float(gps.gpsFix); |
cocorlow | 140:53dbdb207542 | 44 | lp.da = da; |
cocorlow | 140:53dbdb207542 | 45 | lp.de = de; |
cocorlow | 140:53dbdb207542 | 46 | lp.dT = dT; |
cocorlow | 140:53dbdb207542 | 47 | for(int i = 0;i<16;i++){ |
cocorlow | 140:53dbdb207542 | 48 | lp.rc[i] = rc[i]; |
cocorlow | 140:53dbdb207542 | 49 | } |
cocorlow | 140:53dbdb207542 | 50 | for(int i = 0;i<3;i++){ |
cocorlow | 140:53dbdb207542 | 51 | lp.rpy[i] = euler(i); |
cocorlow | 140:53dbdb207542 | 52 | lp.pihat[i] = pihat(i); |
cocorlow | 140:53dbdb207542 | 53 | lp.vihat[i] = vihat(i); |
cocorlow | 140:53dbdb207542 | 54 | } |
cocorlow | 140:53dbdb207542 | 55 | lp.pi[0] = pi(0); |
cocorlow | 140:53dbdb207542 | 56 | lp.pi[1] = pi(1); |
cocorlow | 140:53dbdb207542 | 57 | lp.pi[2] = pi(2); |
cocorlow | 140:53dbdb207542 | 58 | lp.vi[0] = vi(0); |
cocorlow | 140:53dbdb207542 | 59 | lp.vi[1] = vi(1); |
cocorlow | 140:53dbdb207542 | 60 | lp.vi[2] = vi(2); |
cocorlow | 140:53dbdb207542 | 61 | lp.acc[0] = acc(0); |
cocorlow | 140:53dbdb207542 | 62 | lp.acc[1] = acc(1); |
cocorlow | 140:53dbdb207542 | 63 | lp.acc[2] = acc(2); |
cocorlow | 140:53dbdb207542 | 64 | lp.gyro[0] = gyro(0); |
cocorlow | 140:53dbdb207542 | 65 | lp.gyro[1] = gyro(1); |
cocorlow | 140:53dbdb207542 | 66 | lp.gyro[2] = gyro(2); |
cocorlow | 140:53dbdb207542 | 67 | lp.mag[0] = mag(0); |
cocorlow | 140:53dbdb207542 | 68 | lp.mag[1] = mag(1); |
cocorlow | 140:53dbdb207542 | 69 | lp.mag[2] = mag(2); |
cocorlow | 140:53dbdb207542 | 70 | lp.palt = palt; |
cocorlow | 140:53dbdb207542 | 71 | |
cocorlow | 140:53dbdb207542 | 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); |
cocorlow | 140:53dbdb207542 | 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)); |
cocorlow | 140:53dbdb207542 | 74 | sd.Send(0000, &(lp)); |
cocorlow | 140:53dbdb207542 | 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); |
cocorlow | 140:53dbdb207542 | 76 | } |