Diff: transferData.cpp

Revision:

143:53808e4e684c

Parent:

141:725321fe2949

--- a/transferData.cpp	Fri Dec 10 11:20:13 2021 +0000
+++ b/transferData.cpp	Fri Jun 24 05:44:34 2022 +0000
@@ -5,42 +5,160 @@
     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));
+    sp.euler[0] = euler(0)*180.0f/M_PI_F;
+    sp.euler[1] = euler(1)*180.0f/M_PI_F;
+    sp.euler[2] = euler(2)*180.0f/M_PI_F;
+    VectorXf state = eskf.getState();
+    VectorXf variance = eskf.getVariance();
+    
+    switch(hilDataOut){
+    case 1 :
+        //posNED
+        for (int i = 0; i < 3; i++){
+            sp.state[i] = state(i);
+        }
+        sp.state[3] = 1.0f/att_dt;
+        for (int i = 0; i < 3; i++){
+            sp.variance[i] = variance(i);
+        }
+        break;
+    case 2 :
+        //velNED
+        for (int i = 0; i < 3; i++){
+            sp.state[i] = state(i+3);
+        }
+        sp.state[3] = 1.0f/att_dt;
+        for (int i = 0; i < 3; i++){
+            sp.variance[i] = variance(i+3);
+        }
+        break;
+    case 3 :
+        //quaternion
+        for (int i = 0; i < 4; i++){
+            sp.state[i] = state(i+6);
+        }
+        for (int i = 0; i < 3; i++){
+            sp.variance[i] = variance(i+6);
+        }
+        break;
+    case 4 :
+        //accBias
+        for (int i = 0; i < 3; i++){
+            sp.state[i] = state(i+10);
+        }
+        sp.state[3] = 1.0f/att_dt;
+        for (int i = 0; i < 3; i++){
+            sp.variance[i] = variance(i+9);
+        }
+        break;
+    case 5 :
+        //gyroBias
+        for (int i = 0; i < 3; i++){
+            sp.state[i] = state(i+13);
+        }
+        sp.state[3] = 1.0f/att_dt;
+        for (int i = 0; i < 3; i++){
+            sp.variance[i] = variance(i+12);
+        }
+        break;
+    case 6 :
+        //gyroBias
+        for (int i = 0; i < 3; i++){
+            sp.state[i] = state(i+16);
+        }
+        sp.state[3] = 1.0f/att_dt;
+        for (int i = 0; i < 3; i++){
+            sp.variance[i] = variance(i+15);
+        }
+        break;
+    }
+
+    if(hilFlag == true){
+        pc.Send(0000, &(sp));
+    }else{
+        pc.serial.printf("%f %f %f %f %f\r\n",sp.euler[0],sp.euler[1],sp.euler[2],1.0f/att_dt,std::atan2(-mag(1),mag(0))*180.0f/M_PI);
+    }
+    
 }
 
 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));
-        
+
+    Vector3f pihat = eskf.getPihat();
+    Vector3f vihat = eskf.getVihat();
+    //Matrix wind = eskf.getWind();
+    
+    tp.pi[0] = (float)pihat(0);
+    tp.pi[1] = (float)pihat(1);
+    tp.pi[2] = (float)pihat(2);
+    tp.rpy_l[0] = euler(0);
+    tp.rpy_l[1] = euler(1);
+    tp.rpy_l[2] = euler(2);
+    tp.rpy_c[0] = euler(0);
+    tp.rpy_c[1] = euler(1);
+    tp.rpy_c[2] = euler(2);
+    tp.rpy_r[0] = euler(0);
+    tp.rpy_r[1] = euler(1);
+    tp.rpy_r[2] = euler(2);
+    tp.vi[0] = (float)vihat(0);
+    tp.vi[1] = (float)vihat(1);
+    tp.vi[2] = (float)vihat(2);
+    tp.palt = -palt;
+    tp.gps_fix = (float)gps.gpsFix;
+    /*
+    if (preflightFlag)
+    {
+        tp.mode = (float)0.0f;
+    }
+    else if (posValues[0].ap_flag == 0 || posValues[1].ap_flag == 0 || posValues[2].ap_flag == 0 )
+    {
+        tp.mode = (float)1.0f;
+    }
+    else {
+        tp.mode = (float)2.0f;
+    }
+    */
+    tp.mode = (float)1.0f;
+    tp.time = _t.read();
+    tp.gps_acc = gps.Hacc;
+    tp.vx_opt = 0.0f;
+    tp.vy_opt = 0.0f;
+    tp.dist_ir = 0.0f;
+    tp.voltage[0] = 0.0f;
+    tp.voltage[1] = 0.0f;
+    tp.current[0] = 0.0f;
+    tp.current[1] = 0.0f;
+    tp.wind[0] = 0.0f;
+    tp.wind[1] = 0.0f;
+    tp.wind[2] = 0.0f;
+    tp.pitch_obj = pitch_obj;
+    tp.roll_obj = roll_obj;
+    tp.u_pitot = 0.0f;
+    
+    if (hinf_flag)
+    {
+        tp.hinf = 1.0f;
+    }
+    else
+    {
+        tp.hinf = 0.0f;
+    }
+
+    wait(0.001f);
+
+    twelite.Send(0000, &(tp));
+    //pc.printf("r: %f %f %f p: %f %f %f y: %f %f %f de: %f %f %f\r\n",posValues[0].rpy[0]*180.0f/M_PI,posValues[1].rpy[0]*180.0f/M_PI,posValues[2].rpy[0]*180.0f/M_PI,posValues[0].rpy[1]*180.0f/M_PI,posValues[1].rpy[1]*180.0f/M_PI,posValues[2].rpy[1]*180.0f/M_PI,posValues[0].rpy[2]*180.0f/M_PI,posValues[1].rpy[2]*180.0f/M_PI,posValues[2].rpy[2]*180.0f/M_PI,posValues[0].de,posValues[1].de,posValues[2].de);
 }
 
 void writeSDcard()
 {
     Vector3f pihat = eskf.getPihat();
     Vector3f vihat = eskf.getVihat();
+    //Matrix wind = eskf.getWind();
 
     lp.time = _t.read();
     lp.hertz = 1.0f/att_dt;
-//    lp.gpsFix = float(gps.gpsFix);
+    lp.gpsFix = float(gps.gpsFix);
     lp.da = da;
     lp.de = de;
     lp.dT = dT;
@@ -68,6 +186,12 @@
     lp.mag[1] = mag(1);
     lp.mag[2] = mag(2);
     lp.palt = palt;
+    lp.wind[0] = 0.0f;
+    lp.wind[1] = 0.0f;
+    lp.wind[2] = 0.0f;
+    lp.pitch_obj = pitch_obj;
+    lp.roll_obj = roll_obj;
+    lp.u_pitot = 0.0f;
 
     //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));