Diff: transferData.cpp

Revision:

133:346ce20b3950

Parent:

130:3a0ab2affb0f

Parent:

132:896ad37b534b

Child:

134:d57c6b2a706b

--- a/transferData.cpp	Mon Nov 29 13:13:34 2021 +0000
+++ b/transferData.cpp	Tue Nov 30 07:58:46 2021 +0000
@@ -22,14 +22,15 @@
     Matrix accBias = eskf.getAccBias();
     Matrix gyroBias = eskf.getGyroBias();
     Matrix gravity = eskf.getGravity();
-    magCalibrator.getExtremes(magbiasMin,magbiasMax);
+    //magCalibrator.getExtremes(magbiasMin,magbiasMax);
     //twelite.printf("Magbias (Min) : %f, %f, %f\r\n", magbiasMin[0], magbiasMin[1], magbiasMin[2]);
     //twelite.printf("Magbias (Max) : %f, %f, %f\r\n", magbiasMax[0], magbiasMax[1], magbiasMax[2]);
     
     //twelite.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);
     //twelite.printf("%f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f\r\n",1.0f/att_dt,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),accBias(1,1),accBias(2,1),accBias(3,1),gyroBias(1,1),gyroBias(2,1),gyroBias(3,1),gravity(1,1),gravity(2,1),gravity(3,1));
+    twelite.Send(0000, &(sp));
         
-    twelite.printf("%d %f %f %f %f %f %f %f %f %f %f %f %f %f\r\n",gps.gpsFix ,_t.read(),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),sqrt(dynaccnorm2),gps.Longitude,gps.Latitude);
+    //twelite.printf("%d %f %f %f %f %f %f %f %f %f %f %f %f %f\r\n",gps.gpsFix ,_t.read(),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),sqrt(dynaccnorm2),gps.Longitude,gps.Latitude);
     //twelite.printf("%d %f %f %f %f %f %f %f %f %f %f\r\n",gps.gpsFix ,1.0f/att_dt,rpy.x*180.0f/M_PI,rpy.y*180.0f/M_PI,rpy.z*180.0f/M_PI,pi.x,pi.y,pi.z,vi.x,vi.y,vi.z);
     //twelite.printf("%f %f %f %f %f %f\r\n",accBias(1,1),accBias(2,1),accBias(3,1),gyroBias(1,1),gyroBias(2,1),gyroBias(3,1));
     //twelite.printf("%f %f %f %f %f %f\r\n",pi.x,pi.y,pi.z,vi.x,vi.y,vi.z);
@@ -40,11 +41,39 @@
 {
     Matrix pihat = eskf.getPihat();
     Matrix vihat = eskf.getVihat();
-    Matrix accBias = eskf.getAccBias();
-    Matrix gyroBias = eskf.getGyroBias();
-    Matrix gravity = eskf.getGravity();
-    //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));
+
+    lp.time = _t.read();
+    lp.hertz = 1.0f/att_dt;
+    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);
 }