Diff: main.cpp

Revision:

46:99ffb98e1257

Parent:

44:f571273d3223

Child:

47:392592d21d30

--- a/main.cpp	Thu Apr 08 07:37:23 2021 +0000
+++ b/main.cpp	Mon May 17 06:21:43 2021 +0000
@@ -10,6 +10,8 @@
 #include "MatrixMath.h"
 #include "math.h"
 
+#include "UsaPack.hpp"
+
 #define MPU6050_PWR_MGMT_1   0x6B
 #define MPU_ADDRESS  0x68
 #define pi 3.141562
@@ -102,6 +104,22 @@
 int agoffset[6] = {0, 0, 0, -117, -563, 2 };
 float magbias[4] = {-140.868240, 121.863251, -162.735092, 37.112610};
 
+// UsaPack
+UsaPack cobs_sd(PC_12, PD_2, 115200);
+const int cobs_address = 1234;
+struct valuePack
+{
+    float dt;
+    int count;
+    float acc[3];
+    float gyro[3];
+    float mag[3];
+    float rot[3];
+    float rot_g[3];
+};
+valuePack vp;
+
+
 void writeSdcard()
 {
     //magcal.getExtremes(&magmin[0],&magmax[0]);
@@ -116,6 +134,26 @@
     //pc.printf("%f %f %f %f %f %f\r\n",dynacc_x,dynacc_y,dynacc_z,roll*180.0f/pi,pitch*180.0f/pi,yaw*180.0f/pi);
     //pc.printf("%f %f %f : %f %f %f\r\n",magref[0],magref[1],magref[2],mag_x,mag_y,mag_z);
     //pc.printf("%f %f %f %f %f : %f %f %f %f %f %f\r\n",f,magbias[0],magbias[1],magbias[2],magbias[3],magval[0],magval[1],magval[2],mdata.x,mdata.y,mdata.z);
+    
+    vp.dt = 1.0f/att_dt;
+    vp.count = obs_count;
+    vp.acc[0] = acc_x;
+    vp.acc[1] = acc_y;
+    vp.acc[2] = acc_z;
+    vp.gyro[0] = gyro_x;
+    vp.gyro[1] = gyro_y;
+    vp.gyro[2] = gyro_z;
+    vp.mag[0] = mag_x;
+    vp.mag[1] = mag_y;
+    vp.mag[2] = mag_z;
+    vp.rot[0] = roll*180.0f/pi;
+    vp.rot[1] = pitch*180.0f/pi;
+    vp.rot[2] = yaw*180.0f/pi;
+    vp.rot_g[0] = roll_g*180.0f/pi;
+    vp.rot_g[1] = pitch_g*180.0f/pi;
+    vp.rot_g[2] = yaw_g*180.0f/pi;
+    cobs_sd.Send(cobs_address, &vp);
+    
 }
 
 float mapfloat(float x, float in_min, float in_max, float out_min, float out_max)
@@ -457,6 +495,7 @@
     mrefs[2] /= iter_n;
     pc.printf("\r\naccreg : %f, %f, %f\r\n",arefs[0],arefs[1],arefs[2]);
     pc.printf("\r\nmagreg : %f, %f, %f\r\n",mrefs[0],mrefs[1],mrefs[2]);
+
     while(1) {wait(1000);}
 }