gcs-nf/MavLinkNode/mavlinknode.cpp

#include "mavlinknode.h"
#include "QElapsedTimer"
#include <cmath>
/*
 * mavlink 解析相关内容请参考如下
 * https://mavlink.io/en/services/command.html
 * 包含了各个函数、参数、状态机等及其说明
 **/

// WGS-84 椭球参数
constexpr double a = 6378137.0;               // 长半轴 (m)
constexpr double f = 1.0 / 298.257223563;     // 扁率
constexpr double b = a * (1 - f);             // 短半轴
constexpr double e2 = (a * a - b * b) / (a * a);  // 第一偏心率平方

struct Vec3 {
    double x, y, z;
};

// 经纬度高程 -> ECEF (X,Y,Z)
Vec3 llaToEcef(double latDeg, double lonDeg, double h) {
    double lat = latDeg * M_PI / 180.0;
    double lon = lonDeg * M_PI / 180.0;

    double sinLat = std::sin(lat);
    double cosLat = std::cos(lat);
    double sinLon = std::sin(lon);
    double cosLon = std::cos(lon);

    double N = a / std::sqrt(1.0 - e2 * sinLat * sinLat);

    double x = (N + h) * cosLat * cosLon;
    double y = (N + h) * cosLat * sinLon;
    double z = (N * (1 - e2) + h) * sinLat;

    return {x, y, z};
}

// 计算 ΔECEF = target - ref
Vec3 deltaEcef(double refLat, double refLon, double refH,
               double tgtLat, double tgtLon, double tgtH) {
    Vec3 ref = llaToEcef(refLat, refLon, refH);
    Vec3 tgt = llaToEcef(tgtLat, tgtLon, tgtH);
    return {tgt.x - ref.x, tgt.y - ref.y, tgt.z - ref.z};
}

// 计算 East 分量
double calcEast(double refLat, double refLon, double refH,
                double tgtLat, double tgtLon, double tgtH) {
    Vec3 d = deltaEcef(refLat, refLon, refH, tgtLat, tgtLon, tgtH);

    double lon = refLon * M_PI / 180.0;
    double sinLon = std::sin(lon);
    double cosLon = std::cos(lon);

    return -sinLon * d.x + cosLon * d.y;
}

// 计算 North 分量
double calcNorth(double refLat, double refLon, double refH,
                 double tgtLat, double tgtLon, double tgtH) {
    Vec3 d = deltaEcef(refLat, refLon, refH, tgtLat, tgtLon, tgtH);

    double lat = refLat * M_PI / 180.0;
    double lon = refLon * M_PI / 180.0;

    double sinLat = std::sin(lat);
    double cosLat = std::cos(lat);
    double sinLon = std::sin(lon);
    double cosLon = std::cos(lon);

    return -sinLat * cosLon * d.x - sinLat * sinLon * d.y + cosLat * d.z;
}

// 计算 Up 分量
double calcUp(double refLat, double refLon, double refH,
              double tgtLat, double tgtLon, double tgtH) {
    Vec3 d = deltaEcef(refLat, refLon, refH, tgtLat, tgtLon, tgtH);

    double lat = refLat * M_PI / 180.0;
    double lon = refLon * M_PI / 180.0;

    double sinLat = std::sin(lat);
    double cosLat = std::cos(lat);
    double sinLon = std::sin(lon);
    double cosLon = std::cos(lon);

    return cosLat * cosLon * d.x + cosLat * sinLon * d.y + sinLat * d.z;
}


// 计算合速度
double calculateTotalVelocity(double northVelocity, double eastVelocity, double downwardVelocity)
{
    // 合速度的计算公式
    double totalVelocity = std::sqrt(northVelocity * northVelocity + eastVelocity * eastVelocity + downwardVelocity * downwardVelocity);
    return totalVelocity;
}

float to360deg(float raw)
{
    float angle = 0;

     if(raw > 360)
     {
         int a = (int)raw/360;
                 raw = raw - a *360;
     }
     else if(raw < -360)
     {
         int a = (int)raw/360;
                 raw = raw - a *360;
     }

     //0~360
     if(raw < 0)
     {
         raw += 360;
     }

     angle = raw;

    return angle;
}




MavLinkNode::MavLinkNode(QObject *parent) : ThreadTemplet(parent)
{
    //初始化ID
    int Current_sysID = 0xFB;
    int Current_CompID = MAV_COMP_ID_MISSIONPLANNER;



    CommucationOverTimer = 1000;


    setRunFrq(100);//50



    timer = new QTimer();
    //timer->moveToThread(thread);
    timer->setInterval(CommucationOverTimer);

    //connect(thread, SIGNAL(started()), timer, SLOT(start()),Qt::DirectConnection);
    connect(timer,&QTimer::timeout,
            this,&MavLinkNode::TimerOut,Qt::DirectConnection);

    timer->start();

    timer_1s = new QTimer();
    timer_1s->setInterval(1000);
    connect(timer_1s,&QTimer::timeout,
            this,&MavLinkNode::timer_1s_Out,Qt::DirectConnection);

    timer_1s->start();


    if (mavLogFile)
    {
        mavLogFile->close();
        delete mavLogFile;
        mavLogFile = nullptr;
    }

    QDateTime current = QDateTime::currentDateTime();

    startuptime = current;

    mavlogFileName = QString("./log/Tlog/GCS%1.tlog").arg(current.toString("yyyyMMddHHmmss"));
    mavLogFile = new QFile(mavlogFileName);
    mavLogFile->open(QIODevice::WriteOnly);

    /*
    HeartBeatTimeName = QString("./log/Other/HeartBeat%1.csv").arg(current.toString("yyyyMMddHHmmss"));
    HeartBeatTimeFile = new QFile(HeartBeatTimeName);
    HeartBeatTimeFile->open(QIODevice::WriteOnly);



    if(HeartBeatTimeFile)
    {
    QString data;

        data.clear();
        data.append("date");                 data.append(",");
        data.append("elapse");               data.append(",");
        data.append("custom_mode ");         data.append(",");
        data.append("type ");                data.append(",");
        data.append("autopilot ");           data.append(",");
        data.append("base_mode ");           data.append(",");
        data.append("system_status ");       data.append(",");
        data.append("mavlink_version ");     data.append("\n");

        QTextStream stream(HeartBeatTimeFile);
        stream << data;
        HeartBeatTimeFile->flush();
    }
    */


    if(logTimer)
    {
        logTimer->stop();
        delete logTimer;
        logTimer = nullptr;
    }

    logTimer = new QTimer();
    logTimer->setInterval(2 * 10 * 1000);//10s

    connect(logTimer,&QTimer::timeout,
            this,&MavLinkNode::LogTimerOut,Qt::DirectConnection);

    //logTimer->start();



    hasConneted = false;
    //comm

    replay = new Replay();
    connect(replay,SIGNAL(readReady()),
            this,SLOT(readPendingDatagramsReplay()),Qt::DirectConnection);


    Mission = new MissionProcess();
    Mission->setGCSID(Current_sysID,Current_CompID);
    connect(Mission,SIGNAL(SendMessageTo(quint8,quint8*,quint16)),
           this,SIGNAL(SendMessageTo(quint8,quint8*,quint16)),Qt::DirectConnection);

    connect(this,SIGNAL(setCurrentID(int,int)),
            Mission,SLOT(setID(int,int)),Qt::DirectConnection);

    connect(Mission,SIGNAL(showMessage(QString,int)),
            this,SIGNAL(showMessage(QString,int)),Qt::DirectConnection);

    Parameter = new ParameterProcess();
    Parameter->setGCSID(Current_sysID,Current_CompID);
    connect(Parameter,SIGNAL(SendMessageTo(quint8,quint8*,quint16)),
           this,SIGNAL(SendMessageTo(quint8,quint8*,quint16)),Qt::DirectConnection);

    connect(Parameter,SIGNAL(showMessage(QString,int)),
            this,SIGNAL(showMessage(QString,int)),Qt::DirectConnection);

    Commander = new commandprocess();
    Commander->setGCSID(Current_sysID,Current_CompID);
    connect(Commander,SIGNAL(SendMessageTo(quint8,quint8*,quint16)),
            this,SIGNAL(SendMessageTo(quint8,quint8*,quint16)),Qt::DirectConnection);

    connect(this,SIGNAL(setCurrentID(int,int)),
            Commander,SLOT(setID(int,int)),Qt::DirectConnection);

    connect(Commander,SIGNAL(showMessage(QString,int)),
            this,SIGNAL(showMessage(QString,int)),Qt::DirectConnection);


    Status = new statusprocess();
    Status->setGCSID(Current_sysID,Current_CompID);
    connect(Status,SIGNAL(SendMessageTo(quint8,quint8*,quint16)),
            this,SIGNAL(SendMessageTo(quint8,quint8*,quint16)),Qt::DirectConnection);

    connect(this,SIGNAL(setCurrentID(int,int)),
            Status,SLOT(setID(int,int)),Qt::DirectConnection);

    connect(Status,SIGNAL(showMessage(QString,int)),
            this,SIGNAL(showMessage(QString,int)),Qt::DirectConnection);


/*
    Terminal = new terminal();
    Terminal->setGCSID(Current_sysID,Current_CompID);
    connect(Terminal,SIGNAL(SendMessageTo(quint8,quint8*,quint16)),
            this,SIGNAL(SendMessageTo(quint8,quint8*,quint16)),Qt::DirectConnection);

    connect(this,SIGNAL(setCurrentID(int,int)),
            Terminal,SLOT(setID(int,int)),Qt::DirectConnection);

    connect(Terminal,SIGNAL(showMessage(QString,int)),
            this,SIGNAL(showMessage(QString,int)),Qt::DirectConnection);
            */
/*
    RC = new rcprocess();
    RC->setGCSID(Current_sysID,Current_CompID);

    connect(RC,&rcprocess::SendMessageTo,
            this,&MavLinkNode::SendMessageTo,Qt::DirectConnection);


    connect(this,SIGNAL(setCurrentID(int,int)),
            RC,SLOT(setID(int,int)),Qt::DirectConnection);

    connect(RC,SIGNAL(showMessage(QString,int)),
            this,SIGNAL(showMessage(QString,int)),Qt::DirectConnection);
*/


    rtk = new rtkprocess();
    rtk->setGCSID(Current_sysID,Current_CompID);

    connect(rtk,&rtkprocess::SendMessageTo,
            this,&MavLinkNode::SendMessageTo,Qt::DirectConnection);

    connect(this,SIGNAL(setCurrentID(int,int)),
            rtk,SLOT(setID(int,int)),Qt::DirectConnection);

    connect(rtk,SIGNAL(showMessage(QString,int)),
            this,SIGNAL(showMessage(QString,int)),Qt::DirectConnection);


    isCommunicationLost = false;

    qDebug() << "mavlink" << QThread::currentThreadId();

     CreateCSV();


     Parser2_init(&parser);
     Packer2_init(&packer);


     m_heartbeat.autopilot       = 1;
     m_heartbeat.base_mode       = 0;
     m_heartbeat.custom_mode     = 0;
     m_heartbeat.mavlink_version = 3;
     m_heartbeat.system_status   = 0;
     m_heartbeat.type            = 0;





     connect(&showInfoThread, &QThread::started, this, [this]()
            {
                showInfoTimer = new QTimer;
                showInfoTimer->setInterval(200);
                connect(showInfoTimer, &QTimer::timeout, this, &MavLinkNode::showInfoTimerTimeout, Qt::DirectConnection);
                showInfoTimer->start();
            }, Qt::DirectConnection);

     showInfoThread.start();


     datstream = new DataStream();

     datstream->setGCSID(Current_sysID,Current_CompID);
     //datstream->setPlogName(plogFileName);
     connect(datstream,&DataStream::SendMessageTo,
             this,&MavLinkNode::SendMessageTo,Qt::DirectConnection);

     connect(this,SIGNAL(setCurrentID(int,int)),
             datstream,SLOT(setID(int,int)),Qt::DirectConnection);

     connect(datstream,SIGNAL(showMessage(QString,int)),
             this,SIGNAL(showMessage(QString,int)),Qt::DirectConnection);

}

MavLinkNode::~MavLinkNode()
{
    //CreateCSV();

    showInfoThread.quit();

    CloseCSV();

    if (mavLogFile)
    {
        mavLogFile->close();
        delete mavLogFile;
        mavLogFile = NULL;
    }
/*
    if (HeartBeatTimeFile)
    {
        HeartBeatTimeFile->close();
        delete HeartBeatTimeFile;
        HeartBeatTimeFile = NULL;
    }
    */

    if(logTimer)
    {
        logTimer->stop();
        delete logTimer;
        logTimer = nullptr;
    }


    //停止回放
    if(replay)
    {
        delete replay;
        replay = nullptr;
    }

    //停止任务
    if(Mission)
    {
        delete Mission;
        Mission = nullptr;
    }

    //停止参数
    if(Parameter)
    {
        delete Parameter;
        Parameter = nullptr;
    }

    if(Commander)
    {
        delete Commander;
        Commander = nullptr;
    }


    if(Status)
    {
        delete Status;
        Status = nullptr;
    }

/*
    if(Terminal)
    {
        delete Terminal;
        Terminal = nullptr;
    }
    */
/*
    if(RC)
    {
        delete RC;
        RC = nullptr;
    }
    */


    if(rtk)
    {
        delete rtk;
        rtk = nullptr;
    }


    qDebug() << "stop mavlink" << QThread::currentThreadId();
}

void MavLinkNode::setManufacturerIdentification(quint16 id)
{
    ManufacturerID = id;
}

void MavLinkNode::setGCSID(int id)
{
    qDebug() << "set GCS ID" << id;

    int gcsid = id;

      if(Mission)
      {
          Mission->setGCSID(gcsid,MAV_COMP_ID_MISSIONPLANNER);
      }

      if(Parameter)
      {
          Parameter->setGCSID(gcsid,MAV_COMP_ID_MISSIONPLANNER);
      }

      if(Commander)
      {
          Commander->setGCSID(gcsid,MAV_COMP_ID_MISSIONPLANNER);
      }


      if(Status)
      {
          Status->setGCSID(gcsid,MAV_COMP_ID_MISSIONPLANNER);
      }

/*
      if(Terminal)
      {
          Terminal->setGCSID(gcsid,MAV_COMP_ID_MISSIONPLANNER);
      }

      if(RC)
      {
          RC->setGCSID(gcsid,MAV_COMP_ID_MISSIONPLANNER);
      }
*/
      if(rtk)
      {
          rtk->setGCSID(gcsid,MAV_COMP_ID_MISSIONPLANNER);
      }


}

void MavLinkNode::CreateCSV(void)
{



    QDir *csvdir = new QDir;
    if(!csvdir->exists("./log/csv"))
    {
        qDebug() << "make dir tlog";
        csvdir->mkdir("./log/csv");//如果文件夹不存在就新建
    }

    QString filetime = QString("./log/csv/%1").arg(startuptime.toString("yyyyMMddHHmmss"));


    QDir *temp = new QDir;
    if(!temp->exists(filetime))
    {
        qDebug() << "make dir tlog";
        temp->mkdir(filetime);//如果文件夹不存在就新建
    }

    filetime.append("/");

    autopilot_version_file = new QFile(filetime + "autopilot_version.csv");
    autopilot_version_file->open(QIODevice::WriteOnly);

    sys_status_file = new QFile(filetime + "sys_status.csv");
    sys_status_file->open(QIODevice::WriteOnly);

    heartbeat_file = new QFile(filetime + "heartbeat.csv");
    heartbeat_file->open(QIODevice::WriteOnly);

    ping_file = new QFile(filetime + "ping.csv");
    ping_file->open(QIODevice::WriteOnly);

    attitude_file = new QFile(filetime + "attitude.csv");
    attitude_file->open(QIODevice::WriteOnly);

    ins1_file = new QFile(filetime + "ins1.csv");
    ins1_file->open(QIODevice::WriteOnly);

    ins2_file = new QFile(filetime + "ins2.csv");
    ins2_file->open(QIODevice::WriteOnly);

    gps_raw_int_file = new QFile(filetime + "gps_raw_int.csv");
    gps_raw_int_file->open(QIODevice::WriteOnly);

    global_position_int_file = new QFile(filetime + "global_position_int.csv");
    global_position_int_file->open(QIODevice::WriteOnly);

    servo_output_raw_file = new QFile(filetime + "servo_output_raw.csv");
    servo_output_raw_file->open(QIODevice::WriteOnly);

    rc_channels_raw_file = new QFile(filetime + "rc_channels_raw.csv");
    rc_channels_raw_file->open(QIODevice::WriteOnly);

    nav_controller_output_file = new QFile(filetime + "nav_controller_output.csv");
    nav_controller_output_file->open(QIODevice::WriteOnly);

    airspeed_autocal_file = new QFile(filetime + "airspeed_autocal.csv");
    airspeed_autocal_file->open(QIODevice::WriteOnly);

    rpm_file = new QFile(filetime + "rpm.csv");
    rpm_file->open(QIODevice::WriteOnly);

    scaled_pressure_file = new QFile(filetime + "scaled_pressure.csv");
    scaled_pressure_file->open(QIODevice::WriteOnly);

    extended_sys_state_file = new QFile(filetime + "extended_sys_state.csv");
    extended_sys_state_file->open(QIODevice::WriteOnly);

    battery_status_file = new QFile(filetime + "battery_status.csv");
    battery_status_file->open(QIODevice::WriteOnly);

    vibration_file = new QFile(filetime + "vibration.csv");
    vibration_file->open(QIODevice::WriteOnly);

    enginestate_file = new QFile(filetime + "enginestate.csv");
    enginestate_file->open(QIODevice::WriteOnly);

    vfr_hud_file = new QFile(filetime + "vfr_hud.csv");
    vfr_hud_file->open(QIODevice::WriteOnly);

    aoa_ssa_file = new QFile(filetime + "aoa_ssa.csv");
    aoa_ssa_file->open(QIODevice::WriteOnly);

    emb_atom_com_file = new QFile(filetime + "emb_atom_com.csv");
    emb_atom_com_file->open(QIODevice::WriteOnly);

    turbinstate_file  = new QFile(filetime + "turbinstate.csv");
    turbinstate_file->open(QIODevice::WriteOnly);

    bmustate_file = new QFile(filetime + "bmustate.csv");
    bmustate_file->open(QIODevice::WriteOnly);

    ccmstate_file = new QFile(filetime + "ccmstate.csv");
    ccmstate_file->open(QIODevice::WriteOnly);

    serial_control_file = new QFile(filetime + "serial_control.csv");
    serial_control_file->open(QIODevice::WriteOnly);

}


void MavLinkNode::CloseCSV(void)
{
    if(autopilot_version_file)
    {
        autopilot_version_file->close();
    }

    if(sys_status_file)
    {
        sys_status_file->close();
    }

    if(heartbeat_file)
    {
        heartbeat_file->close();
    }

    if(ping_file)
    {
        ping_file->close();
    }

    if(attitude_file)
    {
        attitude_file->close();
    }

    if(ins1_file)
    {
        ins1_file->close();
    }

    if(ins2_file)
    {
        ins2_file->close();
    }

    if(gps_raw_int_file)
    {
        gps_raw_int_file->close();
    }

    if(global_position_int_file)
    {
        global_position_int_file->close();
    }

    if(servo_output_raw_file)
    {
        servo_output_raw_file->close();
    }

    if(rc_channels_raw_file)
    {
        rc_channels_raw_file->close();
    }

    if(nav_controller_output_file)
    {
        nav_controller_output_file->close();
    }

    if(airspeed_autocal_file)
    {
        airspeed_autocal_file->close();
    }

    if(rpm_file)
    {
        rpm_file->close();
    }

    if(scaled_pressure_file)
    {
        scaled_pressure_file->close();
    }

    if(extended_sys_state_file)
    {
        extended_sys_state_file->close();
    }

    if(battery_status_file)
    {
        battery_status_file->close();
    }

    if(vibration_file)
    {
        vibration_file->close();
    }

    if(enginestate_file)
    {
        enginestate_file->close();
    }

    if(vfr_hud_file)
    {
        vfr_hud_file->close();
    }

    if(aoa_ssa_file)
    {
        aoa_ssa_file->close();
    }

    if(emb_atom_com_file)
    {
        emb_atom_com_file->close();
    }

    if(turbinstate_file)
    {
        turbinstate_file->close();
    }

    if(bmustate_file)
    {
        bmustate_file->close();
    }

    if(ccmstate_file)
    {
        ccmstate_file->close();
    }

    if(serial_control_file)
    {
        serial_control_file->close();
    }
}


bool MavLinkNode::setFileData(QFile *file,const QByteArray &data)
{
    if(data.size() == 0)
    {
        return false;
    }

    if((file)&&(file->isOpen()))
    {
        QTextStream out(file);
        out << data;
        return file->flush();
    }
    return false;
}



//这里一直在解码，一直检查双缓冲里面是否有数据，有就解码，没有就休息
void MavLinkNode::process()//线程函数
{
    QThread::msleep(5000);//5s后再发送

    setFileData(autopilot_version_file,      QByteArray("year,mon,day,hour,min,sec,ms,capabilities,flight_sw_version,middleware_sw_version,os_sw_version,board_version,uid,vendor_id,product_id,flight_custom_version[8],middleware_custom_version[8],os_custom_version[8],uid2[18]\n"));
    setFileData(sys_status_file,             QByteArray("year,mon,day,hour,min,sec,ms,present,enabled,health,load,voltage,current,remaining,drop_rate_comm,errors_comm,errors_count1,errors_count2,errors_count3,errors_count4\n"));
    setFileData(heartbeat_file,              QByteArray("year,mon,day,hour,min,sec,ms,type,autopilot,base_mode,custom_mode,system_status,mavlink\n"));
    setFileData(ping_file,                   QByteArray("year,mon,day,hour,min,sec,ms,time_usec,seq,target_system,target_component\n"));
    setFileData(attitude_file,               QByteArray("year,mon,day,hour,min,sec,ms,time_boot_ms,roll,pitch,yaw,rollspeed,pitchspeed,yawspeed\n"));
    setFileData(ins1_file,                   QByteArray("year,mon,day,hour,min,sec,ms,time_boot_ms,pitch,roll,yaw,lon,lat,alt,v_north,v_up,v_east,gx,gy,gz,ax,ay,az,time,sys,com,gps,bit,seq,eph,epv,svn\n"));
    setFileData(ins2_file,                   QByteArray("year,mon,day,hour,min,sec,ms,time_boot_ms,pitch,roll,yaw,lon,lat,alt,v_north,v_up,v_east,gx,gy,gz,ax,ay,az,time,sys,com,gps,bit,seq,eph,epv,svn\n"));
    setFileData(gps_raw_int_file,            QByteArray("year,mon,day,hour,min,sec,ms,time_usec,lat,lon,alt,eph,epv,vel,cog,fix_type,satellites_visible,alt_ellipsoid,h_acc,v_acc,vel_acc,hdg_acc,yaw\n"));
    setFileData(global_position_int_file,    QByteArray("year,mon,day,hour,min,sec,ms,time_boot_ms,lat,lon,alt,relative_alt,vx,vy,vz,hdg\n"));
    setFileData(servo_output_raw_file,       QByteArray("year,mon,day,hour,min,sec,ms,time_usec,port,ch1,ch2,ch3,ch4,ch5,ch6,ch7,ch8,ch9,ch10,ch11,ch12,ch13,ch14,ch15,ch16\n"));
    setFileData(rc_channels_raw_file,        QByteArray("year,mon,day,hour,min,sec,ms,time_boot_ms,port,rssi,chan1_raw,chan2_raw,chan3_raw,chan4_raw,chan5_raw,chan6_raw,chan7_raw,chan8_raw,chan9_raw,chan10_raw,chan11_raw,chan12_raw,chan13_raw,chan14_raw,chan15_raw,chan16_raw\n"));
    setFileData(nav_controller_output_file,  QByteArray("year,mon,day,hour,min,sec,ms,nav_roll,nav_pitch,nav_bearing,target_bearing,wp_dist,alt_err,as_err,xtrack_err\n"));
    setFileData(airspeed_autocal_file,       QByteArray("year,mon,day,hour,min,sec,ms,vx,vy,vz,diff_pressure,EAS2TAS,ratio,state_x,state_y,state_z,Pax,Pby,Pcz\n"));
    setFileData(rpm_file,                    QByteArray("year,mon,day,hour,min,sec,ms,rpm1,rpm2,rpm3,rpm4,rpm5\n"));
    setFileData(scaled_pressure_file,        QByteArray("year,mon,day,hour,min,sec,ms,time_boot_ms,press_abs,press_diff,temperature,temperature_diff\n"));
    setFileData(extended_sys_state_file,     QByteArray("year,mon,day,hour,min,sec,ms,vtol_state,landed_state\n"));
    setFileData(battery_status_file,         QByteArray("year,mon,day,hour,min,sec,ms,current_consumed,energy_consumed,temperature,voltages[0],voltages[1],voltages[2],voltages[3],voltages[4],voltages[5],voltages[6],voltages[7],voltages[8],voltages[9],current_battery,id,battery_function,type,battery_remaining,time_remaining,charge_state,voltages_ext[0],voltages_ext[1],voltages_ext[2],voltages_ext[3]\n"));
    setFileData(vibration_file,              QByteArray("year,mon,day,hour,min,sec,ms,time_usec,vibration_x,vibration_y,vibration_z,clipping_0,clipping_1,clipping_2\n"));
    setFileData(enginestate_file,            QByteArray("year,mon,day,hour,min,sec,ms,time_boot_ms,ChokeFlag,Ignition2Flag,AmbientTemperatur,AirPressure,ActualFuelPressure,FuelPumpDutyCycle,ActualJet1DutyCycle,ActualRPM,CHTemperature1,counts\n"));
    setFileData(vfr_hud_file,                QByteArray("year,mon,day,hour,min,sec,ms,airspeed,groundspeed,alt,climb,heading,throttle\n"));
    setFileData(aoa_ssa_file,                QByteArray("year,mon,day,hour,min,sec,ms,time_usec,AOA,SSA\n"));
    setFileData(emb_atom_com_file,           QByteArray("year,mon,day,hour,min,sec,ms,time_boot_ms,airspeed,beta,alpha,ps,qbar,seq,mach\n"));
    setFileData(turbinstate_file,            QByteArray("year,mon,day,hour,min,sec,ms,time_boot_ms,RPM_mea,T5,Kfuel,RPM_des,RPM_des_ap,RPM_bak,IOState,SysState,Fault,stage_ap,temp_ap,tas_ap,asl_ap,KabMain,KabFire,KDj,T1t,P1t,P3t,P5t,DJS,Vcc,Tbak,rev,CFuelMode,Cmd\n"));
    setFileData(bmustate_file,               QByteArray("year,mon,day,hour,min,sec,ms,time_boot_ms,BAT1_group_voltage_mv,BAT1_group_current_dA,BAT1_remain_perc,BAT1_low_temp_degC,AT1_hi_temp_degC,BAT1_voltages_mv[7],BAT1_hi_voltage_mv,BAT1_low_voltage_mv,BAT2_group_voltage_mv,BAT2_group_current_dA,BAT2_remain_perc,BAT2_low_temp_degC,BAT2_hi_temp_degC,BAT2_voltages_mv[14],BAT2_hi_voltage_mv,BAT2_low_voltage_mv,BAT1_STA1,BAT1_STA2,BAT2_STA1,BAT2_STA2,p500w_enabled\n"));
    setFileData(ccmstate_file,               QByteArray("year,mon,day,hour,min,sec,ms,time_boot_ms,fuel_level,temp[0],temp[1],temp[2],temp[3],volts[0],volts[1],volts[2],volts[3],echo_seq\n"));
    setFileData(serial_control_file,         QByteArray("year,mon,day,hour,min,sec,ms,baudrate,timeout,device,flags,count,data\n"));

    qDebug() << "mavlinknode" << QThread::currentThreadId();

    uint8_t count = 0;
    QByteArray datagram = nullptr;
    bool isIdle = true;


    int HeartBeatTime = QTime::currentTime().msecsSinceStartOfDay();


    QElapsedTimer time;
    time.start();

    while (running_flag)
    {
         time.restart();


         count ++;
         QThread::msleep(1000/running_frq);
         //timer->start();

         /*
         if(isSendHeartBeat == true)
         {
             if((QTime::currentTime().msecsSinceStartOfDay() - HeartBeatTime) >= (1000.0/heartbeatFrq))
             {
                 heartbeat(m_heartbeat.custom_mode,
                           m_heartbeat.type,
                           m_heartbeat.autopilot,
                           m_heartbeat.base_mode,
                           m_heartbeat.system_status,
                           m_heartbeat.mavlink_version);

                 emit heartbeatTimeStamp(QTime::currentTime().msecsSinceStartOfDay() - HeartBeatTime);

                 //qDebug() << "time:" << QTime::currentTime().msecsSinceStartOfDay() - HeartBeatTime;


                 if(HeartBeatTimeFile)
                 {
                     QString data;

                     data.clear();
                     data.append(QDateTime::currentDateTimeUtc().toString("yyyy.MM.dd HH:mm:ss:zzz"));            data.append(",");
                     data.append(QString::number(QTime::currentTime().msecsSinceStartOfDay() - HeartBeatTime));   data.append(",");
                     data.append(QString::number(m_heartbeat.custom_mode));    data.append(",");
                     data.append(QString::number(m_heartbeat.type));           data.append(",");
                     data.append(QString::number(m_heartbeat.autopilot));      data.append(",");
                     data.append(QString::number(m_heartbeat.base_mode));      data.append(",");
                     data.append(QString::number(m_heartbeat.system_status));  data.append(",");
                     data.append(QString::number(m_heartbeat.mavlink_version));data.append("\n");

                     QTextStream stream(HeartBeatTimeFile);

                     stream << data;

                     HeartBeatTimeFile->flush();

                 }

                 HeartBeatTime = QTime::currentTime().msecsSinceStartOfDay();
             }
         }
         */

         if(isSendTerminal == true)
         {
             isSendTerminal = false;
             serial_control();
         }


         //解码从UDP来的
         datagram.clear();
         datagram = readbuff(SourceType::c_sock);//每次全部读取
         if(!datagram.isEmpty())
         {
             //qDebug() << "client parse";
              Mavlinkparse(SourceType::c_sock,datagram);
         }

         //解码从串口来的
         datagram.clear();
         datagram = readbuff(SourceType::s_port);//每次全部读取
         //qDebug() << "serial port parse";
         if(!datagram.isEmpty())
         {
              //isIdle
              //qDebug() << "serial port parse";
              Mavlinkparse(SourceType::s_port,datagram);

         }



         if(isInterruptionRequested())//退出
         {
             break;
         }
         //QApplication::processEvents();
         //QThread::yieldCurrentThread();//打开这个CPU占用50%

         if(isIdle)
         {
           // QThread::msleep(1000/running_frq);
         }

         int nsec = time.nsecsElapsed();
         //qDebug() << "mavlinknode a loop using time:" << nsec << "ns";
    }
}

void MavLinkNode::TimerOut(void)
{
    if(TotalFrame_1s > 0)
    {
        rssi = (float)(TotalFrame_1s - LossFrame_1s)/ TotalFrame_1s * 100;
    }
    else
    {
        rssi = 0;
    }


    TotalFrame_1s = 0;
    LossFrame_1s = 0;


    if(hasConneted)
    {
        CommucationOverCount --;

        if(CommucationOverCount <= 0)
        {
            CommucationOverCount = 0;
            isCommunicationLost = true;
            emit CommuniationLost(isCommunicationLost);
        }
        else
        {
            emit CommuniationLost(isCommunicationLost);
        }
    }

    emit updateDlink(rssi,rate_in,0);
}

void MavLinkNode::LogTimerOut(void)
{
    /*
    if(mavLogFile)
    {
        //qDebug() << "flush file" << mavLogFile->bytesToWrite();

        //mavLogFile->bytesToWrite();

        //if(!mavLogFile->flush())
        //{
        //qDebug() << "bittotal" << bittotal;
            mavLogFile->close();
            mavLogFile->open(QIODevice::Append);
        //}
    }
    */
}



bool MavLinkNode::setLogData(mavlink_message_t msg)
{

    uint8_t buff[MAVLINK_MAX_PACKET_LEN+sizeof(quint64)];
    quint64 currentTimestamp = ((quint64)QDateTime::currentMSecsSinceEpoch()) * 1000;
    qToBigEndian(currentTimestamp, buff);
    uint16_t len = mavlink_msg_to_send_buffer(buff+sizeof(quint64), &msg);
    if (mavLogFile)
    {
        auto size  = Packer2_pack(&packer,1,buff,len+sizeof(quint64));


        QByteArray data;

        data.clear();
        data.append((const char *)packer.buff,size);

        QDataStream stream(mavLogFile);

        stream << data;

        return mavLogFile->flush();
    }
    return false;
}



void MavLinkNode::timer_1s_Out(void)
{
    static qint64 last = QDateTime::currentMSecsSinceEpoch();

    qint64 time = QDateTime::currentMSecsSinceEpoch() - last;

    if(time >= 2000)
    {
        rate_in = count_in / (time * 0.001);
        bittotal += count_in;

        count_in = 0;

        last = QDateTime::currentMSecsSinceEpoch();
    }
}

//mavlink_message_t msg;// =
void MavLinkNode::Mavlinkparse(quint32 src,QByteArray datagram)
{
    static int count = 0;
    static uint8_t last_seq = 0;
    mavlink_message_t msg;// =


    mavlink_status_t status;

    hasConneted = true;

    count_in += datagram.size();

    for (QByteArray::const_iterator i = datagram.cbegin(); i != datagram.cend(); ++i)
    {
        if(MAVLINK_FRAMING_OK == mavlink_parse_char(src,*i,&msg,&status))
        {
            parserSuccess += 1;


            if(msg.seq != ((uint8_t)(last_seq + 1)))
            {
                if(msg.seq > last_seq)
                {
                   LossFrame_1s += msg.seq - last_seq;
                }
                else
                {
                   LossFrame_1s += last_seq - 255 + msg.seq;
                }
            }


            if(msg.seq > last_seq)
            {
               TotalFrame_1s += msg.seq - last_seq;
            }
            else
            {
               TotalFrame_1s += last_seq - 255 + msg.seq;
            }

            last_seq = msg.seq;

            setLogData(msg);


            /*
            uint8_t buff[MAVLINK_MAX_PACKET_LEN+sizeof(quint64)];
            quint64 currentTimestamp = ((quint64)QDateTime::currentMSecsSinceEpoch()) * 1000;
            qToBigEndian(currentTimestamp, buff);
            uint16_t len = mavlink_msg_to_send_buffer(buff+sizeof(quint64), &msg);
            if (mavLogFile)
            {

                QByteArray data;

                data.clear();
                data.append((const char *)buff,len+sizeof(quint64));

                QDataStream stream(mavLogFile);

                stream << data;

                mavLogFile->flush();

            }
            */


            count++;
            if(msg.sysid < 250)             //过滤地面站发过来的数据
            {
                //timer->start();

                CommucationOverCount = 5;
                isCommunicationLost = false;

                //emit CommuniationLost(isCommunicationLost);

                MAVLinkRcv_Handler(msg);                 //接收完一帧数据并处理
                emit recievemsg(msg);                    //将信息广播出去
            }
        }
        else
        {
            switch(status.parse_state)
            {
            case MAVLINK_PARSE_STATE_UNINIT:{
                 //qDebug() << "MAVLINK_PARSE_STATE_UNINIT";
            }break;
            case MAVLINK_PARSE_STATE_IDLE:{
                 //qDebug() << "MAVLINK_PARSE_STATE_IDLE";
            }break;
            case MAVLINK_PARSE_STATE_GOT_STX:{
                 //qDebug() << "MAVLINK_PARSE_STATE_GOT_STX";
            }break;
            case MAVLINK_PARSE_STATE_GOT_LENGTH:{
                 //qDebug() << "MAVLINK_PARSE_STATE_GOT_LENGTH";
            }break;
            case MAVLINK_PARSE_STATE_GOT_INCOMPAT_FLAGS:{
                 //qDebug() << "MAVLINK_PARSE_STATE_GOT_INCOMPAT_FLAGS";
            }break;
            case MAVLINK_PARSE_STATE_GOT_COMPAT_FLAGS:{
                 //qDebug() << "MAVLINK_PARSE_STATE_GOT_COMPAT_FLAGS";
            }break;
            case MAVLINK_PARSE_STATE_GOT_SEQ:{
                 //qDebug() << "MAVLINK_PARSE_STATE_GOT_SEQ";
            }break;
            case MAVLINK_PARSE_STATE_GOT_SYSID:{
                 //qDebug() << "MAVLINK_PARSE_STATE_GOT_SYSID";
            }break;
            case MAVLINK_PARSE_STATE_GOT_COMPID:{
                 //qDebug() << "MAVLINK_PARSE_STATE_GOT_COMPID";
            }break;
            case MAVLINK_PARSE_STATE_GOT_MSGID1:{
                 //qDebug() << "MAVLINK_PARSE_STATE_GOT_MSGID1";
            }break;
            case MAVLINK_PARSE_STATE_GOT_MSGID2:{
                 //qDebug() << "MAVLINK_PARSE_STATE_GOT_MSGID2";
            }break;
            case MAVLINK_PARSE_STATE_GOT_MSGID3:{
                 //qDebug() << "MAVLINK_PARSE_STATE_GOT_MSGID3";
            }break;
            case MAVLINK_PARSE_STATE_GOT_PAYLOAD:{
                 //qDebug() << "MAVLINK_PARSE_STATE_GOT_PAYLOAD";
            }break;
            case MAVLINK_PARSE_STATE_GOT_CRC1:{
                 //qDebug() << "MAVLINK_PARSE_STATE_GOT_CRC1";
            }break;
            case MAVLINK_PARSE_STATE_GOT_BAD_CRC1:{
                 parserFailure += 1;
                 qDebug() << "MAVLINK_PARSE_STATE_GOT_BAD_CRC1" << msg.msgid;
                 QString string("协议解码校验错误");

                 string.append(' ');
                 string.append(QString("%1").arg(msg.msgid));
                 emit showMessage(string);


                 mavlink_message_t msg_payload;
                 mavlink_payloadalarmstate_t payloadalarmstate;

                 payloadalarmstate.uvwd_alarm_flag = 2;
                 payloadalarmstate.rwr_alarm_flag = 0; /*<  RWR alarm status: 0=no alarm; 1=threat detected; 2=alarm triggered (threat level >= set value)*/
                 payloadalarmstate.loadDevice_comm = 6; /*<  payload communication status: bit0=UV; bit1=radar; bit2=DLS.0:Normal;1:Error*/
                 payloadalarmstate.RESERVED1 = 0; /*<  RESERVED1*/
                 payloadalarmstate.RESERVED2 = 0; /*<  RESERVED2*/
                 payloadalarmstate.RESERVED3 = 0; /*<  RESERVED3*/
                 payloadalarmstate.RESERVED4 = 0; /*<  RESERVED4*/
                 payloadalarmstate.RESERVED5 = 0; /*<  RESERVED5*/
                 payloadalarmstate.RESERVED6 = 0; /*<  RESERVED6*/
                 payloadalarmstate.RESERVED7 = 0; /*<  RESERVED7*/
                 payloadalarmstate.RESERVED8 = 0; /*<  RESERVED8*/
                 payloadalarmstate.RESERVED9 = 0; /*<  RESERVED9*/
                 payloadalarmstate.RESERVED10 = 0;

                mavlink_msg_payloadalarmstate_encode(1,1,&msg_payload,&payloadalarmstate);

                uint8_t buff[MAVLINK_MAX_PACKET_LEN+sizeof(quint64)];
                uint16_t len = mavlink_msg_to_send_buffer(buff, &msg_payload);

                QString str;
                str.clear();
                for(int i = 0;i<len;i++)
                {
                    str.append(QString::number(buff[i],16).toUpper());
                    str.append(" ");
                }
                //qDebug() << str << endl;

/*
                qDebug() << QString::number(msg_payload.magic,16).toUpper()
                         << QString::number(msg_payload.len,16).toUpper()
                         << QString::number(msg_payload.compat_flags,16).toUpper()
                         << QString::number(msg_payload.incompat_flags,16).toUpper()
                         << QString::number(msg_payload.seq,16).toUpper()
                         << QString::number(msg_payload.sysid,16).toUpper()
                         << QString::number(msg_payload.compid,16).toUpper()
                         << QString::number(msg_payload.msgid,16).toUpper()
                         << QString::number(msg_payload.payload[0],16).toUpper()
                         << QString::number(msg_payload.payload[1],16).toUpper()
                         << QString::number(msg_payload.payload[2],16).toUpper()
                         << QString::number(msg_payload.checksum,16).toUpper() << endl
                         << QString::number(msg_payload.magic,16).toUpper()
                          << QString::number(msg.len,16).toUpper()
                          << QString::number(msg.compat_flags,16).toUpper()
                          << QString::number(msg.incompat_flags,16).toUpper()
                          << QString::number(msg.seq,16).toUpper()
                          << QString::number(msg.sysid,16).toUpper()
                          << QString::number(msg.compid,16).toUpper()
                          << QString::number(msg.msgid,16).toUpper()
                          << QString::number(msg.payload[0],16).toUpper()
                          << QString::number(msg.payload[1],16).toUpper()
                          << QString::number(msg.payload[2],16).toUpper()
                          << QString::number(msg.checksum,16).toUpper()
                          << QString::number(msg.ck[0],16).toUpper()
                          << QString::number(msg.ck[1],16).toUpper() << endl;
                          */

            }break;
            case MAVLINK_PARSE_STATE_SIGNATURE_WAIT:{
                 qDebug() << "MAVLINK_PARSE_STATE_SIGNATURE_WAIT";
            }break;
            }
        }
    }
}


void MavLinkNode::MAVLinkRcv_Handler(mavlink_message_t msg)
{
        //用于给参数添加设备，便于读取

        CheckVehicle(msg.sysid,msg.compid);

        switch (msg.msgid) {
        //航线部分  
        case MAVLINK_MSG_ID_MISSION_REQUEST_LIST:
        case MAVLINK_MSG_ID_MISSION_COUNT:
        case MAVLINK_MSG_ID_MISSION_REQUEST_INT:
        case MAVLINK_MSG_ID_MISSION_REQUEST:
        case MAVLINK_MSG_ID_MISSION_ITEM_INT:
        case MAVLINK_MSG_ID_MISSION_ITEM:
        case MAVLINK_MSG_ID_MISSION_ACK:
        case MAVLINK_MSG_ID_MISSION_CURRENT:
        case MAVLINK_MSG_ID_MISSION_SET_CURRENT:
        case MAVLINK_MSG_ID_MISSION_CLEAR_ALL:
        case MAVLINK_MSG_ID_MISSION_ITEM_REACHED:
        case MAVLINK_MSG_ID_MISSION_REQUEST_PARTIAL_LIST:
        case MAVLINK_MSG_ID_MISSION_WRITE_PARTIAL_LIST:
           Mission->Parse(msg);
        break;

        //参数
        case MAVLINK_MSG_ID_PARAM_REQUEST_LIST:
        case MAVLINK_MSG_ID_PARAM_REQUEST_READ:
        case MAVLINK_MSG_ID_PARAM_SET:
        case MAVLINK_MSG_ID_PARAM_VALUE:
           Parameter->Parse(msg);
        break;

        //命令
        case MAVLINK_MSG_ID_COMMAND_INT:
        case MAVLINK_MSG_ID_COMMAND_LONG:
        case MAVLINK_MSG_ID_COMMAND_ACK:
           Commander->Parse(msg);
        break;

        //终端
        case MAVLINK_MSG_ID_SERIAL_CONTROL:
        {
           //Terminal->Parse(msg);
            mavlink_serial_control_t serial_control;

            mavlink_msg_serial_control_decode(&msg,&serial_control);

            QByteArray str;

            str.append((char *)serial_control.data,serial_control.count);

            emit Recieve(str);

        }break;


        //状态
        default:
            StatusParse(msg);
         break;

        }
}

void MavLinkNode::StatusParse(mavlink_message_t msg)
{
    _vehicle vehicle = vehicleList.value(msg.sysid);

    vehicle.sysid = msg.sysid;
    vehicle.compid = msg.compid;

    gpsTimer.ms =  QDateTime::currentDateTime().currentMSecsSinceEpoch() % 1000;


    switch (msg.msgid) {
         case MAVLINK_MSG_ID_AUTOPILOT_VERSION: {
            mavlink_msg_autopilot_version_decode(&msg,&vehicle.autopilot_version);

            //gpsTimer.ms = QDateTime::currentDateTimeUtc().;

            QByteArray csvdata;
            csvdata.clear();

            csvdata.append(QString::number(gpsTimer.year));                                          csvdata.append(',');
            csvdata.append(QString::number(gpsTimer.mon));                                           csvdata.append(',');
            csvdata.append(QString::number(gpsTimer.day));                                           csvdata.append(',');
            csvdata.append(QString::number(gpsTimer.hour));                                          csvdata.append(',');
            csvdata.append(QString::number(gpsTimer.min));                                           csvdata.append(',');
            csvdata.append(QString::number(gpsTimer.sec));                                           csvdata.append(',');
            csvdata.append(QString::number(gpsTimer.ms));                                            csvdata.append(',');
            csvdata.append(QString::number(vehicle.autopilot_version.capabilities));                 csvdata.append(',');
            csvdata.append(QString::number(vehicle.autopilot_version.flight_sw_version));            csvdata.append(',');
            csvdata.append(QString::number(vehicle.autopilot_version.middleware_sw_version));        csvdata.append(',');
            csvdata.append(QString::number(vehicle.autopilot_version.os_sw_version));                csvdata.append(',');
            csvdata.append(QString::number(vehicle.autopilot_version.board_version));                csvdata.append(',');
            csvdata.append(QString::number(vehicle.autopilot_version.uid));                          csvdata.append(',');
            csvdata.append(QString::number(vehicle.autopilot_version.vendor_id));                    csvdata.append(',');
            csvdata.append(QString::number(vehicle.autopilot_version.product_id));                   csvdata.append(',');
            csvdata.append(QString::number(vehicle.autopilot_version.flight_custom_version[0]));     csvdata.append(',');
            csvdata.append(QString::number(vehicle.autopilot_version.middleware_custom_version[0])); csvdata.append(',');
            csvdata.append(QString::number(vehicle.autopilot_version.os_custom_version[0]));         csvdata.append(',');
            csvdata.append(QString::number(vehicle.autopilot_version.uid2[0]));                      csvdata.append('\n');

            setFileData(autopilot_version_file,csvdata);


         }break;
         case MAVLINK_MSG_ID_SYS_STATUS: {
            mavlink_msg_sys_status_decode(&msg,&vehicle.sys_status);

            QByteArray csvdata;
            csvdata.clear();

            csvdata.append(QString::number(gpsTimer.year));                                       csvdata.append(',');
            csvdata.append(QString::number(gpsTimer.mon));                                        csvdata.append(',');
            csvdata.append(QString::number(gpsTimer.day));                                        csvdata.append(',');
            csvdata.append(QString::number(gpsTimer.hour));                                       csvdata.append(',');
            csvdata.append(QString::number(gpsTimer.min));                                        csvdata.append(',');
            csvdata.append(QString::number(gpsTimer.sec));                                        csvdata.append(',');
            csvdata.append(QString::number(gpsTimer.ms));                                         csvdata.append(',');
            csvdata.append(QString::number(vehicle.sys_status.onboard_control_sensors_present));  csvdata.append(',');
            csvdata.append(QString::number(vehicle.sys_status.onboard_control_sensors_enabled));  csvdata.append(',');
            csvdata.append(QString::number(vehicle.sys_status.onboard_control_sensors_health));   csvdata.append(',');
            csvdata.append(QString::number(vehicle.sys_status.load));                             csvdata.append(',');
            csvdata.append(QString::number(vehicle.sys_status.voltage_battery));                  csvdata.append(',');
            csvdata.append(QString::number(vehicle.sys_status.current_battery));                  csvdata.append(',');
            csvdata.append(QString::number(vehicle.sys_status.battery_remaining));                csvdata.append(',');
            csvdata.append(QString::number(vehicle.sys_status.drop_rate_comm));                   csvdata.append(',');
            csvdata.append(QString::number(vehicle.sys_status.errors_comm));                      csvdata.append(',');
            csvdata.append(QString::number(vehicle.sys_status.errors_count1));                    csvdata.append(',');
            csvdata.append(QString::number(vehicle.sys_status.errors_count2));                    csvdata.append(',');
            csvdata.append(QString::number(vehicle.sys_status.errors_count3));                    csvdata.append(',');
            csvdata.append(QString::number(vehicle.sys_status.errors_count4));                    csvdata.append('\n');

            setFileData(sys_status_file,csvdata);

            if(((vehicle.sys_status.onboard_control_sensors_health & 0x00001000) > 0)?(true):(false))
            {
                use_ins1 = true;
            }
            else
            {
                use_ins1 = false;
            }

         }break;
         case MAVLINK_MSG_ID_HEARTBEAT: {
            mavlink_msg_heartbeat_decode(&msg,&vehicle.heartbeat);

           m_heartbeat.autopilot       = vehicle.heartbeat.autopilot;
           m_heartbeat.base_mode       = vehicle.heartbeat.base_mode;
           m_heartbeat.custom_mode     = vehicle.heartbeat.custom_mode;
           m_heartbeat.mavlink_version = vehicle.heartbeat.mavlink_version;
           m_heartbeat.system_status   = vehicle.heartbeat.system_status;
           m_heartbeat.type            = vehicle.heartbeat.type;

           QByteArray csvdata;
           csvdata.clear();

           csvdata.append(QString::number(gpsTimer.year));                      csvdata.append(',');
           csvdata.append(QString::number(gpsTimer.mon));                       csvdata.append(',');
           csvdata.append(QString::number(gpsTimer.day));                       csvdata.append(',');
           csvdata.append(QString::number(gpsTimer.hour));                      csvdata.append(',');
           csvdata.append(QString::number(gpsTimer.min));                       csvdata.append(',');
           csvdata.append(QString::number(gpsTimer.sec));                       csvdata.append(',');
           csvdata.append(QString::number(gpsTimer.ms));                        csvdata.append(',');
           csvdata.append(QString::number(vehicle.heartbeat.type));             csvdata.append(',');
           csvdata.append(QString::number(vehicle.heartbeat.autopilot));        csvdata.append(',');
           csvdata.append(QString::number(vehicle.heartbeat.base_mode));        csvdata.append(',');
           csvdata.append(QString::number(vehicle.heartbeat.custom_mode));      csvdata.append(',');
           csvdata.append(QString::number(vehicle.heartbeat.system_status));    csvdata.append(',');
           csvdata.append(QString::number(vehicle.heartbeat.mavlink_version));  csvdata.append('\n');

           setFileData(heartbeat_file,csvdata);


            emit beep(msg.sysid);
         }break;
         case MAVLINK_MSG_ID_PING: {
              mavlink_msg_ping_decode(&msg,&vehicle.ping);

              QByteArray csvdata;
              csvdata.clear();

              csvdata.append(QString::number(gpsTimer.year));                  csvdata.append(',');
              csvdata.append(QString::number(gpsTimer.mon));                   csvdata.append(',');
              csvdata.append(QString::number(gpsTimer.day));                   csvdata.append(',');
              csvdata.append(QString::number(gpsTimer.hour));                  csvdata.append(',');
              csvdata.append(QString::number(gpsTimer.min));                   csvdata.append(',');
              csvdata.append(QString::number(gpsTimer.sec));                   csvdata.append(',');
              csvdata.append(QString::number(gpsTimer.ms));                    csvdata.append(',');
              csvdata.append(QString::number(vehicle.ping.time_usec));         csvdata.append(',');
              csvdata.append(QString::number(vehicle.ping.seq));               csvdata.append(',');
              csvdata.append(QString::number(vehicle.ping.target_system));     csvdata.append(',');
              csvdata.append(QString::number(vehicle.ping.target_component));  csvdata.append('\n');


              setFileData(ping_file,csvdata);
         }break;
         case MAVLINK_MSG_ID_ATTITUDE: {
            mavlink_msg_attitude_decode(&msg,&vehicle.attitude);

            QByteArray csvdata;
            csvdata.clear();

            csvdata.append(QString::number(gpsTimer.year));                   csvdata.append(',');
            csvdata.append(QString::number(gpsTimer.mon));                    csvdata.append(',');
            csvdata.append(QString::number(gpsTimer.day));                    csvdata.append(',');
            csvdata.append(QString::number(gpsTimer.hour));                   csvdata.append(',');
            csvdata.append(QString::number(gpsTimer.min));                    csvdata.append(',');
            csvdata.append(QString::number(gpsTimer.sec));                    csvdata.append(',');
            csvdata.append(QString::number(gpsTimer.ms));                     csvdata.append(',');
            csvdata.append(QString::number(vehicle.attitude.time_boot_ms));   csvdata.append(',');
            csvdata.append(QString::number(vehicle.attitude.roll));           csvdata.append(',');
            csvdata.append(QString::number(vehicle.attitude.pitch));          csvdata.append(',');
            csvdata.append(QString::number(vehicle.attitude.yaw));            csvdata.append(',');
            csvdata.append(QString::number(vehicle.attitude.rollspeed));      csvdata.append(',');
            csvdata.append(QString::number(vehicle.attitude.pitchspeed));     csvdata.append(',');
            csvdata.append(QString::number(vehicle.attitude.yawspeed));       csvdata.append('\n');

            setFileData(attitude_file,csvdata);

         }break;
         case MAVLINK_MSG_ID_INS1: {
            mavlink_msg_ins1_decode(&msg,&vehicle.ins1);

            QByteArray csvdata;
            csvdata.clear();

            csvdata.append(QString::number(gpsTimer.year));                      csvdata.append(',');
            csvdata.append(QString::number(gpsTimer.mon));                       csvdata.append(',');
            csvdata.append(QString::number(gpsTimer.day));                       csvdata.append(',');
            csvdata.append(QString::number(gpsTimer.hour));                      csvdata.append(',');
            csvdata.append(QString::number(gpsTimer.min));                       csvdata.append(',');
            csvdata.append(QString::number(gpsTimer.sec));                       csvdata.append(',');
            csvdata.append(QString::number(gpsTimer.ms));                        csvdata.append(',');
            csvdata.append(QString::number(vehicle.ins1.time_boot_ms));          csvdata.append(',');
            csvdata.append(QString::number(vehicle.ins1.pitch));                 csvdata.append(',');
            csvdata.append(QString::number(vehicle.ins1.roll));                  csvdata.append(',');
            csvdata.append(QString::number(vehicle.ins1.yaw));                   csvdata.append(',');
            csvdata.append(QString::number(vehicle.ins1.lon,'f',8));             csvdata.append(',');
            csvdata.append(QString::number(vehicle.ins1.lat,'f',8));             csvdata.append(',');
            csvdata.append(QString::number(vehicle.ins1.alt));                   csvdata.append(',');
            csvdata.append(QString::number(vehicle.ins1.v_north));               csvdata.append(',');
            csvdata.append(QString::number(vehicle.ins1.v_up));                  csvdata.append(',');
            csvdata.append(QString::number(vehicle.ins1.v_east));                csvdata.append(',');
            csvdata.append(QString::number(vehicle.ins1.gx));                    csvdata.append(',');
            csvdata.append(QString::number(vehicle.ins1.gy));                    csvdata.append(',');
            csvdata.append(QString::number(vehicle.ins1.gz));                    csvdata.append(',');
            csvdata.append(QString::number(vehicle.ins1.ax));                    csvdata.append(',');
            csvdata.append(QString::number(vehicle.ins1.ay));                    csvdata.append(',');
            csvdata.append(QString::number(vehicle.ins1.az));                    csvdata.append(',');
            csvdata.append(QString::number((uint64_t)vehicle.ins1.time,'f',0));  csvdata.append(',');
            csvdata.append(QString::number(vehicle.ins1.sys_status));            csvdata.append(',');
            csvdata.append(QString::number(vehicle.ins1.com_status));            csvdata.append(',');
            csvdata.append(QString::number(vehicle.ins1.gps_status));            csvdata.append(',');
            csvdata.append(QString::number(vehicle.ins1.BIT));                   csvdata.append(',');
            csvdata.append(QString::number(vehicle.ins1.seq));                   csvdata.append(',');
            csvdata.append(QString::number(vehicle.ins1.eph));                   csvdata.append(',');
            csvdata.append(QString::number(vehicle.ins1.epv));                   csvdata.append(',');
            csvdata.append(QString::number(vehicle.ins1.satellites_visible));    csvdata.append('\n');

            setFileData(ins1_file,csvdata);

            emit signal_ins1(vehicle.ins1);


            if(use_ins1 == true)
            {

                uint64_t time = ((uint64_t)vehicle.ins1.time)% 1000000;
                uint64_t date = ((uint64_t)vehicle.ins1.time)/ 1000000;

                //qDebug() << "date" << date;

                uint16_t year = date / 10000;
                uint8_t  mon  = (date % 10000)/100;
                uint8_t  day  = date  % 100;

                uint8_t hour = time / 10000;

                if(hour >= 24){
                    hour -= 24;
                }

                uint8_t min  = (time % 10000)/100;
                uint8_t sec  = time  % 100;

                gpsTimer.year = year;
                gpsTimer.mon  = mon;
                gpsTimer.day  = day;

                gpsTimer.hour = hour;
                gpsTimer.min  = min;
                gpsTimer.sec  = sec;


                if(!gpstimebase)
                {
                    gpstimebase = new QDateTime();
                }

                gpstimebase->setDate(QDate(year,mon,day));
                gpstimebase->setTime(QTime(hour,min,sec));

                vehicle.timebase = gpstimebase->currentMSecsSinceEpoch();

                //qDebug() << gpstimebase->currentMSecsSinceEpoch();




                info.flag = ManufacturerID;
                info.time = (int32_t)(sec+min*60+hour*3600)*1000;
                info.num = 1;
                info.id   = infoExportID;//1
                //info.time = (int32_t)vehicle.ins1.time;//(second+min*60+hour*3600)*1000
                info.t = 0;
                info.pe = (int32_t)(calcEast(refPoint.lat,refPoint.lon, refPoint.hight, vehicle.ins1.lat, vehicle.ins1.lon, vehicle.ins1.alt)* 8);
                info.pu = (int32_t)(calcUp(refPoint.lat,refPoint.lon, refPoint.hight, vehicle.ins1.lat, vehicle.ins1.lon, vehicle.ins1.alt) * 8);
                info.ps = (int32_t)( (-calcNorth(refPoint.lat,refPoint.lon, refPoint.hight, vehicle.ins1.lat, vehicle.ins1.lon, vehicle.ins1.alt) ) * 8);
                info.ve = (int32_t)(vehicle.ins1.v_east * 1024);
                info.vu = (int32_t)(vehicle.ins1.v_up * 1024);
                info.vs = (int32_t)(-vehicle.ins1.v_north * 1024);
                info.vt = (int32_t)(calculateTotalVelocity(vehicle.ins1.v_north, vehicle.ins1.v_east, vehicle.ins1.v_up) * 1024);
                info.rcs = 0;
                info.reserve = 0;

                //qDebug() << "infoExportID" << infoExportID;

            }



         }break;
         case MAVLINK_MSG_ID_INS2: {
            mavlink_msg_ins2_decode(&msg,&vehicle.ins2);

            QByteArray csvdata;
            csvdata.clear();

            csvdata.append(QString::number(gpsTimer.year));                      csvdata.append(',');
            csvdata.append(QString::number(gpsTimer.mon));                       csvdata.append(',');
            csvdata.append(QString::number(gpsTimer.day));                       csvdata.append(',');
            csvdata.append(QString::number(gpsTimer.hour));                      csvdata.append(',');
            csvdata.append(QString::number(gpsTimer.min));                       csvdata.append(',');
            csvdata.append(QString::number(gpsTimer.sec));                       csvdata.append(',');
            csvdata.append(QString::number(gpsTimer.ms));                        csvdata.append(',');
            csvdata.append(QString::number(vehicle.ins2.time_boot_ms));          csvdata.append(',');
            csvdata.append(QString::number(vehicle.ins2.pitch));                 csvdata.append(',');
            csvdata.append(QString::number(vehicle.ins2.roll));                  csvdata.append(',');
            csvdata.append(QString::number(vehicle.ins2.yaw));                   csvdata.append(',');
            csvdata.append(QString::number(vehicle.ins2.lon,'f',8));             csvdata.append(',');
            csvdata.append(QString::number(vehicle.ins2.lat,'f',8));             csvdata.append(',');
            csvdata.append(QString::number(vehicle.ins2.alt));                   csvdata.append(',');
            csvdata.append(QString::number(vehicle.ins2.v_north));               csvdata.append(',');
            csvdata.append(QString::number(vehicle.ins2.v_up));                  csvdata.append(',');
            csvdata.append(QString::number(vehicle.ins2.v_east));                csvdata.append(',');
            csvdata.append(QString::number(vehicle.ins2.gx));                    csvdata.append(',');
            csvdata.append(QString::number(vehicle.ins2.gy));                    csvdata.append(',');
            csvdata.append(QString::number(vehicle.ins2.gz));                    csvdata.append(',');
            csvdata.append(QString::number(vehicle.ins2.ax));                    csvdata.append(',');
            csvdata.append(QString::number(vehicle.ins2.ay));                    csvdata.append(',');
            csvdata.append(QString::number(vehicle.ins2.az));                    csvdata.append(',');
            csvdata.append(QString::number((uint64_t)vehicle.ins2.time,'f',0));  csvdata.append(',');
            csvdata.append(QString::number(vehicle.ins2.sys_status));            csvdata.append(',');
            csvdata.append(QString::number(vehicle.ins2.com_status));            csvdata.append(',');
            csvdata.append(QString::number(vehicle.ins2.gps_status));            csvdata.append(',');
            csvdata.append(QString::number(vehicle.ins2.BIT));                   csvdata.append(',');
            csvdata.append(QString::number(vehicle.ins2.seq));                   csvdata.append(',');
            csvdata.append(QString::number(vehicle.ins2.eph));                   csvdata.append(',');
            csvdata.append(QString::number(vehicle.ins2.epv));                   csvdata.append(',');
            csvdata.append(QString::number(vehicle.ins2.satellites_visible));    csvdata.append('\n');

            setFileData(ins2_file,csvdata);

            emit signal_ins2(vehicle.ins2);


            if(use_ins1 == false)
            {

                uint64_t time = ((uint64_t)vehicle.ins2.time)% 1000000;
                uint64_t date = ((uint64_t)vehicle.ins2.time)/ 1000000;

                //qDebug() << "date" << date;

                uint16_t year = date / 10000;
                uint8_t  mon  = (date % 10000)/100;
                uint8_t  day  = date  % 100;

                uint8_t hour = time / 10000;

                if(hour >= 24){
                    hour -= 24;
                }

                uint8_t min  = (time % 10000)/100;
                uint8_t sec  = time  % 100;

                gpsTimer.year = year;
                gpsTimer.mon  = mon;
                gpsTimer.day  = day;

                gpsTimer.hour = hour;
                gpsTimer.min  = min;
                gpsTimer.sec  = sec;

                if(!gpstimebase)
                {
                    gpstimebase = new QDateTime();
                }

                gpstimebase->setDate(QDate(year,mon,day));
                gpstimebase->setTime(QTime(hour,min,sec));

                vehicle.timebase = gpstimebase->currentMSecsSinceEpoch();

                //qDebug() << gpstimebase->currentMSecsSinceEpoch();


                // info.flag = ManufacturerID;
                // info.id   = infoExportID;//1
                // //info.time = (int32_t)vehicle.ins2.time;
                // info.time = (int32_t)(sec+min*60+hour*3600)*1000;
                // info.lng  = vehicle.ins2.lon * 10000000;//*10000000
                // info.lat  = vehicle.ins2.lat * 10000000;//*10000000
                // info.alt  = vehicle.ins2.alt * 100;//*100
                // info.ve   = vehicle.ins2.v_east * 100;//*100
                // info.vn   = vehicle.ins2.v_north * 100;//*100
                // info.vu   = vehicle.ins2.v_up * 100;//*100
                // info.v    = sqrt(vehicle.ins2.v_east * vehicle.ins2.v_east +
                //                      vehicle.ins2.v_north * vehicle.ins2.v_north +
                //                      vehicle.ins2.v_up * vehicle.ins2.v_up)  * 100;//*100
                // info.course = to360deg(vehicle.ins2.yaw * 57.3) * 100;//*100

                info.flag = ManufacturerID;
                info.time = (int32_t)(sec+min*60+hour*3600)*1000;
                info.num = 1;
                info.id   = infoExportID;//1
                //info.time = (int32_t)vehicle.ins1.time;//(second+min*60+hour*3600)*1000
                info.t = 0;
                info.pe = (int32_t)(calcEast(refPoint.lat,refPoint.lon, refPoint.hight, vehicle.ins1.lat, vehicle.ins1.lon, vehicle.ins1.alt)* 8);
                info.pu = (int32_t)(calcUp(refPoint.lat,refPoint.lon, refPoint.hight, vehicle.ins1.lat, vehicle.ins1.lon, vehicle.ins1.alt) * 8);
                info.ps = (int32_t)( (-calcNorth(refPoint.lat,refPoint.lon, refPoint.hight, vehicle.ins1.lat, vehicle.ins1.lon, vehicle.ins1.alt) ) * 8);
                info.ve = (int32_t)(vehicle.ins2.v_east * 1024);
                info.vu = (int32_t)(vehicle.ins2.v_up * 1024);
                info.vs = (int32_t)(-vehicle.ins2.v_north * 1024);
                info.vt = (int32_t)(calculateTotalVelocity(vehicle.ins2.v_north, vehicle.ins2.v_east, vehicle.ins2.v_up) * 1024);
                info.rcs = 0;
                info.reserve = 0;


                //qDebug() << "infoExportID" << infoExportID;
            }



         }break;
         case MAVLINK_MSG_ID_GPS_RAW_INT: {
            mavlink_msg_gps_raw_int_decode(&msg,&vehicle.gps_raw_int);

            QByteArray csvdata;
            csvdata.clear();

            csvdata.append(QString::number(gpsTimer.year));                            csvdata.append(',');
            csvdata.append(QString::number(gpsTimer.mon));                             csvdata.append(',');
            csvdata.append(QString::number(gpsTimer.day));                             csvdata.append(',');
            csvdata.append(QString::number(gpsTimer.hour));                            csvdata.append(',');
            csvdata.append(QString::number(gpsTimer.min));                             csvdata.append(',');
            csvdata.append(QString::number(gpsTimer.sec));                             csvdata.append(',');
            csvdata.append(QString::number(gpsTimer.ms));                              csvdata.append(',');
            csvdata.append(QString::number(vehicle.gps_raw_int.time_usec));            csvdata.append(',');
            csvdata.append(QString::number(vehicle.gps_raw_int.lat));                  csvdata.append(',');
            csvdata.append(QString::number(vehicle.gps_raw_int.lon));                  csvdata.append(',');
            csvdata.append(QString::number(vehicle.gps_raw_int.alt));                  csvdata.append(',');
            csvdata.append(QString::number(vehicle.gps_raw_int.eph));                  csvdata.append(',');
            csvdata.append(QString::number(vehicle.gps_raw_int.epv));                  csvdata.append(',');
            csvdata.append(QString::number(vehicle.gps_raw_int.vel));                  csvdata.append(',');
            csvdata.append(QString::number(vehicle.gps_raw_int.cog));                  csvdata.append(',');
            csvdata.append(QString::number(vehicle.gps_raw_int.fix_type));             csvdata.append(',');
            csvdata.append(QString::number(vehicle.gps_raw_int.satellites_visible));   csvdata.append(',');
            csvdata.append(QString::number(vehicle.gps_raw_int.alt_ellipsoid));        csvdata.append(',');
            csvdata.append(QString::number(vehicle.gps_raw_int.time_usec));            csvdata.append(',');
            csvdata.append(QString::number(vehicle.gps_raw_int.h_acc));                csvdata.append(',');
            csvdata.append(QString::number(vehicle.gps_raw_int.v_acc));                csvdata.append(',');
            csvdata.append(QString::number(vehicle.gps_raw_int.vel_acc));              csvdata.append(',');
            csvdata.append(QString::number(vehicle.gps_raw_int.hdg_acc));              csvdata.append(',');
            csvdata.append(QString::number(vehicle.gps_raw_int.yaw));                  csvdata.append('\n');

            setFileData(gps_raw_int_file,csvdata);
         }break;
         case MAVLINK_MSG_ID_GLOBAL_POSITION_INT: {
            mavlink_msg_global_position_int_decode(&msg,&vehicle.global_position_int);

            QByteArray csvdata;
            csvdata.clear();

            csvdata.append(QString::number(gpsTimer.year));                              csvdata.append(',');
            csvdata.append(QString::number(gpsTimer.mon));                               csvdata.append(',');
            csvdata.append(QString::number(gpsTimer.day));                               csvdata.append(',');
            csvdata.append(QString::number(gpsTimer.hour));                              csvdata.append(',');
            csvdata.append(QString::number(gpsTimer.min));                               csvdata.append(',');
            csvdata.append(QString::number(gpsTimer.sec));                               csvdata.append(',');
            csvdata.append(QString::number(gpsTimer.ms));                                csvdata.append(',');
            csvdata.append(QString::number(vehicle.global_position_int.time_boot_ms));   csvdata.append(',');
            csvdata.append(QString::number(vehicle.global_position_int.lat));            csvdata.append(',');
            csvdata.append(QString::number(vehicle.global_position_int.lon));            csvdata.append(',');
            csvdata.append(QString::number(vehicle.global_position_int.alt));            csvdata.append(',');
            csvdata.append(QString::number(vehicle.global_position_int.relative_alt));   csvdata.append(',');
            csvdata.append(QString::number(vehicle.global_position_int.vx));             csvdata.append(',');
            csvdata.append(QString::number(vehicle.global_position_int.vy));             csvdata.append(',');
            csvdata.append(QString::number(vehicle.global_position_int.vz));             csvdata.append(',');
            csvdata.append(QString::number(vehicle.global_position_int.hdg));            csvdata.append('\n');

            setFileData(global_position_int_file,csvdata);

         }break;
         case MAVLINK_MSG_ID_SERVO_OUTPUT_RAW: {
            mavlink_msg_servo_output_raw_decode(&msg,&vehicle.servo_output_raw);

            QByteArray csvdata;
            csvdata.clear();

            csvdata.append(QString::number(gpsTimer.year));                         csvdata.append(',');
            csvdata.append(QString::number(gpsTimer.mon));                          csvdata.append(',');
            csvdata.append(QString::number(gpsTimer.day));                          csvdata.append(',');
            csvdata.append(QString::number(gpsTimer.hour));                         csvdata.append(',');
            csvdata.append(QString::number(gpsTimer.min));                          csvdata.append(',');
            csvdata.append(QString::number(gpsTimer.sec));                          csvdata.append(',');
            csvdata.append(QString::number(gpsTimer.ms));                           csvdata.append(',');
            csvdata.append(QString::number(vehicle.servo_output_raw.time_usec));    csvdata.append(',');
            csvdata.append(QString::number(vehicle.servo_output_raw.port));         csvdata.append(',');
            csvdata.append(QString::number(vehicle.servo_output_raw.servo1_raw));   csvdata.append(',');
            csvdata.append(QString::number(vehicle.servo_output_raw.servo2_raw));   csvdata.append(',');
            csvdata.append(QString::number(vehicle.servo_output_raw.servo3_raw));   csvdata.append(',');
            csvdata.append(QString::number(vehicle.servo_output_raw.servo4_raw));   csvdata.append(',');
            csvdata.append(QString::number(vehicle.servo_output_raw.servo5_raw));   csvdata.append(',');
            csvdata.append(QString::number(vehicle.servo_output_raw.servo6_raw));   csvdata.append(',');
            csvdata.append(QString::number(vehicle.servo_output_raw.servo7_raw));   csvdata.append(',');
            csvdata.append(QString::number(vehicle.servo_output_raw.servo8_raw));   csvdata.append(',');
            csvdata.append(QString::number(vehicle.servo_output_raw.servo9_raw));   csvdata.append(',');
            csvdata.append(QString::number(vehicle.servo_output_raw.servo10_raw));  csvdata.append(',');
            csvdata.append(QString::number(vehicle.servo_output_raw.servo11_raw));  csvdata.append(',');
            csvdata.append(QString::number(vehicle.servo_output_raw.servo12_raw));  csvdata.append(',');
            csvdata.append(QString::number(vehicle.servo_output_raw.servo13_raw));  csvdata.append(',');
            csvdata.append(QString::number(vehicle.servo_output_raw.servo14_raw));  csvdata.append(',');
            csvdata.append(QString::number(vehicle.servo_output_raw.servo15_raw));  csvdata.append(',');
            csvdata.append(QString::number(vehicle.servo_output_raw.servo16_raw));  csvdata.append('\n');

            setFileData(servo_output_raw_file,csvdata);


            emit signal_servo_output_raw(vehicle.servo_output_raw);


         }break;
         case MAVLINK_MSG_ID_RC_CHANNELS_RAW: {
            mavlink_msg_rc_channels_raw_decode(&msg,&vehicle.rc_channels_raw);

            QByteArray csvdata;
            csvdata.clear();

            csvdata.append(QString::number(gpsTimer.year));                        csvdata.append(',');
            csvdata.append(QString::number(gpsTimer.mon));                         csvdata.append(',');
            csvdata.append(QString::number(gpsTimer.day));                         csvdata.append(',');
            csvdata.append(QString::number(gpsTimer.hour));                        csvdata.append(',');
            csvdata.append(QString::number(gpsTimer.min));                         csvdata.append(',');
            csvdata.append(QString::number(gpsTimer.sec));                         csvdata.append(',');
            csvdata.append(QString::number(gpsTimer.ms));                          csvdata.append(',');
            csvdata.append(QString::number(vehicle.rc_channels_raw.time_boot_ms)); csvdata.append(',');
            csvdata.append(QString::number(vehicle.rc_channels_raw.port));         csvdata.append(',');
            csvdata.append(QString::number(vehicle.rc_channels_raw.rssi));         csvdata.append(',');
            csvdata.append(QString::number(vehicle.rc_channels_raw.chan1_raw));    csvdata.append(',');
            csvdata.append(QString::number(vehicle.rc_channels_raw.chan2_raw));    csvdata.append(',');
            csvdata.append(QString::number(vehicle.rc_channels_raw.chan3_raw));    csvdata.append(',');
            csvdata.append(QString::number(vehicle.rc_channels_raw.chan4_raw));    csvdata.append(',');
            csvdata.append(QString::number(vehicle.rc_channels_raw.chan5_raw));    csvdata.append(',');
            csvdata.append(QString::number(vehicle.rc_channels_raw.chan6_raw));    csvdata.append(',');
            csvdata.append(QString::number(vehicle.rc_channels_raw.chan7_raw));    csvdata.append(',');
            csvdata.append(QString::number(vehicle.rc_channels_raw.chan8_raw));    csvdata.append(',');
            csvdata.append(QString::number(vehicle.rc_channels_raw.chan9_raw));    csvdata.append(',');
            csvdata.append(QString::number(vehicle.rc_channels_raw.chan10_raw));   csvdata.append(',');
            csvdata.append(QString::number(vehicle.rc_channels_raw.chan11_raw));   csvdata.append(',');
            csvdata.append(QString::number(vehicle.rc_channels_raw.chan12_raw));   csvdata.append(',');
            csvdata.append(QString::number(vehicle.rc_channels_raw.chan13_raw));   csvdata.append(',');
            csvdata.append(QString::number(vehicle.rc_channels_raw.chan14_raw));   csvdata.append(',');
            csvdata.append(QString::number(vehicle.rc_channels_raw.chan15_raw));   csvdata.append(',');
            csvdata.append(QString::number(vehicle.rc_channels_raw.chan16_raw));   csvdata.append('\n');


            setFileData(rc_channels_raw_file,csvdata);
         }break;
         case MAVLINK_MSG_ID_NAV_CONTROLLER_OUTPUT: {
            mavlink_msg_nav_controller_output_decode(&msg,&vehicle.nav_controller_output);

            QByteArray csvdata;
            csvdata.clear();

            csvdata.append(QString::number(gpsTimer.year));                                   csvdata.append(',');
            csvdata.append(QString::number(gpsTimer.mon));                                    csvdata.append(',');
            csvdata.append(QString::number(gpsTimer.day));                                    csvdata.append(',');
            csvdata.append(QString::number(gpsTimer.hour));                                   csvdata.append(',');
            csvdata.append(QString::number(gpsTimer.min));                                    csvdata.append(',');
            csvdata.append(QString::number(gpsTimer.sec));                                    csvdata.append(',');
            csvdata.append(QString::number(gpsTimer.ms));                                     csvdata.append(',');
            csvdata.append(QString::number(vehicle.nav_controller_output.nav_roll));          csvdata.append(',');
            csvdata.append(QString::number(vehicle.nav_controller_output.nav_pitch));         csvdata.append(',');
            csvdata.append(QString::number(vehicle.nav_controller_output.nav_bearing));       csvdata.append(',');
            csvdata.append(QString::number(vehicle.nav_controller_output.target_bearing));    csvdata.append(',');
            csvdata.append(QString::number(vehicle.nav_controller_output.wp_dist));           csvdata.append(',');
            csvdata.append(QString::number(vehicle.nav_controller_output.alt_error));         csvdata.append(',');
            csvdata.append(QString::number(vehicle.nav_controller_output.aspd_error));        csvdata.append(',');
            csvdata.append(QString::number(vehicle.nav_controller_output.xtrack_error));      csvdata.append('\n');

            setFileData(nav_controller_output_file,csvdata);
         }break;
         case MAVLINK_MSG_ID_AIRSPEED_AUTOCAL: {
            mavlink_msg_airspeed_autocal_decode(&msg,&vehicle.airspeed_autocal);
            QByteArray csvdata;
            csvdata.clear();

            csvdata.append(QString::number(gpsTimer.year));                          csvdata.append(',');
            csvdata.append(QString::number(gpsTimer.mon));                           csvdata.append(',');
            csvdata.append(QString::number(gpsTimer.day));                           csvdata.append(',');
            csvdata.append(QString::number(gpsTimer.hour));                          csvdata.append(',');
            csvdata.append(QString::number(gpsTimer.min));                           csvdata.append(',');
            csvdata.append(QString::number(gpsTimer.sec));                           csvdata.append(',');
            csvdata.append(QString::number(gpsTimer.ms));                            csvdata.append(',');
            csvdata.append(QString::number(vehicle.airspeed_autocal.vx));            csvdata.append(',');
            csvdata.append(QString::number(vehicle.airspeed_autocal.vy));            csvdata.append(',');
            csvdata.append(QString::number(vehicle.airspeed_autocal.vz));            csvdata.append(',');
            csvdata.append(QString::number(vehicle.airspeed_autocal.diff_pressure)); csvdata.append(',');
            csvdata.append(QString::number(vehicle.airspeed_autocal.EAS2TAS));       csvdata.append(',');
            csvdata.append(QString::number(vehicle.airspeed_autocal.ratio));         csvdata.append(',');
            csvdata.append(QString::number(vehicle.airspeed_autocal.state_x));       csvdata.append(',');
            csvdata.append(QString::number(vehicle.airspeed_autocal.state_y));       csvdata.append(',');
            csvdata.append(QString::number(vehicle.airspeed_autocal.state_z));       csvdata.append(',');
            csvdata.append(QString::number(vehicle.airspeed_autocal.Pax));           csvdata.append(',');
            csvdata.append(QString::number(vehicle.airspeed_autocal.Pby));           csvdata.append(',');
            csvdata.append(QString::number(vehicle.airspeed_autocal.Pcz));           csvdata.append('\n');

            setFileData(airspeed_autocal_file,csvdata);
         }break;
         case MAVLINK_MSG_ID_RPM: {
            mavlink_msg_rpm_decode(&msg,&vehicle.rpm);

            QByteArray csvdata;
            csvdata.clear();

            csvdata.append(QString::number(gpsTimer.year));    csvdata.append(',');
            csvdata.append(QString::number(gpsTimer.mon));     csvdata.append(',');
            csvdata.append(QString::number(gpsTimer.day));     csvdata.append(',');
            csvdata.append(QString::number(gpsTimer.hour));    csvdata.append(',');
            csvdata.append(QString::number(gpsTimer.min));     csvdata.append(',');
            csvdata.append(QString::number(gpsTimer.sec));     csvdata.append(',');
            csvdata.append(QString::number(gpsTimer.ms));      csvdata.append(',');
            csvdata.append(QString::number(vehicle.rpm.rpm1)); csvdata.append(',');
            csvdata.append(QString::number(vehicle.rpm.rpm2)); csvdata.append(',');
            csvdata.append(QString::number(vehicle.rpm.rpm3)); csvdata.append(',');
            csvdata.append(QString::number(vehicle.rpm.rpm4)); csvdata.append(',');
            csvdata.append(QString::number(vehicle.rpm.rpm5)); csvdata.append('\n');

            setFileData(rpm_file,csvdata);
         }break;
         case MAVLINK_MSG_ID_SCALED_PRESSURE: {
            mavlink_msg_scaled_pressure_decode(&msg,&vehicle.scaled_pressure);

            QByteArray csvdata;
            csvdata.clear();

            csvdata.append(QString::number(gpsTimer.year));                                  csvdata.append(',');
            csvdata.append(QString::number(gpsTimer.mon));                                   csvdata.append(',');
            csvdata.append(QString::number(gpsTimer.day));                                   csvdata.append(',');
            csvdata.append(QString::number(gpsTimer.hour));                                  csvdata.append(',');
            csvdata.append(QString::number(gpsTimer.min));                                   csvdata.append(',');
            csvdata.append(QString::number(gpsTimer.sec));                                   csvdata.append(',');
            csvdata.append(QString::number(gpsTimer.ms));                                    csvdata.append(',');
            csvdata.append(QString::number(vehicle.scaled_pressure.time_boot_ms));           csvdata.append(',');
            csvdata.append(QString::number(vehicle.scaled_pressure.press_abs));              csvdata.append(',');
            csvdata.append(QString::number(vehicle.scaled_pressure.press_diff));             csvdata.append(',');
            csvdata.append(QString::number(vehicle.scaled_pressure.temperature));            csvdata.append(',');
            csvdata.append(QString::number(vehicle.scaled_pressure.temperature_press_diff)); csvdata.append('\n');

            setFileData(scaled_pressure_file,csvdata);
         }break;
         case MAVLINK_MSG_ID_EXTENDED_SYS_STATE: {
            mavlink_msg_extended_sys_state_decode(&msg,&vehicle.extended_sys_state);

            QByteArray csvdata;
            csvdata.clear();

            csvdata.append(QString::number(gpsTimer.year));                           csvdata.append(',');
            csvdata.append(QString::number(gpsTimer.mon));                            csvdata.append(',');
            csvdata.append(QString::number(gpsTimer.day));                            csvdata.append(',');
            csvdata.append(QString::number(gpsTimer.hour));                           csvdata.append(',');
            csvdata.append(QString::number(gpsTimer.min));                            csvdata.append(',');
            csvdata.append(QString::number(gpsTimer.sec));                            csvdata.append(',');
            csvdata.append(QString::number(gpsTimer.ms));                             csvdata.append(',');
            csvdata.append(QString::number(vehicle.extended_sys_state.vtol_state));   csvdata.append(',');
            csvdata.append(QString::number(vehicle.extended_sys_state.landed_state)); csvdata.append('\n');

            setFileData(extended_sys_state_file,csvdata);
         }break;
         case MAVLINK_MSG_ID_BATTERY_STATUS: {
            mavlink_msg_battery_status_decode(&msg,&vehicle.battery_status);

            QByteArray csvdata;
            csvdata.clear();

            csvdata.append(QString::number(gpsTimer.year));                              csvdata.append(',');
            csvdata.append(QString::number(gpsTimer.mon));                               csvdata.append(',');
            csvdata.append(QString::number(gpsTimer.day));                               csvdata.append(',');
            csvdata.append(QString::number(gpsTimer.hour));                              csvdata.append(',');
            csvdata.append(QString::number(gpsTimer.min));                               csvdata.append(',');
            csvdata.append(QString::number(gpsTimer.sec));                               csvdata.append(',');
            csvdata.append(QString::number(gpsTimer.ms));                                csvdata.append(',');
            csvdata.append(QString::number(vehicle.battery_status.current_consumed));    csvdata.append(',');
            csvdata.append(QString::number(vehicle.battery_status.energy_consumed));     csvdata.append(',');
            csvdata.append(QString::number(vehicle.battery_status.temperature));         csvdata.append(',');
            csvdata.append(QString::number(vehicle.battery_status.voltages[0]));         csvdata.append(',');
            csvdata.append(QString::number(vehicle.battery_status.voltages[1]));         csvdata.append(',');
            csvdata.append(QString::number(vehicle.battery_status.voltages[2]));         csvdata.append(',');
            csvdata.append(QString::number(vehicle.battery_status.voltages[3]));         csvdata.append(',');
            csvdata.append(QString::number(vehicle.battery_status.voltages[4]));         csvdata.append(',');
            csvdata.append(QString::number(vehicle.battery_status.voltages[5]));         csvdata.append(',');
            csvdata.append(QString::number(vehicle.battery_status.voltages[6]));         csvdata.append(',');
            csvdata.append(QString::number(vehicle.battery_status.voltages[7]));         csvdata.append(',');
            csvdata.append(QString::number(vehicle.battery_status.voltages[8]));         csvdata.append(',');
            csvdata.append(QString::number(vehicle.battery_status.voltages[9]));         csvdata.append(',');
            csvdata.append(QString::number(vehicle.battery_status.current_battery));     csvdata.append(',');
            csvdata.append(QString::number(vehicle.battery_status.id));                  csvdata.append(',');
            csvdata.append(QString::number(vehicle.battery_status.battery_function));    csvdata.append(',');
            csvdata.append(QString::number(vehicle.battery_status.type));                csvdata.append(',');
            csvdata.append(QString::number(vehicle.battery_status.battery_remaining));   csvdata.append(',');
            csvdata.append(QString::number(vehicle.battery_status.time_remaining));      csvdata.append(',');
            csvdata.append(QString::number(vehicle.battery_status.charge_state));        csvdata.append(',');
            csvdata.append(QString::number(vehicle.battery_status.voltages_ext[0]));     csvdata.append(',');
            csvdata.append(QString::number(vehicle.battery_status.voltages_ext[1]));     csvdata.append(',');
            csvdata.append(QString::number(vehicle.battery_status.voltages_ext[2]));     csvdata.append(',');
            csvdata.append(QString::number(vehicle.battery_status.voltages_ext[3]));     csvdata.append('\n');

            setFileData(battery_status_file,csvdata);
         }break;
         case MAVLINK_MSG_ID_VIBRATION: {
            mavlink_msg_vibration_decode(&msg,&vehicle.vibration);

            QByteArray csvdata;
            csvdata.clear();



            setFileData(vibration_file,csvdata);
         }break;
         case MAVLINK_MSG_ID_EngineState: {
            mavlink_msg_enginestate_decode(&msg,&vehicle.enginestate);

            QByteArray csvdata;
            csvdata.clear();

            csvdata.append(QString::number(gpsTimer.year));                             csvdata.append(',');
            csvdata.append(QString::number(gpsTimer.mon));                              csvdata.append(',');
            csvdata.append(QString::number(gpsTimer.day));                              csvdata.append(',');
            csvdata.append(QString::number(gpsTimer.hour));                             csvdata.append(',');
            csvdata.append(QString::number(gpsTimer.min));                              csvdata.append(',');
            csvdata.append(QString::number(gpsTimer.sec));                              csvdata.append(',');
            csvdata.append(QString::number(gpsTimer.ms));                               csvdata.append(',');
            csvdata.append(QString::number(vehicle.enginestate.time_boot_ms));          csvdata.append(',');
            csvdata.append(QString::number(vehicle.enginestate.ChokeFlag));             csvdata.append(',');
            csvdata.append(QString::number(vehicle.enginestate.Ignition2Flag));         csvdata.append(',');
            csvdata.append(QString::number(vehicle.enginestate.AmbientTemperatur));     csvdata.append(',');
            csvdata.append(QString::number(vehicle.enginestate.AirPressure));           csvdata.append(',');
            csvdata.append(QString::number(vehicle.enginestate.ActualFuelPressure));    csvdata.append(',');
            csvdata.append(QString::number(vehicle.enginestate.FuelPumpDutyCycle));     csvdata.append(',');
            csvdata.append(QString::number(vehicle.enginestate.ActualJet1DutyCycle));   csvdata.append(',');
            csvdata.append(QString::number(vehicle.enginestate.ActualRPM));             csvdata.append(',');
            csvdata.append(QString::number(vehicle.enginestate.CHTemperature1));        csvdata.append(',');
            csvdata.append(QString::number(vehicle.enginestate.counts));                csvdata.append('\n');

            setFileData(enginestate_file,csvdata);
         }break;
         case MAVLINK_MSG_ID_VFR_HUD: {
            mavlink_msg_vfr_hud_decode(&msg,&vehicle.vfr_hud);

            QByteArray csvdata;
            csvdata.clear();

            csvdata.append(QString::number(gpsTimer.year));                            csvdata.append(',');
            csvdata.append(QString::number(gpsTimer.mon));                             csvdata.append(',');
            csvdata.append(QString::number(gpsTimer.day));                             csvdata.append(',');
            csvdata.append(QString::number(gpsTimer.hour));                            csvdata.append(',');
            csvdata.append(QString::number(gpsTimer.min));                             csvdata.append(',');
            csvdata.append(QString::number(gpsTimer.sec));                             csvdata.append(',');
            csvdata.append(QString::number(gpsTimer.ms));                              csvdata.append(',');
            csvdata.append(QString::number(vehicle.vfr_hud.airspeed));                 csvdata.append(',');
            csvdata.append(QString::number(vehicle.vfr_hud.groundspeed));              csvdata.append(',');
            csvdata.append(QString::number(vehicle.vfr_hud.alt));                      csvdata.append(',');
            csvdata.append(QString::number(vehicle.vfr_hud.climb));                    csvdata.append(',');
            csvdata.append(QString::number(vehicle.vfr_hud.heading));                  csvdata.append(',');
            csvdata.append(QString::number(vehicle.vfr_hud.throttle));                 csvdata.append('\n');

            setFileData(vfr_hud_file,csvdata);
         }break;
         case MAVLINK_MSG_ID_EMB_ATMO_COM: {
            mavlink_msg_emb_atmo_com_decode(&msg,&vehicle.emb_atom_com);

            QByteArray csvdata;
            csvdata.clear();

            csvdata.append(QString::number(gpsTimer.year));                        csvdata.append(',');
            csvdata.append(QString::number(gpsTimer.mon));                         csvdata.append(',');
            csvdata.append(QString::number(gpsTimer.day));                         csvdata.append(',');
            csvdata.append(QString::number(gpsTimer.hour));                        csvdata.append(',');
            csvdata.append(QString::number(gpsTimer.min));                         csvdata.append(',');
            csvdata.append(QString::number(gpsTimer.sec));                         csvdata.append(',');
            csvdata.append(QString::number(gpsTimer.ms));                          csvdata.append(',');
            csvdata.append(QString::number(vehicle.emb_atom_com.time_boot_ms));    csvdata.append(',');
            csvdata.append(QString::number(vehicle.emb_atom_com.Airspeed));        csvdata.append(',');
            csvdata.append(QString::number(vehicle.emb_atom_com.beta));            csvdata.append(',');
            csvdata.append(QString::number(vehicle.emb_atom_com.alpha));           csvdata.append(',');
            csvdata.append(QString::number(vehicle.emb_atom_com.ps));              csvdata.append(',');
            csvdata.append(QString::number(vehicle.emb_atom_com.qbar));            csvdata.append(',');
            csvdata.append(QString::number(vehicle.emb_atom_com.seq));             csvdata.append(',');
            csvdata.append(QString::number(vehicle.emb_atom_com.mach));            csvdata.append('\n');

            setFileData(emb_atom_com_file,csvdata);




            QDateTime *timebase = new QDateTime();


            timebase->setDate(QDate(gpsTimer.year,gpsTimer.mon,gpsTimer.day));
            timebase->setTime(QTime(gpsTimer.hour,gpsTimer.min,gpsTimer.sec));

            emit setMa(timebase->toSecsSinceEpoch(), vehicle.emb_atom_com.mach);

            delete timebase;

         }break;
         case MAVLINK_MSG_ID_TurbineState: {
            mavlink_msg_turbinestate_decode(&msg,&vehicle.turbinstate);

            QByteArray csvdata;
            csvdata.clear();

            csvdata.append(QString::number(gpsTimer.year));                       csvdata.append(',');
            csvdata.append(QString::number(gpsTimer.mon));                        csvdata.append(',');
            csvdata.append(QString::number(gpsTimer.day));                        csvdata.append(',');
            csvdata.append(QString::number(gpsTimer.hour));                       csvdata.append(',');
            csvdata.append(QString::number(gpsTimer.min));                        csvdata.append(',');
            csvdata.append(QString::number(gpsTimer.sec));                        csvdata.append(',');
            csvdata.append(QString::number(gpsTimer.ms));                         csvdata.append(',');
            csvdata.append(QString::number(vehicle.turbinstate.time_boot_ms));    csvdata.append(',');
            csvdata.append(QString::number(vehicle.turbinstate.RPM_mea));         csvdata.append(',');
            csvdata.append(QString::number(vehicle.turbinstate.T5));              csvdata.append(',');
            csvdata.append(QString::number(vehicle.turbinstate.Kfuel));           csvdata.append(',');
            csvdata.append(QString::number(vehicle.turbinstate.RPM_des));         csvdata.append(',');
            csvdata.append(QString::number(vehicle.turbinstate.RPM_des_ap));      csvdata.append(',');
            csvdata.append(QString::number(vehicle.turbinstate.RPM_bak));         csvdata.append(',');
            csvdata.append(QString::number(vehicle.turbinstate.IOState));         csvdata.append(',');
            csvdata.append(QString::number(vehicle.turbinstate.SysState));        csvdata.append(',');
            csvdata.append(QString::number(vehicle.turbinstate.Fault));           csvdata.append(',');
            csvdata.append(QString::number(vehicle.turbinstate.stage_ap));        csvdata.append(',');
            csvdata.append(QString::number(vehicle.turbinstate.temp_ap));         csvdata.append(',');
            csvdata.append(QString::number(vehicle.turbinstate.tas_ap));          csvdata.append(',');
            csvdata.append(QString::number(vehicle.turbinstate.asl_ap));          csvdata.append(',');
            csvdata.append(QString::number(vehicle.turbinstate.KabMain));         csvdata.append(',');
            csvdata.append(QString::number(vehicle.turbinstate.KabFire));         csvdata.append(',');
            csvdata.append(QString::number(vehicle.turbinstate.KDj));             csvdata.append(',');
            csvdata.append(QString::number(vehicle.turbinstate.T1t));             csvdata.append(',');
            csvdata.append(QString::number(vehicle.turbinstate.P1t));             csvdata.append(',');
            csvdata.append(QString::number(vehicle.turbinstate.P3t));             csvdata.append(',');
            csvdata.append(QString::number(vehicle.turbinstate.P5t));             csvdata.append(',');
            csvdata.append(QString::number(vehicle.turbinstate.DJS));             csvdata.append(',');
            csvdata.append(QString::number(vehicle.turbinstate.Vcc));             csvdata.append(',');
            csvdata.append(QString::number(vehicle.turbinstate.Tbak));            csvdata.append(',');
            csvdata.append(QString::number(vehicle.turbinstate.rev));             csvdata.append(',');
            csvdata.append(QString::number(vehicle.turbinstate.CFuelMode));       csvdata.append(',');
            csvdata.append(QString::number(vehicle.turbinstate.Cmd));             csvdata.append('\n');

            setFileData(turbinstate_file,csvdata);
         }break;
         case MAVLINK_MSG_ID_BMUState: {
            mavlink_msg_bmustate_decode(&msg,&vehicle.bmustate);

            QByteArray csvdata;
            csvdata.clear();

            csvdata.append(QString::number(gpsTimer.year));                          csvdata.append(',');
            csvdata.append(QString::number(gpsTimer.mon));                           csvdata.append(',');
            csvdata.append(QString::number(gpsTimer.day));                           csvdata.append(',');
            csvdata.append(QString::number(gpsTimer.hour));                          csvdata.append(',');
            csvdata.append(QString::number(gpsTimer.min));                           csvdata.append(',');
            csvdata.append(QString::number(gpsTimer.sec));                           csvdata.append(',');
            csvdata.append(QString::number(gpsTimer.ms));                            csvdata.append(',');
            csvdata.append(QString::number(vehicle.bmustate.time_boot_ms));          csvdata.append(',');
            csvdata.append(QString::number(vehicle.bmustate.BAT1_group_voltage_mv)); csvdata.append(',');
            csvdata.append(QString::number(vehicle.bmustate.BAT1_group_current_dA)); csvdata.append(',');
            csvdata.append(QString::number(vehicle.bmustate.BAT1_remain_perc));      csvdata.append(',');
            csvdata.append(QString::number(vehicle.bmustate.BAT1_low_temp_degC));    csvdata.append(',');
            csvdata.append(QString::number(vehicle.bmustate.BAT1_voltages_mv[0]));   csvdata.append(',');
            csvdata.append(QString::number(vehicle.bmustate.BAT1_hi_voltage_mv));    csvdata.append(',');
            csvdata.append(QString::number(vehicle.bmustate.BAT1_low_voltage_mv));   csvdata.append(',');
            csvdata.append(QString::number(vehicle.bmustate.BAT2_group_voltage_mv)); csvdata.append(',');
            csvdata.append(QString::number(vehicle.bmustate.BAT2_group_current_dA)); csvdata.append(',');
            csvdata.append(QString::number(vehicle.bmustate.BAT2_remain_perc));      csvdata.append(',');
            csvdata.append(QString::number(vehicle.bmustate.BAT2_low_temp_degC));    csvdata.append(',');
            csvdata.append(QString::number(vehicle.bmustate.BAT2_hi_temp_degC));     csvdata.append(',');
            csvdata.append(QString::number(vehicle.bmustate.BAT2_voltages_mv[0]));   csvdata.append(',');
            csvdata.append(QString::number(vehicle.bmustate.BAT2_hi_voltage_mv));    csvdata.append(',');
            csvdata.append(QString::number(vehicle.bmustate.BAT2_low_voltage_mv));   csvdata.append(',');
            csvdata.append(QString::number(vehicle.bmustate.BAT1_STA1));             csvdata.append(',');
            csvdata.append(QString::number(vehicle.bmustate.BAT1_STA2));             csvdata.append(',');
            csvdata.append(QString::number(vehicle.bmustate.BAT2_STA1));             csvdata.append(',');
            csvdata.append(QString::number(vehicle.bmustate.BAT2_STA2));             csvdata.append(',');
            csvdata.append(QString::number(vehicle.bmustate.p500w_enabled));         csvdata.append('\n');

            setFileData(bmustate_file,csvdata);
         }break;
         case MAVLINK_MSG_ID_CCMState: {
            mavlink_msg_ccmstate_decode(&msg,&vehicle.ccmstate);

            QByteArray csvdata;
            csvdata.clear();

            csvdata.append(QString::number(gpsTimer.year));                  csvdata.append(',');
            csvdata.append(QString::number(gpsTimer.mon));                   csvdata.append(',');
            csvdata.append(QString::number(gpsTimer.day));                   csvdata.append(',');
            csvdata.append(QString::number(gpsTimer.hour));                  csvdata.append(',');
            csvdata.append(QString::number(gpsTimer.min));                   csvdata.append(',');
            csvdata.append(QString::number(gpsTimer.sec));                   csvdata.append(',');
            csvdata.append(QString::number(gpsTimer.ms));                    csvdata.append(',');
            csvdata.append(QString::number(vehicle.ccmstate.time_boot_ms));  csvdata.append(',');
            csvdata.append(QString::number(vehicle.ccmstate.fuel_level));    csvdata.append(',');
            csvdata.append(QString::number(vehicle.ccmstate.temp[0]));       csvdata.append(',');
            csvdata.append(QString::number(vehicle.ccmstate.temp[1]));       csvdata.append(',');
            csvdata.append(QString::number(vehicle.ccmstate.temp[2]));       csvdata.append(',');
            csvdata.append(QString::number(vehicle.ccmstate.temp[3]));       csvdata.append(',');
            csvdata.append(QString::number(vehicle.ccmstate.volts[0]));      csvdata.append(',');
            csvdata.append(QString::number(vehicle.ccmstate.volts[1]));      csvdata.append(',');
            csvdata.append(QString::number(vehicle.ccmstate.volts[2]));      csvdata.append(',');
            csvdata.append(QString::number(vehicle.ccmstate.volts[3]));      csvdata.append(',');
            csvdata.append(QString::number(vehicle.ccmstate.echo_seq));      csvdata.append('\n');

            setFileData(ccmstate_file,csvdata);
         }break;
        case MAVLINK_MSG_ID_UVWDState : {
            mavlink_msg_uvwdstate_decode(&msg,&vehicle.uvwdstate);
        }break;
        case MAVLINK_MSG_ID_RWRState : {
            mavlink_msg_rwrstate_decode(&msg,&vehicle.rwrstate);
        }break;
        case MAVLINK_MSG_ID_DLSState : {
            mavlink_msg_dlsstate_decode(&msg,&vehicle.dlsstate);
        }break;
        case MAVLINK_MSG_ID_PayloadAlarmState : {
            mavlink_msg_payloadalarmstate_decode(&msg,&vehicle.payloadalarmstate);

           // qDebug() << "recieve payload data";



        }break;
        case MAVLINK_MSG_ID_ENCAPSULATED_DATA: {

           mavlink_encapsulated_data_t encapsulated_data;

           mavlink_msg_encapsulated_data_decode(&msg,&encapsulated_data);

           QByteArray data;

           for (int var = 0; var < 253; ++var) {
               data.push_back(encapsulated_data.data[var]);
           }

           emit enCapData(encapsulated_data.seqnr,data);


        }break;

        }




        vehicleList.insert(msg.sysid,vehicle);//直接覆盖

        //emit signal_vehicle(vehicle);

        static qint64 frq_time = 0;
        if((QDateTime::currentMSecsSinceEpoch() - frq_time) >= 200)
        {
            emit state_updated();
            frq_time = QDateTime::currentMSecsSinceEpoch();
        }
}


void MavLinkNode::CommandParse(mavlink_message_t msg)
{
    switch (msg.msgid) {
        case MAVLINK_MSG_ID_COMMAND_ACK: {
           mavlink_command_ack_t ack;
           mavlink_msg_command_ack_decode(&msg,&ack);
        }break;
    }
}


void MavLinkNode::CheckVehicle(int sysid,int compid)
{
     if(!vehicleList.contains(sysid))
     {
         _vehicle v;

         v.sysid = sysid;
         v.compid = compid;

         vehicleList.insert(sysid,v);

         emit showMessage(tr("出现新的设备,识别号为 %1").arg(v.sysid));
         emit addVehicles(sysid,compid);
     }
}

void MavLinkNode::setCurrentSelected(int sysid,int compid)
{
   //qDebug() << "CurrentSelected" << sysid << compid;

   Current_sysID = sysid;
   Current_CompID = compid;

   emit setCurrentID(sysid,compid);

}

void MavLinkNode::readPendingDatagramsReplay(void)
{
    if(replay)
    {
        QByteArray datagram = replay->readAll();
        setbuff(SourceType::c_sock,datagram);
    }
}

void MavLinkNode::setLogfile(QString file)
{
    if(replay)
    {
       replay->setLogfile(file);
    }
}

//发送函数
void MavLinkNode::setHeartbeat(QVariant state,QVariant frq)
{
     //给指令赋值

     //开启线程开始传输
     if(state.toBool() == true)
     {
         isSendHeartBeat = true;//发送模式
     }
     else
     {
         isSendHeartBeat = false;//空闲模式
     }

     heartbeatFrq = frq.toDouble();
}

void MavLinkNode::heartbeat(uint32_t custom_mode,uint8_t type,uint8_t autopilot,uint8_t base_mode,uint8_t system_status,uint8_t mavlink_version)
{
    static mavlink_message_t msg;
    static mavlink_heartbeat_t heartbeat;

    heartbeat.custom_mode = custom_mode;
    heartbeat.type = type;

    heartbeat.autopilot = autopilot;
    heartbeat.base_mode = base_mode;
    heartbeat.system_status = system_status;
    heartbeat.mavlink_version = mavlink_version;

    mavlink_msg_heartbeat_encode(GCS_SysID,GCS_CompID, &msg,&heartbeat);
    Send(msg);
}

void MavLinkNode::showInfoTimerTimeout()
{
    infoExport(info);
}

void MavLinkNode::Transmit(QString msg)
{

    SerialData.clear();
    SerialData.append(msg);
    isSendTerminal = true;

}

void MavLinkNode::serial_control(void)
{
   static mavlink_message_t msg;
   static mavlink_serial_control_t serial_control;

   serial_control.baudrate = 115200;
   serial_control.count = SerialData.size();

   memcpy(serial_control.data,SerialData.toLocal8Bit().data(),SerialData.size());

   serial_control.flags = SERIAL_CONTROL_FLAG_EXCLUSIVE | SERIAL_CONTROL_FLAG_RESPOND | SERIAL_CONTROL_FLAG_MULTI;
   serial_control.device = SERIAL_CONTROL_DEV_SHELL;
   serial_control.timeout = 5000;

   mavlink_msg_serial_control_encode(GCS_SysID,GCS_CompID, &msg,&serial_control);
   Send(msg);


}


void MavLinkNode::infoExport(_showinfo info)
{
    uint8_t buff[250];
    int dataCount = 0;

    // union {uint8_t B[4];uint16_t D[2];int32_t H;}src;
    union {uint8_t B[4];int16_t D[2];int32_t H;}src;


#if 0
    src.D[0] = info.flag;
    buff[dataCount++] = src.B[1];
    buff[dataCount++] = src.B[0];

    src.D[0] = info.id;
    buff[dataCount++] = src.B[1];
    buff[dataCount++] = src.B[0];

    src.H = info.time;
    buff[dataCount++] = src.B[3];
    buff[dataCount++] = src.B[2];
    buff[dataCount++] = src.B[1];
    buff[dataCount++] = src.B[0];

    src.H = info.lng;
    buff[dataCount++] = src.B[3];
    buff[dataCount++] = src.B[2];
    buff[dataCount++] = src.B[1];
    buff[dataCount++] = src.B[0];

    src.H = info.lat;
    buff[dataCount++] = src.B[3];
    buff[dataCount++] = src.B[2];
    buff[dataCount++] = src.B[1];
    buff[dataCount++] = src.B[0];

    src.H = info.alt;
    buff[dataCount++] = src.B[3];
    buff[dataCount++] = src.B[2];
    buff[dataCount++] = src.B[1];
    buff[dataCount++] = src.B[0];

    src.H = info.ve;
    buff[dataCount++] = src.B[3];
    buff[dataCount++] = src.B[2];
    buff[dataCount++] = src.B[1];
    buff[dataCount++] = src.B[0];

    src.H = info.vn;
    buff[dataCount++] = src.B[3];
    buff[dataCount++] = src.B[2];
    buff[dataCount++] = src.B[1];
    buff[dataCount++] = src.B[0];

    src.H = info.vu;
    buff[dataCount++] = src.B[3];
    buff[dataCount++] = src.B[2];
    buff[dataCount++] = src.B[1];
    buff[dataCount++] = src.B[0];

    src.H = info.v;
    buff[dataCount++] = src.B[3];
    buff[dataCount++] = src.B[2];
    buff[dataCount++] = src.B[1];
    buff[dataCount++] = src.B[0];

    src.H = info.course;
    buff[dataCount++] = src.B[3];
    buff[dataCount++] = src.B[2];
    buff[dataCount++] = src.B[1];
    buff[dataCount++] = src.B[0];

    src.H = 0;
    buff[dataCount++] = src.B[3];
    buff[dataCount++] = src.B[2];
    buff[dataCount++] = src.B[1];
    buff[dataCount++] = src.B[0];

    src.H = 0;
    buff[dataCount++] = src.B[3];
    buff[dataCount++] = src.B[2];
    buff[dataCount++] = src.B[1];
    buff[dataCount++] = src.B[0];
#else
    // src.D[0] = info.flag;
    // buff[dataCount++] = src.B[0];
    // buff[dataCount++] = src.B[1];

    // src.D[0] = info.id;
    // buff[dataCount++] = src.B[0];
    // buff[dataCount++] = src.B[1];

    // src.H = info.time;
    // buff[dataCount++] = src.B[0];
    // buff[dataCount++] = src.B[1];
    // buff[dataCount++] = src.B[2];
    // buff[dataCount++] = src.B[3];

    // src.H = info.lng;
    // buff[dataCount++] = src.B[0];
    // buff[dataCount++] = src.B[1];
    // buff[dataCount++] = src.B[2];
    // buff[dataCount++] = src.B[3];

    // src.H = info.lat;
    // buff[dataCount++] = src.B[0];
    // buff[dataCount++] = src.B[1];
    // buff[dataCount++] = src.B[2];
    // buff[dataCount++] = src.B[3];

    // src.H = info.alt;
    // buff[dataCount++] = src.B[0];
    // buff[dataCount++] = src.B[1];
    // buff[dataCount++] = src.B[2];
    // buff[dataCount++] = src.B[3];

    // src.H = info.ve;
    // buff[dataCount++] = src.B[0];
    // buff[dataCount++] = src.B[1];
    // buff[dataCount++] = src.B[2];
    // buff[dataCount++] = src.B[3];

    // src.H = info.vn;
    // buff[dataCount++] = src.B[0];
    // buff[dataCount++] = src.B[1];
    // buff[dataCount++] = src.B[2];
    // buff[dataCount++] = src.B[3];

    // src.H = info.vu;
    // buff[dataCount++] = src.B[0];
    // buff[dataCount++] = src.B[1];
    // buff[dataCount++] = src.B[2];
    // buff[dataCount++] = src.B[3];

    // src.H = info.v;
    // buff[dataCount++] = src.B[0];
    // buff[dataCount++] = src.B[1];
    // buff[dataCount++] = src.B[2];
    // buff[dataCount++] = src.B[3];

    // src.H = info.course;
    // buff[dataCount++] = src.B[0];
    // buff[dataCount++] = src.B[1];
    // buff[dataCount++] = src.B[2];
    // buff[dataCount++] = src.B[3];

    // src.H = 0;
    // buff[dataCount++] = src.B[0];
    // buff[dataCount++] = src.B[1];
    // buff[dataCount++] = src.B[2];
    // buff[dataCount++] = src.B[3];

    // src.H = 0;
    // buff[dataCount++] = src.B[0];
    // buff[dataCount++] = src.B[1];
    // buff[dataCount++] = src.B[2];
    // buff[dataCount++] = src.B[3];
#endif

    src.D[0] = info.flag;
    buff[dataCount++] = src.B[0];
    buff[dataCount++] = src.B[1];

    src.H = info.time;
    buff[dataCount++] = src.B[0];
    buff[dataCount++] = src.B[1];
    buff[dataCount++] = src.B[2];
    buff[dataCount++] = src.B[3];

    src.D[0] = info.num;
    buff[dataCount++] = src.B[0];
    buff[dataCount++] = src.B[1];

    src.D[0] = info.id;
    buff[dataCount++] = src.B[0];
    buff[dataCount++] = src.B[1];

    src.D[0] = info.t;
    buff[dataCount++] = src.B[0];
    buff[dataCount++] = src.B[1];

    src.H = info.pe;
    buff[dataCount++] = src.B[0];
    buff[dataCount++] = src.B[1];
    buff[dataCount++] = src.B[2];
    buff[dataCount++] = src.B[3];

    src.H = info.pu;
    buff[dataCount++] = src.B[0];
    buff[dataCount++] = src.B[1];
    buff[dataCount++] = src.B[2];
    buff[dataCount++] = src.B[3];

    src.H = info.ps;
    buff[dataCount++] = src.B[0];
    buff[dataCount++] = src.B[1];
    buff[dataCount++] = src.B[2];
    buff[dataCount++] = src.B[3];

    src.H = info.ve;
    buff[dataCount++] = src.B[0];
    buff[dataCount++] = src.B[1];
    buff[dataCount++] = src.B[2];
    buff[dataCount++] = src.B[3];

    src.H = info.vu;
    buff[dataCount++] = src.B[0];
    buff[dataCount++] = src.B[1];
    buff[dataCount++] = src.B[2];
    buff[dataCount++] = src.B[3];

    src.H = info.vs;
    buff[dataCount++] = src.B[0];
    buff[dataCount++] = src.B[1];
    buff[dataCount++] = src.B[2];
    buff[dataCount++] = src.B[3];

    src.H = info.vt;
    buff[dataCount++] = src.B[0];
    buff[dataCount++] = src.B[1];
    buff[dataCount++] = src.B[2];
    buff[dataCount++] = src.B[3];

    src.H = info.rcs;
    buff[dataCount++] = src.B[0];
    buff[dataCount++] = src.B[1];
    buff[dataCount++] = src.B[2];
    buff[dataCount++] = src.B[3];

    src.H = info.reserve;
    buff[dataCount++] = src.B[0];
    buff[dataCount++] = src.B[1];
    buff[dataCount++] = src.B[2];
    buff[dataCount++] = src.B[3];

    emit SendMessageToExport(0,buff,dataCount);
}

void MavLinkNode::setReferencePoint(double lon, double lat, double h)
{
    refPoint.lon = lon;
    refPoint.lat = lat;
    refPoint.hight = h;
}