File content as of revision 141:725321fe2949:

#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);
}