1879 lines
88 KiB
C++
1879 lines
88 KiB
C++
#include "mavlinknode.h"
|
|
#include "QElapsedTimer"
|
|
/*
|
|
* mavlink 解析相关内容请参考如下
|
|
* https://mavlink.io/en/services/command.html
|
|
* 包含了各个函数、参数、状态机等及其说明
|
|
**/
|
|
|
|
|
|
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/%1.tlog").arg(current.toString("yyyyMMddHHmmss"));
|
|
mavLogFile = new QFile(mavlogFileName);
|
|
mavLogFile->open(QIODevice::WriteOnly);
|
|
|
|
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;
|
|
|
|
}
|
|
|
|
MavLinkNode::~MavLinkNode()
|
|
{
|
|
//CreateCSV();
|
|
|
|
CloseCSV();
|
|
|
|
if (mavLogFile)
|
|
{
|
|
mavLogFile->close();
|
|
delete mavLogFile;
|
|
mavLogFile = 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);
|
|
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)
|
|
{
|
|
|
|
//QApplication::processEvents();
|
|
|
|
|
|
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);
|
|
|
|
|
|
}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);
|
|
|
|
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;
|
|
|
|
|
|
|
|
|
|
|
|
}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);
|
|
|
|
}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);
|
|
|
|
}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);
|
|
|
|
|
|
}
|
|
|