Diff: transferData.cpp

Revision:

140:53dbdb207542

Parent:

139:b378528c05f2

Child:

141:725321fe2949

--- a/transferData.cpp	Mon Dec 06 08:26:16 2021 +0000
+++ b/transferData.cpp	Mon Dec 06 11:37:55 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(0)*180.0f/M_PI_F;
+    sp.rpy[1] = rpy(1)*180.0f/M_PI_F;
+    sp.rpy[2] = rpy(2)*180.0f/M_PI_F;
+    Vector3f vihat = eskf.getVihat();
+    sp.vihat[0] = vihat(0);
+    sp.vihat[1] = vihat(1);
+    sp.vihat[2] = vihat(2);
+    pc.Send(0000, &(sp));
+}
+
+void sendTelemetry()
+{
+    Vector3f pihat = eskf.getPihat();
+    Vector3f 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*180.0f/M_PI_F);
+        tp.pihat[i] = pihat(i);
+        tp.vihat[i] = vihat(i);
+    }
+    tp.dynaccNorm = std::sqrt(dynaccnorm2);
+    
+    twelite.Send(0000, &(tp));
+        
+}
+
+void writeSDcard()
+{
+    Vector3f pihat = eskf.getPihat();
+    Vector3f 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);
+        lp.pihat[i] = pihat(i);
+        lp.vihat[i] = vihat(i);
+    }
+    lp.pi[0] = pi(0);
+    lp.pi[1] = pi(1);
+    lp.pi[2] = pi(2);
+    lp.vi[0] = vi(0);
+    lp.vi[1] = vi(1);
+    lp.vi[2] = vi(2);
+    lp.acc[0] = acc(0);
+    lp.acc[1] = acc(1);
+    lp.acc[2] = acc(2);
+    lp.gyro[0] = gyro(0);
+    lp.gyro[1] = gyro(1);
+    lp.gyro[2] = gyro(2);
+    lp.mag[0] = mag(0);
+    lp.mag[1] = mag(1);
+    lp.mag[2] = mag(2);
+    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);
+}