HAPSRG / Mbed 2 deprecated hexaTest_Eigen

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@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?

UserRevisionLine numberNew 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 //}