diff --git a/App/mainwindow.cpp b/App/mainwindow.cpp index dfdb98d..ab32592 100644 --- a/App/mainwindow.cpp +++ b/App/mainwindow.cpp @@ -286,11 +286,11 @@ MainWindow::MainWindow(QWidget *parent) //dlink --- toolui-index3 - connect(dlink->mavlinknode->Terminal,SIGNAL(Recieve(QString)), + connect(dlink->mavlinknode,SIGNAL(Recieve(QString)), toolsui->index3,SLOT(Recieve(QString))); connect(toolsui->index3,SIGNAL(Terminal_Transmit(QString)), - dlink->mavlinknode->Terminal,SLOT(Transmit(QString)),Qt::DirectConnection); + dlink->mavlinknode,SLOT(Transmit(QString)),Qt::DirectConnection); @@ -632,7 +632,7 @@ MainWindow::MainWindow(QWidget *parent) dlink->mavlinknode,&MavLinkNode::setGCSID,Qt::DirectConnection); connect(setting->index0->globalsetting,&GlobalSetting::setHeartBeat, - dlink->mavlinknode->Status,&statusprocess::setHeartbeat,Qt::DirectConnection); + dlink->mavlinknode,&MavLinkNode::setHeartbeat,Qt::DirectConnection); //设置地面站ID @@ -1750,7 +1750,8 @@ void MainWindow::updateUI()//事件驱动式更新数据 } - healthui->setState(28,getBit(health,31)?(HealthUI::state::inital):(HealthUI::state::failure)); + healthui->setState(28,getBit(health,31)?(HealthUI::state::inital):(HealthUI::state::failure));//上行有效 + healthui->setState(29,getBit(health,30)?(HealthUI::state::success):(HealthUI::state::inital)); //healthui->setValueState(29,getBit(health,30)?(tr("记录已打开")):(tr("")));//sel diff --git a/MavLinkNode/Terminal.cpp b/MavLinkNode/Terminal.cpp index 2960129..e82aba0 100644 --- a/MavLinkNode/Terminal.cpp +++ b/MavLinkNode/Terminal.cpp @@ -55,15 +55,15 @@ void terminal::Parse(mavlink_message_t msg) switch (msg.msgid) { case MAVLINK_MSG_ID_SERIAL_CONTROL : - mavlink_serial_control_t serial_control; + mavlink_serial_control_t serial_control; - mavlink_msg_serial_control_decode(&msg,&serial_control); + mavlink_msg_serial_control_decode(&msg,&serial_control); - QByteArray str; + QByteArray str; - str.append((char *)serial_control.data,serial_control.count); + str.append((char *)serial_control.data,serial_control.count); - emit Recieve(str); + emit Recieve(str); break; diff --git a/MavLinkNode/ThreadTemplet.cpp b/MavLinkNode/ThreadTemplet.cpp index 5cd5521..049b220 100644 --- a/MavLinkNode/ThreadTemplet.cpp +++ b/MavLinkNode/ThreadTemplet.cpp @@ -86,6 +86,7 @@ void ThreadTemplet::pause() } + void ThreadTemplet::process()//线程函数 { diff --git a/MavLinkNode/commandprocess.cpp b/MavLinkNode/commandprocess.cpp index 5d32906..33fb00a 100644 --- a/MavLinkNode/commandprocess.cpp +++ b/MavLinkNode/commandprocess.cpp @@ -26,6 +26,7 @@ void commandprocess::process()//线程函数 while (true) { + //QThread::msleep((status.m_Mode != 0)?(0):(-1)); count ++; switch(status.m_Mode) { diff --git a/MavLinkNode/mavlinknode.cpp b/MavLinkNode/mavlinknode.cpp index 4d077b4..6eb4d5c 100644 --- a/MavLinkNode/mavlinknode.cpp +++ b/MavLinkNode/mavlinknode.cpp @@ -110,6 +110,7 @@ MavLinkNode::MavLinkNode(QObject *parent) : ThreadTemplet(parent) 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)), @@ -120,8 +121,9 @@ MavLinkNode::MavLinkNode(QObject *parent) : ThreadTemplet(parent) 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)), @@ -132,6 +134,7 @@ MavLinkNode::MavLinkNode(QObject *parent) : ThreadTemplet(parent) connect(Terminal,SIGNAL(showMessage(QString,int)), this,SIGNAL(showMessage(QString,int)),Qt::DirectConnection); + */ /* RC = new rcprocess(); RC->setGCSID(Current_sysID,Current_CompID); @@ -147,6 +150,7 @@ MavLinkNode::MavLinkNode(QObject *parent) : ThreadTemplet(parent) this,SIGNAL(showMessage(QString,int)),Qt::DirectConnection); */ + /* rtk = new rtkprocess(); rtk->setGCSID(Current_sysID,Current_CompID); @@ -158,6 +162,7 @@ MavLinkNode::MavLinkNode(QObject *parent) : ThreadTemplet(parent) connect(rtk,SIGNAL(showMessage(QString,int)), this,SIGNAL(showMessage(QString,int)),Qt::DirectConnection); + */ isCommunicationLost = false; @@ -169,6 +174,14 @@ MavLinkNode::MavLinkNode(QObject *parent) : ThreadTemplet(parent) Parser2_init(&parser); Packer2_init(&packer); + + m_heartbeat.autopilot = 1; + m_heartbeat.base_mode = 0; + m_heartbeat.custom_mode = 0; + m_heartbeat.mavlink_version = 3; + m_heartbeat.system_status = 0; + m_heartbeat.type = 0; + } MavLinkNode::~MavLinkNode() @@ -219,17 +232,20 @@ MavLinkNode::~MavLinkNode() Commander = nullptr; } + /* if(Status) { delete Status; Status = nullptr; } - + */ +/* if(Terminal) { delete Terminal; Terminal = nullptr; } + */ /* if(RC) { @@ -238,11 +254,13 @@ MavLinkNode::~MavLinkNode() } */ + /* if(rtk) { delete rtk; rtk = nullptr; } + */ qDebug() << "stop mavlink" << QThread::currentThreadId(); } @@ -268,11 +286,13 @@ void MavLinkNode::setGCSID(int id) 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); @@ -287,6 +307,8 @@ void MavLinkNode::setGCSID(int id) { rtk->setGCSID(gcsid,MAV_COMP_ID_MISSIONPLANNER); } + */ + } void MavLinkNode::CreateCSV(void) @@ -585,6 +607,9 @@ void MavLinkNode::process()//线程函数 bool isIdle = true; + int HeartBeatTime = QTime::currentTime().msecsSinceStartOfDay(); + + QElapsedTimer time; time.start(); @@ -597,6 +622,27 @@ void MavLinkNode::process()//线程函数 QThread::msleep(1000/running_frq); //timer->start(); + if(isSendHeartBeat == true) + { + if((QTime::currentTime().msecsSinceStartOfDay() - HeartBeatTime) > (1000.0/heartbeatFrq)) + { + heartbeat(m_heartbeat.custom_mode, + m_heartbeat.type, + m_heartbeat.autopilot, + m_heartbeat.base_mode, + m_heartbeat.system_status, + m_heartbeat.mavlink_version); + HeartBeatTime = QTime::currentTime().msecsSinceStartOfDay(); + } + } + + if(isSendTerminal == true) + { + isSendTerminal = false; + serial_control(); + } + + //解码从UDP来的 datagram.clear(); datagram = readbuff(SourceType::c_sock);//每次全部读取 @@ -618,6 +664,8 @@ void MavLinkNode::process()//线程函数 } + + if(isInterruptionRequested())//退出 { break; @@ -923,8 +971,19 @@ void MavLinkNode::MAVLinkRcv_Handler(mavlink_message_t msg) //终端 case MAVLINK_MSG_ID_SERIAL_CONTROL: - Terminal->Parse(msg); - break; + { + //Terminal->Parse(msg); + mavlink_serial_control_t serial_control; + + mavlink_msg_serial_control_decode(&msg,&serial_control); + + QByteArray str; + + str.append((char *)serial_control.data,serial_control.count); + + emit Recieve(str); + + }break; //状态 @@ -1012,12 +1071,12 @@ void MavLinkNode::StatusParse(mavlink_message_t msg) 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; + m_heartbeat.autopilot = vehicle.heartbeat.autopilot; + m_heartbeat.base_mode = vehicle.heartbeat.base_mode; + m_heartbeat.custom_mode = vehicle.heartbeat.custom_mode; + m_heartbeat.mavlink_version = vehicle.heartbeat.mavlink_version; + m_heartbeat.system_status = vehicle.heartbeat.system_status; + m_heartbeat.type = vehicle.heartbeat.type; QByteArray csvdata; csvdata.clear(); @@ -1753,3 +1812,67 @@ void MavLinkNode::setLogfile(QString file) } } +//发送函数 +void MavLinkNode::setHeartbeat(QVariant state,QVariant frq) +{ + //给指令赋值 + + //开启线程开始传输 + if(state.toBool() == true) + { + isSendHeartBeat = true;//发送模式 + } + else + { + isSendHeartBeat = false;//空闲模式 + } + + heartbeatFrq = frq.toDouble(); +} + +void MavLinkNode::heartbeat(uint32_t custom_mode,uint8_t type,uint8_t autopilot,uint8_t base_mode,uint8_t system_status,uint8_t mavlink_version) +{ + static mavlink_message_t msg; + static mavlink_heartbeat_t heartbeat; + + heartbeat.custom_mode = custom_mode; + heartbeat.type = type; + + heartbeat.autopilot = autopilot; + heartbeat.base_mode = base_mode; + heartbeat.system_status = system_status; + heartbeat.mavlink_version = mavlink_version; + + mavlink_msg_heartbeat_encode(GCS_SysID,GCS_CompID, &msg,&heartbeat); + Send(msg); +} + +void MavLinkNode::Transmit(QString msg) +{ + + SerialData.clear(); + SerialData.append(msg); + isSendTerminal = true; + +} + +void MavLinkNode::serial_control(void) +{ + static mavlink_message_t msg; + static mavlink_serial_control_t serial_control; + + serial_control.baudrate = 115200; + serial_control.count = SerialData.size(); + + memcpy(serial_control.data,SerialData.toLocal8Bit().data(),SerialData.size()); + + serial_control.flags = SERIAL_CONTROL_FLAG_EXCLUSIVE | SERIAL_CONTROL_FLAG_RESPOND | SERIAL_CONTROL_FLAG_MULTI; + serial_control.device = SERIAL_CONTROL_DEV_SHELL; + serial_control.timeout = 5000; + + mavlink_msg_serial_control_encode(GCS_SysID,GCS_CompID, &msg,&serial_control); + Send(msg); + + +} + diff --git a/MavLinkNode/mavlinknode.h b/MavLinkNode/mavlinknode.h index 72c6777..1fff545 100644 --- a/MavLinkNode/mavlinknode.h +++ b/MavLinkNode/mavlinknode.h @@ -205,6 +205,8 @@ signals: void setPos(int sysid,int compid,double x,double y,double z); void setHeading(int sysid,int compid,double x,double y,double z); + void Recieve(QString msg); + public slots: //线程对外接口 void CreateCSV(void); @@ -222,6 +224,10 @@ public slots: void setGCSID(int id); + void setHeartbeat(QVariant state, QVariant frq ); + + void serial_control(void); + void Transmit(QString msg); private slots: //线程私有接口 @@ -247,6 +253,13 @@ private slots: bool setLogData(mavlink_message_t msg); + void heartbeat(uint32_t custom_mode, + uint8_t type, + uint8_t autopilot, + uint8_t base_mode, + uint8_t system_status, + uint8_t mavlink_version); + protected: int Current_sysID = 0xF1; @@ -294,7 +307,16 @@ protected: mutable QReadWriteLock RWlock; + qreal heartbeatFrq = 1; + bool isSendHeartBeat = false; + mavlink_heartbeat_t m_heartbeat; + + QTimer *heartbeatTimer = nullptr; + + + bool isSendTerminal = false; + QString SerialData; }; diff --git a/dlink/dlink.cpp b/dlink/dlink.cpp index 6a7f4b4..9e9326e 100644 --- a/dlink/dlink.cpp +++ b/dlink/dlink.cpp @@ -57,7 +57,7 @@ void DLink::timeout() qint64 time = QDateTime::currentMSecsSinceEpoch() - last; - if(time >= 2000) + if(time >= 3000) { Byte_Out_per = (double)Byte_Out_ALL / (time * 0.001);