1220 lines
56 KiB
C++
1220 lines
56 KiB
C++
#include "mavlinknode.h"
|
|
|
|
/*
|
|
* 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(10);//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();
|
|
|
|
}
|
|
|
|
MavLinkNode::~MavLinkNode()
|
|
{
|
|
CreateCSV();
|
|
|
|
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_file.csv");
|
|
autopilot_version_file->open(QIODevice::WriteOnly);
|
|
|
|
sys_status_file = new QFile(filetime + "sys_status_file.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_file.csv");
|
|
ping_file->open(QIODevice::WriteOnly);
|
|
|
|
attitude_file = new QFile(filetime + "attitude_file.csv");
|
|
attitude_file->open(QIODevice::WriteOnly);
|
|
|
|
ins1_file = new QFile(filetime + "ins1_file.csv");
|
|
ins1_file->open(QIODevice::WriteOnly);
|
|
|
|
ins2_file = new QFile(filetime + "ins2_file.csv");
|
|
ins2_file->open(QIODevice::WriteOnly);
|
|
|
|
gps_raw_int_file = new QFile(filetime + "gps_raw_int_file.csv");
|
|
gps_raw_int_file->open(QIODevice::WriteOnly);
|
|
|
|
global_position_int_file = new QFile(filetime + "global_position_int_file.csv");
|
|
global_position_int_file->open(QIODevice::WriteOnly);
|
|
|
|
servo_output_raw_file = new QFile(filetime + "servo_output_raw_file.csv");
|
|
servo_output_raw_file->open(QIODevice::WriteOnly);
|
|
|
|
rc_channels_raw_file = new QFile(filetime + "rc_channels_raw_file.csv");
|
|
rc_channels_raw_file->open(QIODevice::WriteOnly);
|
|
|
|
nav_controller_output_file = new QFile(filetime + "nav_controller_output_file.csv");
|
|
nav_controller_output_file->open(QIODevice::WriteOnly);
|
|
|
|
airspeed_autocal_file = new QFile(filetime + "airspeed_autocal_file.csv");
|
|
airspeed_autocal_file->open(QIODevice::WriteOnly);
|
|
|
|
rpm_file = new QFile(filetime + "rpm_file.csv");
|
|
rpm_file->open(QIODevice::WriteOnly);
|
|
|
|
scaled_pressure_file = new QFile(filetime + "scaled_pressure_file.csv");
|
|
scaled_pressure_file->open(QIODevice::WriteOnly);
|
|
|
|
extended_sys_state_file = new QFile(filetime + "extended_sys_state_file.csv");
|
|
extended_sys_state_file->open(QIODevice::WriteOnly);
|
|
|
|
battery_status_file = new QFile(filetime + "battery_status_file.csv");
|
|
battery_status_file->open(QIODevice::WriteOnly);
|
|
|
|
vibration_file = new QFile(filetime + "vibration_file.csv");
|
|
vibration_file->open(QIODevice::WriteOnly);
|
|
|
|
enginestate_file = new QFile(filetime + "enginestate_file.csv");
|
|
enginestate_file->open(QIODevice::WriteOnly);
|
|
|
|
vfr_hud_file = new QFile(filetime + "vfr_hud_file.csv");
|
|
vfr_hud_file->open(QIODevice::WriteOnly);
|
|
|
|
aoa_ssa_file = new QFile(filetime + "aoa_ssa_file.csv");
|
|
aoa_ssa_file->open(QIODevice::WriteOnly);
|
|
|
|
emb_atom_com_file = new QFile(filetime + "emb_atom_com_file.csv");
|
|
emb_atom_com_file->open(QIODevice::WriteOnly);
|
|
|
|
turbinstate_file = new QFile(filetime + "turbinstate_file.csv");
|
|
turbinstate_file->open(QIODevice::WriteOnly);
|
|
|
|
bmustate_file = new QFile(filetime + "bmustate_file.csv");
|
|
bmustate_file->open(QIODevice::WriteOnly);
|
|
|
|
ccmstate_file = new QFile(filetime + "ccmstate_file.csv");
|
|
ccmstate_file->open(QIODevice::WriteOnly);
|
|
|
|
serial_control_file = new QFile(filetime + "serial_control_file.csv");
|
|
serial_control_file->open(QIODevice::WriteOnly);
|
|
|
|
|
|
autopilot_version_file->write(autopilot_version_csv);
|
|
sys_status_file->write(sys_status_csv);
|
|
heartbeat_file->write(heartbeat_csv);
|
|
ping_file->write(ping_csv);
|
|
attitude_file->write(attitude_csv);
|
|
ins1_file->write(ins1_csv);
|
|
ins2_file->write(ins2_csv);
|
|
gps_raw_int_file->write(gps_raw_int_csv);
|
|
global_position_int_file->write(global_position_int_csv);
|
|
|
|
for(int i = 0;i < 10;i++)
|
|
{
|
|
servo_output_raw_file->write(servo_output_raw_csv[i]);
|
|
}
|
|
rc_channels_raw_file->write(rc_channels_raw_csv);
|
|
nav_controller_output_file->write(nav_controller_output_csv);
|
|
airspeed_autocal_file->write(airspeed_autocal_csv);
|
|
rpm_file->write(rpm_csv);
|
|
scaled_pressure_file->write(scaled_pressure_csv);
|
|
extended_sys_state_file->write(extended_sys_state_csv);
|
|
battery_status_file->write(battery_status_csv);
|
|
vibration_file->write(vibration_csv);
|
|
enginestate_file->write(enginestate_csv);
|
|
vfr_hud_file->write(vfr_hud_csv);
|
|
aoa_ssa_file->write(aoa_ssa_csv);
|
|
emb_atom_com_file->write(emb_atom_com_csv);
|
|
turbinstate_file->write(turbinstate_csv);
|
|
bmustate_file->write(bmustate_csv);
|
|
ccmstate_file->write(ccmstate_csv);
|
|
serial_control_file->write(serial_control_csv);
|
|
|
|
autopilot_version_file->close();
|
|
sys_status_file->close();
|
|
heartbeat_file->close();
|
|
ping_file->close();
|
|
attitude_file->close();
|
|
ins1_file->close();
|
|
ins2_file->close();
|
|
gps_raw_int_file->close();
|
|
global_position_int_file->close();
|
|
servo_output_raw_file->close();
|
|
rc_channels_raw_file->close();
|
|
nav_controller_output_file->close();
|
|
airspeed_autocal_file->close();
|
|
rpm_file->close();
|
|
scaled_pressure_file->close();
|
|
extended_sys_state_file->close();
|
|
battery_status_file->close();
|
|
vibration_file->close();
|
|
enginestate_file->close();
|
|
vfr_hud_file->close();
|
|
aoa_ssa_file->close();
|
|
emb_atom_com_file->close();
|
|
turbinstate_file->close();
|
|
bmustate_file->close();
|
|
ccmstate_file->close();
|
|
serial_control_file->close();
|
|
|
|
}
|
|
|
|
|
|
//这里一直在解码，一直检查双缓冲里面是否有数据，有就解码，没有就休息
|
|
void MavLinkNode::process()//线程函数
|
|
{
|
|
autopilot_version_csv.append("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");
|
|
sys_status_csv.append("present,enabled,health,load,voltage,current,remaining,drop_rate_comm,errors_comm,errors_count1,errors_count2,errors_count3,errors_count4\n");
|
|
heartbeat_csv.append("type,autopilot,base_mode,custom_mode,system_status,mavlink\n");
|
|
ping_csv;
|
|
attitude_csv.append("time_boot_ms,roll,pitch,yaw,rollspeed,pitchspeed,yawspeed\n");
|
|
ins1_csv.append("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");
|
|
ins2_csv.append("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");
|
|
gps_raw_int_csv.append("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");
|
|
global_position_int_csv.append("time_boot_ms,lat,lon,alt,relative_alt,vx,vy,vz,hdg\n");
|
|
servo_output_raw_csv[0].append("time_usec,port,ch1,ch2,ch3,ch4,ch5,ch6,ch7,ch8,ch9,ch10,ch11,ch12,ch13,ch14,ch15,ch16\n");
|
|
rc_channels_raw_csv;
|
|
nav_controller_output_csv.append("nav_roll,nav_pitch,nav_bearing,target_bearing,wp_dist,alt_err,as_err,xtrack_err\n");
|
|
airspeed_autocal_csv.append("vx,vy,vz,diff_pressure,EAS2TAS,ratio,state_x,state_y,state_z,Pax,Pby,Pcz\n");
|
|
rpm_csv.append("rpm1,rpm2,rpm3,rpm4,rpm5\n");
|
|
scaled_pressure_csv.append("time_boot_ms,press_abs,press_diff,temperature,temperature_diff\n");
|
|
extended_sys_state_csv;
|
|
battery_status_csv;
|
|
vibration_csv;
|
|
enginestate_csv;
|
|
vfr_hud_csv.append("airspeed,groundspeed,alt,climb,heading,throttle\n");
|
|
aoa_ssa_csv;
|
|
emb_atom_com_csv.append("time_boot_ms,airspeed,beta,alpha,ps,qbar,seq,mach\n");
|
|
turbinstate_csv.append("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");
|
|
bmustate_csv.append("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");
|
|
ccmstate_csv.append("time_boot_ms,fuel_level,temp[0],temp[1],temp[2],temp[3],volts[0],volts[1],volts[2],volts[3],echo_seq\n");
|
|
serial_control_csv.append("baudrate,timeout,device,flags,count,data\n");
|
|
|
|
qDebug() << "mavlinknode" << QThread::currentThreadId();
|
|
|
|
uint8_t count = 0;
|
|
QByteArray datagram = nullptr;
|
|
while (running_flag)
|
|
{
|
|
count ++;
|
|
//QThread::msleep(1000/running_frq);
|
|
//timer->start();
|
|
|
|
//解码从UDP来的
|
|
datagram.clear();
|
|
datagram = readbuff(SourceType::c_sock);//每次全部读取
|
|
if(!datagram.isEmpty())
|
|
{
|
|
//qDebug() << "client parse";
|
|
Mavlinkparse(SourceType::c_sock,datagram);
|
|
QApplication::processEvents();
|
|
}
|
|
else
|
|
{
|
|
//QThread::msleep(1000/running_frq);
|
|
QThread::yieldCurrentThread();
|
|
}
|
|
|
|
//解码从串口来的
|
|
datagram.clear();
|
|
datagram = readbuff(SourceType::s_port);//每次全部读取
|
|
//qDebug() << "serial port parse";
|
|
if(!datagram.isEmpty())
|
|
{
|
|
//qDebug() << "serial port parse";
|
|
Mavlinkparse(SourceType::s_port,datagram);
|
|
QApplication::processEvents();
|
|
}
|
|
else
|
|
{
|
|
//QThread::msleep(1000/running_frq);
|
|
QThread::yieldCurrentThread();
|
|
}
|
|
|
|
if(isInterruptionRequested())//退出
|
|
{
|
|
break;
|
|
}
|
|
//QApplication::processEvents();
|
|
}
|
|
}
|
|
|
|
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);
|
|
//}
|
|
}
|
|
*/
|
|
}
|
|
|
|
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;
|
|
|
|
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)
|
|
{
|
|
/*
|
|
mavLogFile->write((const char *)buff, len+sizeof(quint64));
|
|
mavLogFile->flush();
|
|
*/
|
|
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);
|
|
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;
|
|
|
|
switch (msg.msgid) {
|
|
case MAVLINK_MSG_ID_AUTOPILOT_VERSION: {
|
|
mavlink_msg_autopilot_version_decode(&msg,&vehicle.autopilot_version);
|
|
}break;
|
|
case MAVLINK_MSG_ID_SYS_STATUS: {
|
|
mavlink_msg_sys_status_decode(&msg,&vehicle.sys_status);
|
|
|
|
sys_status_csv.append(QString::number(vehicle.sys_status.onboard_control_sensors_present)); sys_status_csv.append(',');
|
|
sys_status_csv.append(QString::number(vehicle.sys_status.onboard_control_sensors_enabled)); sys_status_csv.append(',');
|
|
sys_status_csv.append(QString::number(vehicle.sys_status.onboard_control_sensors_health)); sys_status_csv.append(',');
|
|
sys_status_csv.append(QString::number(vehicle.sys_status.load)); sys_status_csv.append(',');
|
|
sys_status_csv.append(QString::number(vehicle.sys_status.voltage_battery)); sys_status_csv.append(',');
|
|
sys_status_csv.append(QString::number(vehicle.sys_status.current_battery)); sys_status_csv.append(',');
|
|
sys_status_csv.append(QString::number(vehicle.sys_status.battery_remaining)); sys_status_csv.append(',');
|
|
sys_status_csv.append(QString::number(vehicle.sys_status.drop_rate_comm)); sys_status_csv.append(',');
|
|
sys_status_csv.append(QString::number(vehicle.sys_status.errors_comm)); sys_status_csv.append(',');
|
|
sys_status_csv.append(QString::number(vehicle.sys_status.errors_count1)); sys_status_csv.append(',');
|
|
sys_status_csv.append(QString::number(vehicle.sys_status.errors_count2)); sys_status_csv.append(',');
|
|
sys_status_csv.append(QString::number(vehicle.sys_status.errors_count3)); sys_status_csv.append(',');
|
|
sys_status_csv.append(QString::number(vehicle.sys_status.errors_count4)); sys_status_csv.append('\n');
|
|
|
|
|
|
}break;
|
|
case MAVLINK_MSG_ID_HEARTBEAT: {
|
|
mavlink_msg_heartbeat_decode(&msg,&vehicle.heartbeat);
|
|
|
|
Status->m_heartbeat.autopilot = vehicle.heartbeat.autopilot;
|
|
Status->m_heartbeat.base_mode = vehicle.heartbeat.base_mode;
|
|
Status->m_heartbeat.custom_mode = vehicle.heartbeat.custom_mode;
|
|
Status->m_heartbeat.mavlink_version = vehicle.heartbeat.mavlink_version;
|
|
Status->m_heartbeat.system_status = vehicle.heartbeat.system_status;
|
|
Status->m_heartbeat.type = vehicle.heartbeat.type;
|
|
|
|
|
|
heartbeat_csv.append(QString::number(vehicle.heartbeat.type)); heartbeat_csv.append(',');
|
|
heartbeat_csv.append(QString::number(vehicle.heartbeat.autopilot)); heartbeat_csv.append(',');
|
|
heartbeat_csv.append(QString::number(vehicle.heartbeat.base_mode)); heartbeat_csv.append(',');
|
|
heartbeat_csv.append(QString::number(vehicle.heartbeat.custom_mode)); heartbeat_csv.append(',');
|
|
heartbeat_csv.append(QString::number(vehicle.heartbeat.system_status)); heartbeat_csv.append(',');
|
|
heartbeat_csv.append(QString::number(vehicle.heartbeat.mavlink_version)); heartbeat_csv.append('\n');
|
|
|
|
|
|
emit beep(msg.sysid);
|
|
}break;
|
|
case MAVLINK_MSG_ID_PING: {
|
|
mavlink_msg_ping_decode(&msg,&vehicle.ping);
|
|
}break;
|
|
case MAVLINK_MSG_ID_ATTITUDE: {
|
|
mavlink_msg_attitude_decode(&msg,&vehicle.attitude);
|
|
|
|
attitude_csv.append(QString::number(vehicle.attitude.time_boot_ms)); attitude_csv.append(',');
|
|
attitude_csv.append(QString::number(vehicle.attitude.roll)); attitude_csv.append(',');
|
|
attitude_csv.append(QString::number(vehicle.attitude.pitch)); attitude_csv.append(',');
|
|
attitude_csv.append(QString::number(vehicle.attitude.yaw)); attitude_csv.append(',');
|
|
attitude_csv.append(QString::number(vehicle.attitude.rollspeed)); attitude_csv.append(',');
|
|
attitude_csv.append(QString::number(vehicle.attitude.pitchspeed)); attitude_csv.append(',');
|
|
attitude_csv.append(QString::number(vehicle.attitude.yawspeed)); attitude_csv.append('\n');
|
|
|
|
|
|
}break;
|
|
case MAVLINK_MSG_ID_INS1: {
|
|
mavlink_msg_ins1_decode(&msg,&vehicle.ins1);
|
|
|
|
ins1_csv.append(QString::number(vehicle.ins1.time_boot_ms)); ins1_csv.append(',');
|
|
ins1_csv.append(QString::number(vehicle.ins1.pitch)); ins1_csv.append(',');
|
|
ins1_csv.append(QString::number(vehicle.ins1.roll)); ins1_csv.append(',');
|
|
ins1_csv.append(QString::number(vehicle.ins1.yaw)); ins1_csv.append(',');
|
|
ins1_csv.append(QString::number(vehicle.ins1.lon,'f',8)); ins1_csv.append(',');
|
|
ins1_csv.append(QString::number(vehicle.ins1.lat,'f',8)); ins1_csv.append(',');
|
|
ins1_csv.append(QString::number(vehicle.ins1.alt)); ins1_csv.append(',');
|
|
ins1_csv.append(QString::number(vehicle.ins1.v_north)); ins1_csv.append(',');
|
|
ins1_csv.append(QString::number(vehicle.ins1.v_up)); ins1_csv.append(',');
|
|
ins1_csv.append(QString::number(vehicle.ins1.v_east)); ins1_csv.append(',');
|
|
ins1_csv.append(QString::number(vehicle.ins1.gx)); ins1_csv.append(',');
|
|
ins1_csv.append(QString::number(vehicle.ins1.gy)); ins1_csv.append(',');
|
|
ins1_csv.append(QString::number(vehicle.ins1.gz)); ins1_csv.append(',');
|
|
ins1_csv.append(QString::number(vehicle.ins1.ax)); ins1_csv.append(',');
|
|
ins1_csv.append(QString::number(vehicle.ins1.ay)); ins1_csv.append(',');
|
|
ins1_csv.append(QString::number(vehicle.ins1.az)); ins1_csv.append(',');
|
|
ins1_csv.append(QString::number((uint64_t)vehicle.ins1.time,'f',0)); ins1_csv.append(',');
|
|
ins1_csv.append(QString::number(vehicle.ins1.sys_status)); ins1_csv.append(',');
|
|
ins1_csv.append(QString::number(vehicle.ins1.com_status)); ins1_csv.append(',');
|
|
ins1_csv.append(QString::number(vehicle.ins1.gps_status)); ins1_csv.append(',');
|
|
ins1_csv.append(QString::number(vehicle.ins1.BIT)); ins1_csv.append(',');
|
|
ins1_csv.append(QString::number(vehicle.ins1.seq)); ins1_csv.append(',');
|
|
ins1_csv.append(QString::number(vehicle.ins1.eph)); ins1_csv.append(',');
|
|
ins1_csv.append(QString::number(vehicle.ins1.epv)); ins1_csv.append(',');
|
|
ins1_csv.append(QString::number(vehicle.ins1.satellites_visible)); ins1_csv.append('\n');
|
|
|
|
emit signal_ins1(vehicle.ins1);
|
|
|
|
}break;
|
|
case MAVLINK_MSG_ID_INS2: {
|
|
mavlink_msg_ins2_decode(&msg,&vehicle.ins2);
|
|
|
|
ins2_csv.append(QString::number(vehicle.ins2.time_boot_ms)); ins2_csv.append(',');
|
|
ins2_csv.append(QString::number(vehicle.ins2.pitch)); ins2_csv.append(',');
|
|
ins2_csv.append(QString::number(vehicle.ins2.roll)); ins2_csv.append(',');
|
|
ins2_csv.append(QString::number(vehicle.ins2.yaw)); ins2_csv.append(',');
|
|
ins2_csv.append(QString::number(vehicle.ins2.lon,'f',8)); ins2_csv.append(',');
|
|
ins2_csv.append(QString::number(vehicle.ins2.lat,'f',8)); ins2_csv.append(',');
|
|
ins2_csv.append(QString::number(vehicle.ins2.alt)); ins2_csv.append(',');
|
|
ins2_csv.append(QString::number(vehicle.ins2.v_north)); ins2_csv.append(',');
|
|
ins2_csv.append(QString::number(vehicle.ins2.v_up)); ins2_csv.append(',');
|
|
ins2_csv.append(QString::number(vehicle.ins2.v_east)); ins2_csv.append(',');
|
|
ins2_csv.append(QString::number(vehicle.ins2.gx)); ins2_csv.append(',');
|
|
ins2_csv.append(QString::number(vehicle.ins2.gy)); ins2_csv.append(',');
|
|
ins2_csv.append(QString::number(vehicle.ins2.gz)); ins2_csv.append(',');
|
|
ins2_csv.append(QString::number(vehicle.ins2.ax)); ins2_csv.append(',');
|
|
ins2_csv.append(QString::number(vehicle.ins2.ay)); ins2_csv.append(',');
|
|
ins2_csv.append(QString::number(vehicle.ins2.az)); ins2_csv.append(',');
|
|
ins2_csv.append(QString::number((uint64_t)vehicle.ins2.time,'f',0)); ins2_csv.append(',');
|
|
ins2_csv.append(QString::number(vehicle.ins2.sys_status)); ins2_csv.append(',');
|
|
ins2_csv.append(QString::number(vehicle.ins2.com_status)); ins2_csv.append(',');
|
|
ins2_csv.append(QString::number(vehicle.ins2.gps_status)); ins2_csv.append(',');
|
|
ins2_csv.append(QString::number(vehicle.ins2.BIT)); ins2_csv.append(',');
|
|
ins2_csv.append(QString::number(vehicle.ins2.seq)); ins2_csv.append(',');
|
|
ins2_csv.append(QString::number(vehicle.ins2.eph)); ins2_csv.append(',');
|
|
ins2_csv.append(QString::number(vehicle.ins2.epv)); ins2_csv.append(',');
|
|
ins2_csv.append(QString::number(vehicle.ins2.satellites_visible)); ins2_csv.append('\n');
|
|
|
|
emit signal_ins2(vehicle.ins2);
|
|
|
|
}break;
|
|
case MAVLINK_MSG_ID_GPS_RAW_INT: {
|
|
mavlink_msg_gps_raw_int_decode(&msg,&vehicle.gps_raw_int);
|
|
|
|
gps_raw_int_csv.append(QString::number(vehicle.gps_raw_int.time_usec));gps_raw_int_csv.append(',');
|
|
gps_raw_int_csv.append(QString::number(vehicle.gps_raw_int.lat));gps_raw_int_csv.append(',');
|
|
gps_raw_int_csv.append(QString::number(vehicle.gps_raw_int.lon));gps_raw_int_csv.append(',');
|
|
gps_raw_int_csv.append(QString::number(vehicle.gps_raw_int.alt));gps_raw_int_csv.append(',');
|
|
gps_raw_int_csv.append(QString::number(vehicle.gps_raw_int.eph));gps_raw_int_csv.append(',');
|
|
gps_raw_int_csv.append(QString::number(vehicle.gps_raw_int.epv));gps_raw_int_csv.append(',');
|
|
gps_raw_int_csv.append(QString::number(vehicle.gps_raw_int.vel));gps_raw_int_csv.append(',');
|
|
gps_raw_int_csv.append(QString::number(vehicle.gps_raw_int.cog));gps_raw_int_csv.append(',');
|
|
gps_raw_int_csv.append(QString::number(vehicle.gps_raw_int.fix_type));gps_raw_int_csv.append(',');
|
|
gps_raw_int_csv.append(QString::number(vehicle.gps_raw_int.satellites_visible));gps_raw_int_csv.append(',');
|
|
gps_raw_int_csv.append(QString::number(vehicle.gps_raw_int.alt_ellipsoid));gps_raw_int_csv.append(',');
|
|
gps_raw_int_csv.append(QString::number(vehicle.gps_raw_int.time_usec));gps_raw_int_csv.append(',');
|
|
gps_raw_int_csv.append(QString::number(vehicle.gps_raw_int.h_acc));gps_raw_int_csv.append(',');
|
|
gps_raw_int_csv.append(QString::number(vehicle.gps_raw_int.v_acc));gps_raw_int_csv.append(',');
|
|
gps_raw_int_csv.append(QString::number(vehicle.gps_raw_int.vel_acc));gps_raw_int_csv.append(',');
|
|
gps_raw_int_csv.append(QString::number(vehicle.gps_raw_int.hdg_acc));gps_raw_int_csv.append(',');
|
|
gps_raw_int_csv.append(QString::number(vehicle.gps_raw_int.yaw));gps_raw_int_csv.append('\n');
|
|
|
|
|
|
}break;
|
|
case MAVLINK_MSG_ID_GLOBAL_POSITION_INT: {
|
|
mavlink_msg_global_position_int_decode(&msg,&vehicle.global_position_int);
|
|
}break;
|
|
case MAVLINK_MSG_ID_SERVO_OUTPUT_RAW: {
|
|
mavlink_msg_servo_output_raw_decode(&msg,&vehicle.servo_output_raw);
|
|
|
|
servo_output_raw_csv[vehicle.servo_output_raw.port].append(QString::number(vehicle.servo_output_raw.time_usec)); servo_output_raw_csv[vehicle.servo_output_raw.port].append(',');
|
|
servo_output_raw_csv[vehicle.servo_output_raw.port].append(QString::number(vehicle.servo_output_raw.port)); servo_output_raw_csv[vehicle.servo_output_raw.port].append(',');
|
|
servo_output_raw_csv[vehicle.servo_output_raw.port].append(QString::number(vehicle.servo_output_raw.servo1_raw)); servo_output_raw_csv[vehicle.servo_output_raw.port].append(',');
|
|
servo_output_raw_csv[vehicle.servo_output_raw.port].append(QString::number(vehicle.servo_output_raw.servo2_raw)); servo_output_raw_csv[vehicle.servo_output_raw.port].append(',');
|
|
servo_output_raw_csv[vehicle.servo_output_raw.port].append(QString::number(vehicle.servo_output_raw.servo3_raw)); servo_output_raw_csv[vehicle.servo_output_raw.port].append(',');
|
|
servo_output_raw_csv[vehicle.servo_output_raw.port].append(QString::number(vehicle.servo_output_raw.servo4_raw)); servo_output_raw_csv[vehicle.servo_output_raw.port].append(',');
|
|
servo_output_raw_csv[vehicle.servo_output_raw.port].append(QString::number(vehicle.servo_output_raw.servo5_raw)); servo_output_raw_csv[vehicle.servo_output_raw.port].append(',');
|
|
servo_output_raw_csv[vehicle.servo_output_raw.port].append(QString::number(vehicle.servo_output_raw.servo6_raw)); servo_output_raw_csv[vehicle.servo_output_raw.port].append(',');
|
|
servo_output_raw_csv[vehicle.servo_output_raw.port].append(QString::number(vehicle.servo_output_raw.servo7_raw)); servo_output_raw_csv[vehicle.servo_output_raw.port].append(',');
|
|
servo_output_raw_csv[vehicle.servo_output_raw.port].append(QString::number(vehicle.servo_output_raw.servo8_raw)); servo_output_raw_csv[vehicle.servo_output_raw.port].append(',');
|
|
servo_output_raw_csv[vehicle.servo_output_raw.port].append(QString::number(vehicle.servo_output_raw.servo9_raw)); servo_output_raw_csv[vehicle.servo_output_raw.port].append(',');
|
|
servo_output_raw_csv[vehicle.servo_output_raw.port].append(QString::number(vehicle.servo_output_raw.servo10_raw)); servo_output_raw_csv[vehicle.servo_output_raw.port].append(',');
|
|
servo_output_raw_csv[vehicle.servo_output_raw.port].append(QString::number(vehicle.servo_output_raw.servo11_raw)); servo_output_raw_csv[vehicle.servo_output_raw.port].append(',');
|
|
servo_output_raw_csv[vehicle.servo_output_raw.port].append(QString::number(vehicle.servo_output_raw.servo12_raw)); servo_output_raw_csv[vehicle.servo_output_raw.port].append(',');
|
|
servo_output_raw_csv[vehicle.servo_output_raw.port].append(QString::number(vehicle.servo_output_raw.servo13_raw)); servo_output_raw_csv[vehicle.servo_output_raw.port].append(',');
|
|
servo_output_raw_csv[vehicle.servo_output_raw.port].append(QString::number(vehicle.servo_output_raw.servo14_raw)); servo_output_raw_csv[vehicle.servo_output_raw.port].append(',');
|
|
servo_output_raw_csv[vehicle.servo_output_raw.port].append(QString::number(vehicle.servo_output_raw.servo15_raw)); servo_output_raw_csv[vehicle.servo_output_raw.port].append(',');
|
|
servo_output_raw_csv[vehicle.servo_output_raw.port].append(QString::number(vehicle.servo_output_raw.servo16_raw)); servo_output_raw_csv[vehicle.servo_output_raw.port].append('\n');
|
|
|
|
|
|
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);
|
|
}break;
|
|
case MAVLINK_MSG_ID_NAV_CONTROLLER_OUTPUT: {
|
|
mavlink_msg_nav_controller_output_decode(&msg,&vehicle.nav_controller_output);
|
|
|
|
nav_controller_output_csv.append(QString::number(vehicle.nav_controller_output.nav_roll)); nav_controller_output_csv.append(',');
|
|
nav_controller_output_csv.append(QString::number(vehicle.nav_controller_output.nav_pitch)); nav_controller_output_csv.append(',');
|
|
nav_controller_output_csv.append(QString::number(vehicle.nav_controller_output.nav_bearing)); nav_controller_output_csv.append(',');
|
|
nav_controller_output_csv.append(QString::number(vehicle.nav_controller_output.target_bearing)); nav_controller_output_csv.append(',');
|
|
nav_controller_output_csv.append(QString::number(vehicle.nav_controller_output.wp_dist)); nav_controller_output_csv.append(',');
|
|
nav_controller_output_csv.append(QString::number(vehicle.nav_controller_output.alt_error)); nav_controller_output_csv.append(',');
|
|
nav_controller_output_csv.append(QString::number(vehicle.nav_controller_output.aspd_error)); nav_controller_output_csv.append(',');
|
|
nav_controller_output_csv.append(QString::number(vehicle.nav_controller_output.xtrack_error)); nav_controller_output_csv.append('\n');
|
|
|
|
|
|
}break;
|
|
case MAVLINK_MSG_ID_AIRSPEED_AUTOCAL: {
|
|
mavlink_msg_airspeed_autocal_decode(&msg,&vehicle.airspeed_autocal);
|
|
}break;
|
|
case MAVLINK_MSG_ID_RPM: {
|
|
mavlink_msg_rpm_decode(&msg,&vehicle.rpm);
|
|
}break;
|
|
case MAVLINK_MSG_ID_SCALED_PRESSURE: {
|
|
mavlink_msg_scaled_pressure_decode(&msg,&vehicle.scaled_pressure);
|
|
|
|
scaled_pressure_csv.append(QString::number(vehicle.scaled_pressure.time_boot_ms)); scaled_pressure_csv.append(',');
|
|
scaled_pressure_csv.append(QString::number(vehicle.scaled_pressure.press_abs)); scaled_pressure_csv.append(',');
|
|
scaled_pressure_csv.append(QString::number(vehicle.scaled_pressure.press_diff)); scaled_pressure_csv.append(',');
|
|
scaled_pressure_csv.append(QString::number(vehicle.scaled_pressure.temperature)); scaled_pressure_csv.append(',');
|
|
scaled_pressure_csv.append(QString::number(vehicle.scaled_pressure.temperature_press_diff)); scaled_pressure_csv.append('\n');
|
|
|
|
|
|
}break;
|
|
case MAVLINK_MSG_ID_EXTENDED_SYS_STATE: {
|
|
mavlink_msg_extended_sys_state_decode(&msg,&vehicle.extended_sys_state);
|
|
}break;
|
|
case MAVLINK_MSG_ID_BATTERY_STATUS: {
|
|
mavlink_msg_battery_status_decode(&msg,&vehicle.battery_status);
|
|
|
|
/*
|
|
battery_status_csv.append(QString::number(vehicle.battery_status.time_boot_ms)); battery_status_csv.append(',');
|
|
battery_status_csv.append(QString::number(vehicle.battery_status.Airspeed)); battery_status_csv.append(',');
|
|
battery_status_csv.append(QString::number(vehicle.battery_status.beta)); battery_status_csv.append(',');
|
|
battery_status_csv.append(QString::number(vehicle.battery_status.alpha)); battery_status_csv.append(',');
|
|
battery_status_csv.append(QString::number(vehicle.battery_status.ps)); battery_status_csv.append(',');
|
|
battery_status_csv.append(QString::number(vehicle.battery_status.qbar)); battery_status_csv.append(',');
|
|
battery_status_csv.append(QString::number(vehicle.battery_status.seq)); battery_status_csv.append(',');
|
|
battery_status_csv.append(QString::number(vehicle.battery_status.mach)); battery_status_csv.append('\n');
|
|
*/
|
|
|
|
}break;
|
|
case MAVLINK_MSG_ID_VIBRATION: {
|
|
mavlink_msg_vibration_decode(&msg,&vehicle.vibration);
|
|
}break;
|
|
case MAVLINK_MSG_ID_EngineState: {
|
|
mavlink_msg_enginestate_decode(&msg,&vehicle.enginestate);
|
|
|
|
/*
|
|
battery_status_csv.append(QString::number(vehicle.emb_atom_com.time_boot_ms)); emb_atom_com_csv.append(',');
|
|
emb_atom_com_csv.append(QString::number(vehicle.emb_atom_com.Airspeed)); emb_atom_com_csv.append(',');
|
|
emb_atom_com_csv.append(QString::number(vehicle.emb_atom_com.beta)); emb_atom_com_csv.append(',');
|
|
emb_atom_com_csv.append(QString::number(vehicle.emb_atom_com.alpha)); emb_atom_com_csv.append(',');
|
|
emb_atom_com_csv.append(QString::number(vehicle.emb_atom_com.ps)); emb_atom_com_csv.append(',');
|
|
emb_atom_com_csv.append(QString::number(vehicle.emb_atom_com.qbar)); emb_atom_com_csv.append(',');
|
|
emb_atom_com_csv.append(QString::number(vehicle.emb_atom_com.seq)); emb_atom_com_csv.append(',');
|
|
emb_atom_com_csv.append(QString::number(vehicle.emb_atom_com.mach)); emb_atom_com_csv.append('\n');
|
|
*/
|
|
|
|
}break;
|
|
case MAVLINK_MSG_ID_VFR_HUD: {
|
|
mavlink_msg_vfr_hud_decode(&msg,&vehicle.vfr_hud);
|
|
|
|
vfr_hud_csv.append(QString::number(vehicle.vfr_hud.airspeed)); vfr_hud_csv.append(',');
|
|
vfr_hud_csv.append(QString::number(vehicle.vfr_hud.groundspeed)); vfr_hud_csv.append(',');
|
|
vfr_hud_csv.append(QString::number(vehicle.vfr_hud.alt)); vfr_hud_csv.append(',');
|
|
vfr_hud_csv.append(QString::number(vehicle.vfr_hud.climb)); vfr_hud_csv.append(',');
|
|
vfr_hud_csv.append(QString::number(vehicle.vfr_hud.heading)); vfr_hud_csv.append(',');
|
|
vfr_hud_csv.append(QString::number(vehicle.vfr_hud.throttle)); vfr_hud_csv.append('\n');
|
|
|
|
}break;
|
|
case MAVLINK_MSG_ID_EMB_ATMO_COM: {
|
|
mavlink_msg_emb_atmo_com_decode(&msg,&vehicle.emb_atom_com);
|
|
|
|
emb_atom_com_csv.append(QString::number(vehicle.emb_atom_com.time_boot_ms)); emb_atom_com_csv.append(',');
|
|
emb_atom_com_csv.append(QString::number(vehicle.emb_atom_com.Airspeed)); emb_atom_com_csv.append(',');
|
|
emb_atom_com_csv.append(QString::number(vehicle.emb_atom_com.beta)); emb_atom_com_csv.append(',');
|
|
emb_atom_com_csv.append(QString::number(vehicle.emb_atom_com.alpha)); emb_atom_com_csv.append(',');
|
|
emb_atom_com_csv.append(QString::number(vehicle.emb_atom_com.ps)); emb_atom_com_csv.append(',');
|
|
emb_atom_com_csv.append(QString::number(vehicle.emb_atom_com.qbar)); emb_atom_com_csv.append(',');
|
|
emb_atom_com_csv.append(QString::number(vehicle.emb_atom_com.seq)); emb_atom_com_csv.append(',');
|
|
emb_atom_com_csv.append(QString::number(vehicle.emb_atom_com.mach)); emb_atom_com_csv.append('\n');
|
|
|
|
|
|
}break;
|
|
case MAVLINK_MSG_ID_TurbineState: {
|
|
mavlink_msg_turbinestate_decode(&msg,&vehicle.turbinstate);
|
|
|
|
turbinstate_csv.append(QString::number(vehicle.turbinstate.time_boot_ms)); turbinstate_csv.append(',');
|
|
turbinstate_csv.append(QString::number(vehicle.turbinstate.RPM_mea)); turbinstate_csv.append(',');
|
|
turbinstate_csv.append(QString::number(vehicle.turbinstate.T5)); turbinstate_csv.append(',');
|
|
turbinstate_csv.append(QString::number(vehicle.turbinstate.Kfuel)); turbinstate_csv.append(',');
|
|
turbinstate_csv.append(QString::number(vehicle.turbinstate.RPM_des)); turbinstate_csv.append(',');
|
|
turbinstate_csv.append(QString::number(vehicle.turbinstate.RPM_des_ap)); turbinstate_csv.append(',');
|
|
turbinstate_csv.append(QString::number(vehicle.turbinstate.RPM_bak)); turbinstate_csv.append(',');
|
|
turbinstate_csv.append(QString::number(vehicle.turbinstate.IOState)); turbinstate_csv.append(',');
|
|
turbinstate_csv.append(QString::number(vehicle.turbinstate.SysState)); turbinstate_csv.append(',');
|
|
turbinstate_csv.append(QString::number(vehicle.turbinstate.Fault)); turbinstate_csv.append(',');
|
|
turbinstate_csv.append(QString::number(vehicle.turbinstate.stage_ap)); turbinstate_csv.append(',');
|
|
turbinstate_csv.append(QString::number(vehicle.turbinstate.temp_ap)); turbinstate_csv.append(',');
|
|
turbinstate_csv.append(QString::number(vehicle.turbinstate.tas_ap)); turbinstate_csv.append(',');
|
|
turbinstate_csv.append(QString::number(vehicle.turbinstate.asl_ap)); turbinstate_csv.append(',');
|
|
turbinstate_csv.append(QString::number(vehicle.turbinstate.KabMain)); turbinstate_csv.append(',');
|
|
turbinstate_csv.append(QString::number(vehicle.turbinstate.KabFire)); turbinstate_csv.append(',');
|
|
turbinstate_csv.append(QString::number(vehicle.turbinstate.KDj)); turbinstate_csv.append(',');
|
|
turbinstate_csv.append(QString::number(vehicle.turbinstate.T1t)); turbinstate_csv.append(',');
|
|
turbinstate_csv.append(QString::number(vehicle.turbinstate.P1t)); turbinstate_csv.append(',');
|
|
turbinstate_csv.append(QString::number(vehicle.turbinstate.P3t)); turbinstate_csv.append(',');
|
|
turbinstate_csv.append(QString::number(vehicle.turbinstate.P5t)); turbinstate_csv.append(',');
|
|
turbinstate_csv.append(QString::number(vehicle.turbinstate.DJS)); turbinstate_csv.append(',');
|
|
turbinstate_csv.append(QString::number(vehicle.turbinstate.Vcc)); turbinstate_csv.append(',');
|
|
turbinstate_csv.append(QString::number(vehicle.turbinstate.Tbak)); turbinstate_csv.append(',');
|
|
turbinstate_csv.append(QString::number(vehicle.turbinstate.rev)); turbinstate_csv.append(',');
|
|
turbinstate_csv.append(QString::number(vehicle.turbinstate.CFuelMode)); turbinstate_csv.append(',');
|
|
turbinstate_csv.append(QString::number(vehicle.turbinstate.Cmd)); turbinstate_csv.append('\n');
|
|
|
|
}break;
|
|
case MAVLINK_MSG_ID_BMUState: {
|
|
mavlink_msg_bmustate_decode(&msg,&vehicle.bmustate);
|
|
|
|
bmustate_csv.append(QString::number(vehicle.bmustate.time_boot_ms)); bmustate_csv.append(',');
|
|
bmustate_csv.append(QString::number(vehicle.bmustate.BAT1_group_voltage_mv)); bmustate_csv.append(',');
|
|
bmustate_csv.append(QString::number(vehicle.bmustate.BAT1_group_current_dA)); bmustate_csv.append(',');
|
|
bmustate_csv.append(QString::number(vehicle.bmustate.BAT1_remain_perc)); bmustate_csv.append(',');
|
|
bmustate_csv.append(QString::number(vehicle.bmustate.BAT1_low_temp_degC)); bmustate_csv.append(',');
|
|
bmustate_csv.append(QString::number(vehicle.bmustate.BAT1_voltages_mv[0])); bmustate_csv.append(',');
|
|
bmustate_csv.append(QString::number(vehicle.bmustate.BAT1_hi_voltage_mv)); bmustate_csv.append(',');
|
|
bmustate_csv.append(QString::number(vehicle.bmustate.BAT1_low_voltage_mv)); bmustate_csv.append(',');
|
|
bmustate_csv.append(QString::number(vehicle.bmustate.BAT2_group_voltage_mv)); bmustate_csv.append(',');
|
|
bmustate_csv.append(QString::number(vehicle.bmustate.BAT2_group_current_dA)); bmustate_csv.append(',');
|
|
bmustate_csv.append(QString::number(vehicle.bmustate.BAT2_remain_perc)); bmustate_csv.append(',');
|
|
bmustate_csv.append(QString::number(vehicle.bmustate.BAT2_low_temp_degC)); bmustate_csv.append(',');
|
|
bmustate_csv.append(QString::number(vehicle.bmustate.BAT2_hi_temp_degC)); bmustate_csv.append(',');
|
|
bmustate_csv.append(QString::number(vehicle.bmustate.BAT2_voltages_mv[0])); bmustate_csv.append(',');
|
|
bmustate_csv.append(QString::number(vehicle.bmustate.BAT2_hi_voltage_mv)); bmustate_csv.append(',');
|
|
bmustate_csv.append(QString::number(vehicle.bmustate.BAT2_low_voltage_mv)); bmustate_csv.append(',');
|
|
bmustate_csv.append(QString::number(vehicle.bmustate.BAT1_STA1)); bmustate_csv.append(',');
|
|
bmustate_csv.append(QString::number(vehicle.bmustate.BAT1_STA2)); bmustate_csv.append(',');
|
|
bmustate_csv.append(QString::number(vehicle.bmustate.BAT2_STA1)); bmustate_csv.append(',');
|
|
bmustate_csv.append(QString::number(vehicle.bmustate.BAT2_STA2)); bmustate_csv.append(',');
|
|
bmustate_csv.append(QString::number(vehicle.bmustate.p500w_enabled)); bmustate_csv.append('\n');
|
|
|
|
}break;
|
|
case MAVLINK_MSG_ID_CCMState: {
|
|
mavlink_msg_ccmstate_decode(&msg,&vehicle.ccmstate);
|
|
|
|
ccmstate_csv.append(QString::number(vehicle.ccmstate.time_boot_ms)); ccmstate_csv.append(',');
|
|
ccmstate_csv.append(QString::number(vehicle.ccmstate.fuel_level)); ccmstate_csv.append(',');
|
|
ccmstate_csv.append(QString::number(vehicle.ccmstate.temp[0])); ccmstate_csv.append(',');
|
|
ccmstate_csv.append(QString::number(vehicle.ccmstate.temp[1])); ccmstate_csv.append(',');
|
|
ccmstate_csv.append(QString::number(vehicle.ccmstate.temp[2])); ccmstate_csv.append(',');
|
|
ccmstate_csv.append(QString::number(vehicle.ccmstate.temp[3])); ccmstate_csv.append(',');
|
|
ccmstate_csv.append(QString::number(vehicle.ccmstate.volts[0])); ccmstate_csv.append(',');
|
|
ccmstate_csv.append(QString::number(vehicle.ccmstate.volts[1])); ccmstate_csv.append(',');
|
|
ccmstate_csv.append(QString::number(vehicle.ccmstate.volts[2])); ccmstate_csv.append(',');
|
|
ccmstate_csv.append(QString::number(vehicle.ccmstate.volts[3])); ccmstate_csv.append(',');
|
|
ccmstate_csv.append(QString::number(vehicle.ccmstate.echo_seq)); ccmstate_csv.append('\n');
|
|
|
|
|
|
}break;
|
|
}
|
|
|
|
|
|
|
|
|
|
//vehicleList.insert(msg.sysid,vehicle);//直接覆盖
|
|
|
|
|
|
|
|
emit signal_vehicle(vehicle);
|
|
|
|
emit state_updated();
|
|
|
|
}
|
|
|
|
|
|
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);
|
|
}
|
|
}
|
|
|