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