File content as of revision 144:b3a713b4f1c4:

#include "global.hpp"

void sendData2PC()
{
    
    for(int i = 0;i<sizeof(scaledMotorOut)/sizeof(scaledMotorOut[0]) ;i++){
        sp.motor[i] = scaledMotorOut[i];
    }
    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 :
        //gravity
        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();
    //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 = 0.0f;
    tp.roll_obj = 0.0f;
    tp.u_pitot = 0.0f;
    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.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;
    lp.wind[0] = 0.0f;
    lp.wind[1] = 0.0f;
    lp.wind[2] = 0.0f;
    lp.pitch_obj = 0.0f;
    lp.roll_obj = 0.0f;
    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));
    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);
}