Diff: transferData.cpp

Revision:

139:b378528c05f2

Parent:

135:49f8916588da

Child:

140:53dbdb207542

--- a/transferData.cpp	Wed Dec 01 19:17:36 2021 +0000
+++ b/transferData.cpp	Mon Dec 06 08:26:16 2021 +0000
@@ -1,76 +1,76 @@
-#include "global.hpp"
-
-void sendData2PC()
-{
-    sp.da = da;
-    sp.de = de;
-    sp.dT = dT;
-    sp.rpy[0] = rpy.x*180.0f/M_PI;
-    sp.rpy[1] = rpy.y*180.0f/M_PI;
-    sp.rpy[2] = rpy.z*180.0f/M_PI;
-    Matrix vihat = eskf.getVihat();
-    sp.vihat[0] = vihat(1,1);
-    sp.vihat[1] = vihat(2,1);
-    sp.vihat[2] = vihat(3,1);
-    pc.Send(0000, &(sp));
-}
-
-void sendTelemetry()
-{
-    Matrix pihat = eskf.getPihat();
-    Matrix vihat = eskf.getVihat();
-    tp.time=_t.read();
-    tp.hertz = 1.0f/att_dt;
-    tp.gpsFix = float(gps.gpsFix);
-    for(int i = 0;i<3;i++){
-        tp.rpy[i] = euler(i+1,1)*180.0f/M_PI;
-        tp.pihat[i] = pihat(i+1,1);
-        tp.vihat[i] = vihat(i+1,1);
-    }
-    tp.dynaccNorm = sqrt(dynaccnorm2);
-    
-    twelite.Send(0000, &(tp));
-        
-}
-
-void writeSDcard()
-{
-    Matrix pihat = eskf.getPihat();
-    Matrix vihat = eskf.getVihat();
-
-    lp.time = _t.read();
-    lp.hertz = 1.0f/att_dt;
-    lp.gpsFix = float(gps.gpsFix);
-    lp.da = da;
-    lp.de = de;
-    lp.dT = dT;
-    for(int i = 0;i<16;i++){
-        lp.rc[i] = rc[i];
-    }
-    for(int i = 0;i<3;i++){
-        lp.rpy[i] = euler(i+1,1);
-        lp.pihat[i] = pihat(i+1,1);
-        lp.vihat[i] = vihat(i+1,1);
-    }
-    lp.pi[0] = pi.x;
-    lp.pi[1] = pi.y;
-    lp.pi[2] = pi.z;
-    lp.vi[0] = vi.x;
-    lp.vi[1] = vi.y;
-    lp.vi[2] = vi.z;
-    lp.acc[0] = acc.x;
-    lp.acc[1] = acc.y;
-    lp.acc[2] = acc.z;
-    lp.gyro[0] = gyro.x;
-    lp.gyro[1] = gyro.y;
-    lp.gyro[2] = gyro.z;
-    lp.mag[0] = mag.x;
-    lp.mag[1] = mag.y;
-    lp.mag[2] = mag.z;
-    lp.palt = palt;
-
-    //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);
-    //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));
-    sd.Send(0000, &(lp));
-    //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);
-}
+//#include "global.hpp"
+//
+//void sendData2PC()
+//{
+//    sp.da = da;
+//    sp.de = de;
+//    sp.dT = dT;
+//    sp.rpy[0] = rpy.x*180.0f/M_PI;
+//    sp.rpy[1] = rpy.y*180.0f/M_PI;
+//    sp.rpy[2] = rpy.z*180.0f/M_PI;
+//    Matrix vihat = eskf.getVihat();
+//    sp.vihat[0] = vihat(1,1);
+//    sp.vihat[1] = vihat(2,1);
+//    sp.vihat[2] = vihat(3,1);
+//    pc.Send(0000, &(sp));
+//}
+//
+//void sendTelemetry()
+//{
+//    Matrix pihat = eskf.getPihat();
+//    Matrix vihat = eskf.getVihat();
+//    tp.time=_t.read();
+//    tp.hertz = 1.0f/att_dt;
+//    tp.gpsFix = float(gps.gpsFix);
+//    for(int i = 0;i<3;i++){
+//        tp.rpy[i] = euler(i+1,1)*180.0f/M_PI;
+//        tp.pihat[i] = pihat(i+1,1);
+//        tp.vihat[i] = vihat(i+1,1);
+//    }
+//    tp.dynaccNorm = sqrt(dynaccnorm2);
+//    
+//    twelite.Send(0000, &(tp));
+//        
+//}
+//
+//void writeSDcard()
+//{
+//    Matrix pihat = eskf.getPihat();
+//    Matrix vihat = eskf.getVihat();
+//
+//    lp.time = _t.read();
+//    lp.hertz = 1.0f/att_dt;
+//    lp.gpsFix = float(gps.gpsFix);
+//    lp.da = da;
+//    lp.de = de;
+//    lp.dT = dT;
+//    for(int i = 0;i<16;i++){
+//        lp.rc[i] = rc[i];
+//    }
+//    for(int i = 0;i<3;i++){
+//        lp.rpy[i] = euler(i+1,1);
+//        lp.pihat[i] = pihat(i+1,1);
+//        lp.vihat[i] = vihat(i+1,1);
+//    }
+//    lp.pi[0] = pi.x;
+//    lp.pi[1] = pi.y;
+//    lp.pi[2] = pi.z;
+//    lp.vi[0] = vi.x;
+//    lp.vi[1] = vi.y;
+//    lp.vi[2] = vi.z;
+//    lp.acc[0] = acc.x;
+//    lp.acc[1] = acc.y;
+//    lp.acc[2] = acc.z;
+//    lp.gyro[0] = gyro.x;
+//    lp.gyro[1] = gyro.y;
+//    lp.gyro[2] = gyro.z;
+//    lp.mag[0] = mag.x;
+//    lp.mag[1] = mag.y;
+//    lp.mag[2] = mag.z;
+//    lp.palt = palt;
+//
+//    //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);
+//    //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));
+//    sd.Send(0000, &(lp));
+//    //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);
+//}