#include "mavlinknode.h" #include "QElapsedTimer" /* * mavlink 解析相关内容请参考如下 * https://mavlink.io/en/services/command.html * 包含了各个函数、参数、状态机等及其说明 **/ 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(&showInfoTimer, &QTimer::timeout, this, [this]() { infoExport(info); }); showInfoTimer.start(200); } MavLinkNode::~MavLinkNode() { //CreateCSV(); 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::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(); } } 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"; emit showMessage("协议解码校验错误"); }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 = 0x24a8; info.id = infoExportID;//1 //info.time = (int32_t)vehicle.ins1.time;//(second+min*60+hour*3600)*1000 info.time = (int32_t)(sec+min*60+hour*3600)*1000; info.lng = vehicle.ins1.lon * 10000000;//*10000000 info.lat = vehicle.ins1.lat * 10000000;//*10000000 info.alt = vehicle.ins1.alt * 100;//*100 info.ve = vehicle.ins1.v_east * 100;//*100 info.vn = vehicle.ins1.v_north * 100;//*100 info.vu = vehicle.ins1.v_up * 100;//*100 info.v = sqrt(vehicle.ins1.v_east * vehicle.ins1.v_east + vehicle.ins1.v_north * vehicle.ins1.v_north + vehicle.ins1.v_up * vehicle.ins1.v_up) * 100;//*100 info.course = to360deg(vehicle.ins1.yaw * 57.3) * 100;//*100 //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 = 0x24a8; 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 //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; } 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::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; #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 emit SendMessageToExport(0,buff,dataCount); }