HAPSRG / Mbed 2 deprecated optWingforHAPS_Eigen

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@134:d57c6b2a706b, 2021-11-30 (annotated)

Committer:
NaotoMorita
Date:
Tue Nov 30 11:27:09 2021 +0000
Revision:
134:d57c6b2a706b
Parent:
133:346ce20b3950
Child:
135:49f8916588da
teleme usapack

Who changed what in which revision?

UserRevisionLine numberNew 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 132:896ad37b534b 43 lp.da = da;
NaotoMorita 132:896ad37b534b 44 lp.de = de;
NaotoMorita 132:896ad37b534b 45 lp.dT = dT;
NaotoMorita 132:896ad37b534b 46 for(int i = 0;i<16;i++){
NaotoMorita 132:896ad37b534b 47 lp.rc[i] = rc[i];
NaotoMorita 132:896ad37b534b 48 }
NaotoMorita 132:896ad37b534b 49 for(int i = 0;i<3;i++){
NaotoMorita 132:896ad37b534b 50 lp.rpy[i] = euler(i+1,1);
NaotoMorita 132:896ad37b534b 51 lp.pihat[i] = pihat(i+1,1);
NaotoMorita 132:896ad37b534b 52 lp.vihat[i] = vihat(i+1,1);
NaotoMorita 132:896ad37b534b 53 }
NaotoMorita 132:896ad37b534b 54 lp.pi[0] = pi.x;
NaotoMorita 132:896ad37b534b 55 lp.pi[1] = pi.y;
NaotoMorita 132:896ad37b534b 56 lp.pi[2] = pi.z;
NaotoMorita 132:896ad37b534b 57 lp.vi[0] = vi.x;
NaotoMorita 132:896ad37b534b 58 lp.vi[1] = vi.y;
NaotoMorita 132:896ad37b534b 59 lp.vi[2] = vi.z;
NaotoMorita 132:896ad37b534b 60 lp.acc[0] = acc.x;
NaotoMorita 132:896ad37b534b 61 lp.acc[1] = acc.y;
NaotoMorita 132:896ad37b534b 62 lp.acc[2] = acc.z;
NaotoMorita 132:896ad37b534b 63 lp.gyro[0] = gyro.x;
NaotoMorita 132:896ad37b534b 64 lp.gyro[1] = gyro.y;
NaotoMorita 132:896ad37b534b 65 lp.gyro[2] = gyro.z;
NaotoMorita 132:896ad37b534b 66 lp.mag[0] = mag.x;
NaotoMorita 132:896ad37b534b 67 lp.mag[1] = mag.y;
NaotoMorita 132:896ad37b534b 68 lp.mag[2] = mag.z;
NaotoMorita 132:896ad37b534b 69 lp.palt = palt;
NaotoMorita 94:579e875a4244 70
osaka 88:be349faa1976 71 //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 72 //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 73 sd.Send(0000, &(lp));
NaotoMorita 132:896ad37b534b 74 //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 75 }