添加终端

MavLinkNode/MavLinkNode.pro

@@ -51,6 +51,7 @@ DEFINES         += QtMavlinkNode
 
 
 HEADERS += \
+    Terminal.h \
     commandprocess.h \
     mavlinknode.h \
     parameterprocess.h \
@@ -60,6 +61,7 @@ HEADERS += \
     statusprocess.h
 
 SOURCES += \
+    Terminal.cpp \
     commandprocess.cpp \
     mavlinknode.cpp \
     parameterprocess.cpp \

MavLinkNode/Terminal.cpp

@@ -0,0 +1,200 @@
+#include "Terminal.h"
+
+
+
+terminal::terminal(QObject *parent) : QObject(parent)
+{
+    setRunFrq(20);//默认50Hz频率运行
+
+    status.m_Mode = Nop_Mode;
+}
+
+
+void terminal::setRunFrq(uint32_t frq)
+{
+    if((frq != 0)||(frq <= 1000))
+    {
+        running_frq = frq;
+        qDebug() << "set mission thread running frquency:" <<frq <<"Hz";
+    }
+}
+
+void terminal::start()
+{
+    if(thread == nullptr)
+    {
+        thread = new QThread();
+        this->moveToThread(thread);
+        connect(thread, &QThread::started, this, &terminal::process);
+    }
+
+    if(!thread->isRunning())
+    {
+        running_flag = true;
+        thread->start();
+        qDebug() << "thread start" << running_flag;
+    }
+    else
+    {
+        qDebug() << "thread has started";
+    }
+}
+
+void terminal::stop()
+{
+    if(thread->isRunning())
+    {
+        running_flag = false;
+        qDebug() << "thread stop"
+                 << QThread::currentThreadId()
+                 << QThread::currentThread()
+                 << "running state:"
+                 << running_flag;
+    }
+    else
+    {
+        qDebug() << "thread is not running";
+    }
+}
+
+void terminal::process()//线程函数
+{
+    uint8_t count = 0;
+    while (running_flag)
+    {
+         count ++;
+         QThread::msleep(1000.0/running_frq);
+
+         switch(status.m_Mode)
+         {
+            default:
+            case Nop_Mode     : break;
+            case RecieveMode  : ReadStateMachine();break;
+            case TransmitMode : WriteStateMachine();break;
+         }
+        //qDebug() << count;
+    }
+    //退出线程
+    disconnect(thread, nullptr, nullptr, nullptr);
+    thread->quit();
+    //thread->wait();//等待结束
+    thread->deleteLater();
+    thread = nullptr;
+}
+
+void terminal::setID(int m_sysid,int m_compid)
+{
+    sysid = m_sysid;
+    compid = m_compid;
+}
+
+
+
+void terminal::SendMessage(mavlink_message_t msg)
+{
+    uint8_t  buff[256+20];
+    uint16_t len = mavlink_msg_to_send_buffer(buff, &msg);
+
+    emit SendMessageTo(0,buff, len);//使用信号和槽
+}
+
+void terminal::Transmit(QString msg)
+{
+    if(status.m_Mode == Nop_Mode)//没有任务在上传
+    {
+        SerialData.clear();
+        SerialData.append(msg);
+        status.transmit.type = 0;
+        status.m_Mode = TransmitMode;//发送模式
+        qDebug() << "terminal" << sysid << compid << SerialData;
+        start();//开启线程
+    }
+}
+
+
+
+
+
+//这个函数类似中断，专门处理接收到的状态
+void terminal::Parse(mavlink_message_t msg)
+{
+    switch (msg.msgid) {
+    case MAVLINK_MSG_ID_SERIAL_CONTROL :
+
+       mavlink_serial_control_t serial_control;
+
+       mavlink_msg_serial_control_decode(&msg,&serial_control);
+
+       QByteArray str;//((char *)serial_control.data);
+
+       //str = (char *)serial_control.data;
+
+       str.append((char *)serial_control.data,serial_control.count);
+
+
+
+       //str.setRawData((char *)serial_control.data,serial_control.count);
+
+        //memcpy(str,serial_control.data,SerialData.size());
+
+       //qDebug() << str;
+
+       emit Recieve(str);
+
+       //delete str;
+
+    break;
+
+    }
+
+}
+
+
+//读航线状态机
+void terminal::ReadStateMachine(void)
+{
+   static uint8_t step = 0;
+   static uint8_t timeout_count = 0;
+   static int time = 0;
+
+   status.m_Mode = _modetype::Nop_Mode;
+}
+
+//写航线状态机
+void terminal::WriteStateMachine(void)
+{
+
+    serial_control();
+
+    status.m_Mode = _modetype::Nop_Mode;
+
+}
+
+void terminal::serial_control(void)
+{
+   static mavlink_message_t msg;
+   static mavlink_serial_control_t serial_control;
+
+   serial_control.baudrate = 115200;
+   serial_control.count = SerialData.size();
+   //serial_control.data = SerialData.toLatin1();
+
+   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;
+
+
+   qDebug() << serial_control.baudrate
+            << serial_control.count
+            << serial_control.data
+            << serial_control.device
+            << serial_control.flags
+            << serial_control.timeout;
+
+   mavlink_msg_serial_control_encode(Current_sysID,Current_CompID, &msg,&serial_control);
+   SendMessage(msg);
+}
+
+

MavLinkNode/Terminal.h

@@ -0,0 +1,137 @@
+#ifndef TERMINAL_H
+#define TERMINAL_H
+
+#include <QObject>
+
+#include "QDebug"
+#include "QThread"
+#include "mavlink.h"
+#include "QTimer"
+#include "QTime"
+
+
+#ifdef QtMavlinkNode
+#include <mavlinknodeglobal.h>
+class MAVLINKNODESHARED_EXPORT terminal : public QObject {
+#else
+class terminal : public QObject
+{
+#endif
+    Q_OBJECT
+
+    enum _modetype {
+        Nop_Mode = 0,
+        RecieveMode,
+        TransmitMode
+    };
+
+
+    typedef struct {
+        //bool isWaitingforCount;
+        bool isWaitingforValue;
+
+
+    }_recieve;
+
+    typedef struct {
+       bool isWaitingforACK;
+       uint8_t type;
+    }_transmit;
+
+    typedef struct
+    {
+        _recieve recieve;
+        _transmit transmit;
+        _modetype m_Mode;
+    }_status_;
+
+
+
+
+public:
+
+    explicit terminal(QObject *parent = nullptr);
+
+    _status_ status;
+
+    void setGCSID(int m_sysid, int m_compid)
+    {
+        Current_sysID = m_sysid;
+        Current_CompID = m_compid;
+    }
+
+
+
+    mavlink_serial_control_t m_serial_control;
+
+public slots:
+
+    void setID(int m_sysid, int m_compid);
+
+    //读取和写入指令
+
+    void Transmit(QString msg);
+
+
+    //线程对外接口
+    void setRunFrq(uint32_t frq);
+    void start();
+    void stop();
+    bool isActive(void)
+    {
+        return running_flag;
+    }
+
+    void Parse(mavlink_message_t msg);
+private slots:
+   //线程私有接口
+   void SendMessage(mavlink_message_t msg);
+
+   void process();
+
+   //状态机
+   void ReadStateMachine(void);
+   void WriteStateMachine(void);
+
+
+   //所有相关函数
+   void serial_control(void);
+
+
+signals:
+   void readError();
+
+   void SendMessageTo(quint8 ch, quint8 *data,quint16 len);
+
+   void commandAccepted(bool flag,uint16_t command,uint8_t result);
+
+   void showMessage(const QString &message,int TimeOut = 0);
+
+
+   void Recieve(QString msg);
+
+
+
+private:
+   bool running_flag = false;
+   quint32 running_frq = 200;//200Hz
+   QThread *thread = nullptr;
+
+   //目标id
+   int sysid;
+   int compid;
+
+   //本机的id
+   int Current_sysID = 0xF1;
+   int Current_CompID = 0xF1;
+
+   //超时时间（ms）
+   int readTimeout = 5000;
+   int sendTimeout = 5000;
+
+
+   QString SerialData;
+
+};
+
+#endif // STATUSPROCESS_H

MavLinkNode/mavlinknode.cpp

@@ -99,6 +99,18 @@ MavLinkNode::MavLinkNode(QObject *parent) : QObject(parent)
             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);
+
+
 
     isCommunicationLost = false;
 
@@ -189,6 +201,17 @@ MavLinkNode::~MavLinkNode()
         }
     }
 
+    if(Terminal)
+    {
+        if(Terminal->isActive())
+        {
+            Terminal->stop();//可能还没停止线程，后面不能紧跟着删除
+            QThread::msleep(10);
+            delete Terminal;
+            Terminal = nullptr;
+        }
+    }
+
     if(timer)
     {
         timer->stop();
@@ -236,6 +259,11 @@ void MavLinkNode::setGCSID(int id)
       {
           Status->setGCSID(gcsid,MAV_COMP_ID_MISSIONPLANNER);
       }
+
+      if(Terminal)
+      {
+          Terminal->setGCSID(gcsid,MAV_COMP_ID_MISSIONPLANNER);
+      }
 }
 
 
@@ -317,6 +345,11 @@ void MavLinkNode::start()
         {
             Parameter->start();
         }
+
+        if(Terminal)
+        {
+            Terminal->start();
+        }
     }
     else
     {
@@ -768,6 +801,12 @@ void MavLinkNode::MAVLinkRcv_Handler(mavlink_message_t msg)
            Commander->Parse(msg);
         break;
 
+        //终端
+        case MAVLINK_MSG_ID_SERIAL_CONTROL:
+           Terminal->Parse(msg);
+        break;
+
+
         //状态
         default:
             StatusParse(msg);

MavLinkNode/mavlinknode.h

@@ -12,7 +12,7 @@
 #include "parameterprocess.h"
 #include "commandprocess.h"
 #include "statusprocess.h"
-
+#include "Terminal.h"
 
 
 
@@ -62,6 +62,7 @@ class MavLinkNode : public QObject
         mavlink_turbinestate_t turbinstate;
         mavlink_bmustate_t bmustate;
         mavlink_ccmstate_t ccmstate;
+        mavlink_serial_control_t serial_control;
 
     }_vehicle;
 
@@ -78,8 +79,8 @@ public:
      MissionProcess *Mission = nullptr;
      ParameterProcess *Parameter = nullptr;
      commandprocess *Commander = nullptr;
-
      statusprocess *Status = nullptr;
+     terminal *Terminal = nullptr;
 
 
      bool isCommunicationLost = false;

MavLinkNode/replay.cpp

@@ -169,8 +169,38 @@ void Replay::process()//线程函数
 
                          if((currentTimestamp - lastTimestamp) >0)
                          {
+                             /*
+                            if(timeStamp * multiple > 100)
+                            {
+                                QThread::usleep(timeStamp * multiple);
+                            }
+                            else
+                            {
+                                QThread::usleep(50000);
+                            }
+                            */
+                             //QThread::msleep(timeStamp * (multiple / 1000.0));
+
+
+                             if(timeStamp < 50)
+                             {
+                                 timeStamp = 50;
+                             }
+
+                             QThread::msleep(timeStamp);
+
+
+                             /*
+                             if((timeStamp * (multiple / 1000.0)) > 50)
+                             {
+                                 QThread::msleep(timeStamp * (multiple / 1000.0));
+                             }
+                             else
+                             {
+                                 QThread::msleep(50);
+                             }
+                             */
 
-                            QThread::usleep(timeStamp * multiple);
                          }
 
                          position += (uint8_t)raw.at(raw.indexOf(0xFD)+1) + 20;