参数可以读取，参数状态机完成，支持全部读取和单个读取

App/mainwindow.cpp

@@ -128,7 +128,7 @@ void MainWindow::keyPressEvent(QKeyEvent *event)   //键盘按下事件
             if(event->modifiers() == Qt::AltModifier)
             {//下载参数
               qInfo() << "atl + P";
-              //dlink->Mavlink_param_request_list();
+              dlink->mavlinknode->Parameter->ReadCmd(1,1,0);
 
             }
         }break;

MavLinkNode/mavlinknode.cpp

@@ -11,17 +11,15 @@ MavLinkNode::MavLinkNode(QObject *parent) : QObject(parent)
     //初始化buff
     initbuff();
 
-    Mission = new MissionProcess();
+    Mission = new MissionProcess(this);
-    connect(Mission,SIGNAL(SendMessageTo(quint8,quint8*,quint16)),
+    /*connect(Mission,SIGNAL(SendMessageTo(quint8,quint8*,quint16)),
            this,SIGNAL(SendMessageTo(quint8,quint8*,quint16)));
+           */
 
-    Parameter = new ParameterProcess();
+    Parameter = new ParameterProcess(this);
-    connect(Parameter,SIGNAL(SendMessageTo(quint8,quint8*,quint16)),
+    /*connect(Parameter,SIGNAL(SendMessageTo(quint8,quint8*,quint16)),
            this,SIGNAL(SendMessageTo(quint8,quint8*,quint16)));
-
+           */
-
-    //Parameterthread = new QThread();
-
 
 }
 

MavLinkNode/missionprocess.cpp

@@ -96,7 +96,7 @@ void MissionProcess::WriteCmd(uint8_t m_sysid, uint8_t m_compid ,uint32_t count
     sysid = m_sysid;
     compid = m_compid;
     mission_status.transmit.count = count;
-    mission_status.m_Mode = TransmitMode;//接收模式
+    mission_status.m_Mode = TransmitMode;//发送模式
 
     start();//开启线程
 }
@@ -352,7 +352,7 @@ void MissionProcess::WriteStateMachine(void)
                else
                {
                    step++;
-                   mission_status.transmit.isWaiteforACK == true;
+                   mission_status.transmit.isWaiteforACK = true;
                    time = QTime::currentTime().msecsSinceStartOfDay();
                }
            }

MavLinkNode/missionprocess.h

@@ -80,10 +80,8 @@ private slots:
    //状态机
    void ReadStateMachine(void);
    void WriteStateMachine(void);
-   //航线传输
-   //void ReadMissionRequest();
-
 
+   //所有的相关函数
    void request_list(void);
    void count(uint16_t count);
    void request_int(uint16_t seq);

MavLinkNode/parameterprocess.cpp

@@ -2,7 +2,7 @@
 
 ParameterProcess::ParameterProcess(QObject *parent) : QObject(parent)
 {
-    setRunFrq(200);
+    setRunFrq(1000);
 }
 
 void ParameterProcess::setRunFrq(uint32_t frq)
@@ -56,7 +56,13 @@ void ParameterProcess::process()//线程函数
          count ++;
          QThread::msleep(1000/running_frq);
 
-
+         switch(status.m_Mode)
+         {
+            default:
+            case Nop_Mode     :break;
+            case RecieveMode  : ReadStateMachine();break;
+            case TransmitMode : WriteStateMachine();break;
+         }
 
     }
     //退出线程
@@ -75,6 +81,25 @@ void ParameterProcess::SendMessage(mavlink_message_t msg)
 }
 
 
+void ParameterProcess::ReadCmd(uint8_t m_sysid, uint8_t m_compid,uint8_t type)
+{
+    sysid = m_sysid;
+    compid = m_compid;
+    status.m_Mode = RecieveMode;//接收模式
+    status.recieve.type = (type)?(_readtype::One):(_readtype::All);
+    start();//开启线程
+}
+
+void ParameterProcess::WriteCmd(uint8_t m_sysid, uint8_t m_compid ,uint32_t count )
+{
+    sysid = m_sysid;
+    compid = m_compid;
+    //mission_status.transmit.count = count;
+    status.m_Mode = TransmitMode;//发送模式
+
+    start();//开启线程
+}
+
 
 //这个函数类似中断，专门处理接收到的状态
 void ParameterProcess::ParameterParse(mavlink_message_t msg)
@@ -85,6 +110,9 @@ void ParameterProcess::ParameterParse(mavlink_message_t msg)
     }break;
     case MAVLINK_MSG_ID_PARAM_VALUE:{//
          mavlink_msg_param_value_decode(&msg,&param_value);
+         status.recieve.isWaitingforValue = false;
+         status.transmit.isWaitingforValue = false;
+         qDebug() << "param_value.param_index" << param_value.param_index;
     }break;
     case MAVLINK_MSG_ID_PARAM_EXT_ACK:{
          mavlink_msg_param_ext_ack_decode(&msg,&param_ext_ack);
@@ -112,23 +140,142 @@ void ParameterProcess::ParameterParse(mavlink_message_t msg)
 }
 
 
+//读航线状态机
+void ParameterProcess::ReadStateMachine(void)//一整列
+{
+   static uint8_t step = 0;
+   static uint8_t timeout_count = 0;
+   static uint32_t time = 0;
+
+
+   if(step == 0)
+   {
+       qDebug() << "start reading parameter";
+       status.recieve.isWaitingforValue = true;
+
+       if(status.recieve.type == _readtype::All)
+       {
+         request_list();//一整列,也可以一个,龚老师飞控并不支持这个
+       }
+       else if(status.recieve.type == _readtype::One)
+       {
+         request_read("xxx",0);//读取0
+       }
+       step++;
+       time = QTime::currentTime().msecsSinceStartOfDay();
+   }
+   else if(step == 1)//等待收到Item
+   {
+       //如果超时，那么结束
+       if((QTime::currentTime().msecsSinceStartOfDay() - time) > 1000)
+       {
+          qDebug() << "parameter item time out ";
+          step = 2;
+          timeout_count = 0;
+          qDebug() << "parameter read fail";
+
+       }
+       else//如果没有超时
+       {
+           if(status.recieve.isWaitingforValue == false)//收到item
+           {
+              qDebug() << "parameter item reccieved";
+              timeout_count = 0;
+
+              time = QTime::currentTime().msecsSinceStartOfDay();
+
+              //如果参数count 大于当前index，那么继续等待
+              if((param_value.param_index+1) < param_value.param_count)
+              {
+
+                status.recieve.isWaitingforValue = true;
+                if(status.recieve.type == _readtype::One)
+                {
+                    request_read("xxx",param_value.param_index+1);//读取
+                }
+
+              }
+              else
+              {
+                 step ++;
+              }
+           }
+       }
+
+   }
+   else if(step == 2)//传输结束
+   {
+      qDebug() << "step" << step << "recieve ok";
+      step = 0;
+      status.m_Mode = Nop_Mode;//切换到什么都不做
+      timeout_count = 0;
+      status.recieve.isWaitingforValue = false;
+   }
+}
+
+//写航线状态机
+void ParameterProcess::WriteStateMachine(void)
+{
+   static uint8_t step = 0;
+   static uint8_t timeout_count = 0;
+   static uint32_t time = 0;
+
+   if(step == 0)
+   {
+       //向其他线程或者自己读取航点的数量
+       status.transmit.isWaitingforValue = true;
+
+       set("id",0,1);//发送一个参数
+       time = QTime::currentTime().msecsSinceStartOfDay();
+       step++;//下一个阶段
+   }
+   else if(step == 1)//等待返回
+   {
+       if((QTime::currentTime().msecsSinceStartOfDay() - time) > 1000)
+       {
+          timeout_count ++;
+          step = 0;//返回上一个步骤
+          if(timeout_count > 5)//如果指令发5次还没发出去，那么结束发送
+          {
+             qDebug() << "parameter set value fail";
+             step = 0;
+             status.m_Mode = Nop_Mode;
+             status.transmit.isWaitingforValue = false;
+             timeout_count = 0;
+          }
+       }
+       else
+       {
+           //收到Value
+           if(status.transmit.isWaitingforValue == false)
+           {
+               step = 0;
+               status.m_Mode = Nop_Mode;
+               status.transmit.isWaitingforValue = false;
+               timeout_count = 0;
+           }
+       }
+   }
+
+}
+
 
 
 
 
 /*all msg
- MAVLINK_MSG_ID_PARAM_SET
+ MAVLINK_MSG_ID_PARAM_SET//
- MAVLINK_MSG_ID_PARAM_VALUE
+ MAVLINK_MSG_ID_PARAM_VALUE//
  MAVLINK_MSG_ID_PARAM_EXT_ACK
  MAVLINK_MSG_ID_PARAM_EXT_SET
  MAVLINK_MSG_ID_PARAM_EXT_VALUE
- MAVLINK_MSG_ID_PARAM_REQUEST_LIST
+ MAVLINK_MSG_ID_PARAM_REQUEST_LIST//
- MAVLINK_MSG_ID_PARAM_REQUEST_READ
+ MAVLINK_MSG_ID_PARAM_REQUEST_READ//
  MAVLINK_MSG_ID_PARAM_EXT_REQUEST_LIST
  MAVLINK_MSG_ID_PARAM_EXT_REQUEST_READ
 */
 
-void ParameterProcess::set(char id[16],uint8_t type,float value)//
+void ParameterProcess::set(const char *id,uint8_t type,float value)//
 {
     static mavlink_message_t msg;
     static mavlink_param_set_t param_set;
@@ -180,7 +327,7 @@ void ParameterProcess::ext_ack(float value)
     SendMessage(msg);
 }
 
-void ParameterProcess::ext_set(char id[16],uint8_t type,float value)
+void ParameterProcess::ext_set(const char *id,uint8_t type,float value)
 {
     static mavlink_message_t msg;
     static mavlink_param_ext_set_t param_ext_set;
@@ -226,7 +373,7 @@ void ParameterProcess::request_list(void)//
     SendMessage(msg);
 }
 
-void ParameterProcess::request_read(const char id[16],int16_t index)//
+void ParameterProcess::request_read(const char *id, int16_t index)//
 {
     static mavlink_message_t msg;
     static mavlink_param_request_read_t param_request_read;

MavLinkNode/parameterprocess.h

@@ -16,9 +16,53 @@ class ParameterProcess : public QObject
 {
 #endif
     Q_OBJECT
+
+
+    enum _modetype {
+        Nop_Mode = 0,
+        RecieveMode,
+        TransmitMode
+    };
+
+    enum _readtype {
+        All = 0,
+        One = 1
+    };
+
+
+    typedef struct {
+        //bool isWaitingforCount;
+        bool isWaitingforValue;
+
+        _readtype type;
+
+    }_recieve;
+
+    typedef struct {
+       uint32_t count;
+       //bool isWaiteforRequest;
+       bool isWaitingforValue;
+    }_transmit;
+
+    typedef struct
+    {
+        _recieve recieve;
+        _transmit transmit;
+
+
+        _modetype m_Mode;
+
+    }_parameter_;
+
+
 public:
     explicit ParameterProcess(QObject *parent = nullptr);
 
+    _parameter_ status;
+
+    //读取和写入指令
+    void ReadCmd(uint8_t m_sysid, uint8_t m_compid, uint8_t type);
+    void WriteCmd(uint8_t m_sysid, uint8_t m_compid ,uint32_t count );
 
 public slots:
     //线程对外接口
@@ -35,14 +79,19 @@ private slots:
 
    void process();
 
+   //状态机
+   void ReadStateMachine(void);
+   void WriteStateMachine(void);
 
-   void set(char id[], uint8_t type, float value);
+
+   //所有的相关函数
+   void set(const char *id, uint8_t type, float value);
    void value(float value);
    void ext_ack(float value);
-   void ext_set(char id[], uint8_t type, float value);
+   void ext_set(const char *id, uint8_t type, float value);
    void ext_value(float value);
    void request_list(void);
-   void request_read(const char id[], int16_t index);
+   void request_read(const char *id, int16_t index);
 
 signals:
    void readError();

dlink/dlink.cpp

@@ -10,7 +10,13 @@ DLink::DLink(QObject *parent) : QObject(parent)
      qDebug() << "Dlink " <<  QThread::currentThreadId();
 
      mavlinknode = new MavLinkNode();//不允许带参数,因为这是单独的线程
-     connect(mavlinknode,SIGNAL(SendMessageTo(quint8,quint8*,quint16)),
+     /*connect(mavlinknode,SIGNAL(SendMessageTo(quint8,quint8*,quint16)),
+            this,SLOT(SendMessageTo(quint8,quint8*,quint16)));*/
+
+     connect(mavlinknode->Mission,SIGNAL(SendMessageTo(quint8,quint8*,quint16)),
+            this,SLOT(SendMessageTo(quint8,quint8*,quint16)));
+
+     connect(mavlinknode->Parameter,SIGNAL(SendMessageTo(quint8,quint8*,quint16)),
             this,SLOT(SendMessageTo(quint8,quint8*,quint16)));
 }
 DLink::~DLink()
@@ -22,12 +28,15 @@ DLink::~DLink()
 
 int DLink::SendMessageTo(quint8 ch, quint8 *msg, quint16 len)
 {
+    Q_UNUSED(ch);
+
     if (DLink::Clientsock)
     {
         foreach(Node node,clientSockets)
         {
             DLink::Clientsock->writeDatagram((const char *)msg, len,
                                        node.addr, node.port);
+            //qDebug() << "Client Send Msg";
         }
     }