#include "mavlinknode.h" /* * mavlink 解析相关内容请参考如下 * https://mavlink.io/en/services/command.html * 包含了各个函数、参数、状态机等及其说明 **/ MavLinkNode::MavLinkNode(QObject *parent) : ThreadTemplet(parent) { QDir *temp = new QDir; if(!temp->exists("./Tlog")) { qDebug() << "make dir tlog"; temp->mkdir("./Tlog");//如果文件夹不存在就新建 } //初始化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(); mavlogFileName = QString("./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("./csv")) { qDebug() << "make dir tlog"; csvdir->mkdir("./csv");//如果文件夹不存在就新建 } QDateTime current = QDateTime::currentDateTime(); QString filetime = QString("./csv/%1").arg(current.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(); } 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("mavlink got bad crc1"); }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("new vehicle be found,id %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); } }