Diff: transferData.cpp

Revision:

93:b827f78a717a

Parent:

92:00460f6df439

Child:

94:579e875a4244

--- a/transferData.cpp	Thu Oct 28 09:44:47 2021 +0000
+++ b/transferData.cpp	Fri Oct 29 13:30:24 2021 +0000
@@ -8,18 +8,26 @@
     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;
-    sp.vihat[0] = eskf.vihat(1,1);
-    sp.vihat[1] = eskf.vihat(2,1);
-    sp.vihat[2] = eskf.vihat(3,1);
+    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();
+    Matrix accBias = eskf.getAccBias();
+    Matrix gyroBias = eskf.getGyroBias();
+    Matrix gravity = eskf.getGravity();
+    
     //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\r\n",1.0f/att_dt,eskf.vihat(1,1), eskf.vihat(2,1), eskf.vihat(3,1),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.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);
-    twelite.printf("%f %f %f %f %f %f %f %f %f\r\n", lsm.ax, lsm.ay, lsm.az, lsm.gx, lsm.gy, lsm.gz, lsm.mx, lsm.my, lsm.mz);
+    //twelite.printf("%f %f %f %f %f %f\r\n", acc.x, acc.y, acc.z, gyro.x, gyro.y, gyro.z);
 }
 
 void writeSDcard()