添加所有的航线相关的函数

MavLinkNode/missionprocess.cpp

@@ -57,6 +57,12 @@ void MissionProcess::process()//线程函数
          QThread::msleep(1000/running_frq);
          //状态机
 
+         //读取航线
+
+
+
+
+
 
          //如果要读取航线，线发送读取指令，然后等待返回，如果返回，那么启动线程进入循环接收
 
@@ -75,8 +81,7 @@ void MissionProcess::MissionParse(mavlink_message_t msg)
         //航线部分
         case MAVLINK_MSG_ID_MISSION_COUNT: {//飞控发来计数值，然后开始读取
            mavlink_msg_mission_count_decode(&msg,&mission_count);
-
+           isRecieveCount = true;//收到了计数，那可以开始读取第一个
-           //Mavlink_msg_mission_request_int(0);
 
         }break;
         case MAVLINK_MSG_ID_MISSION_ITEM: {
@@ -107,9 +112,313 @@ void MissionProcess::MissionParse(mavlink_message_t msg)
 }
 
 
+/*All msg
+ *
+ *  MISSION_REQUEST_LIST
+ *  MISSION_COUNT
+ *  MISSION_REQUEST_INT
+ *  MISSION_REQUEST
+ *  MISSION_ITEM_INT
+ *  MISSION_ITEM
+ *  MISSION_ACK
+ * MISSION_CURRENT
+ * MISSION_SET_CURRENT
+ * STATUSTEXT
+ *  MISSION_CLEAR_ALL
+ * MISSION_ITEM_REACHED
+ * MISSION_REQUEST_PARTIAL_LIST
+ * MISSION_WRITE_PARTIAL_LIST
+ *
+ *
+ **/
+
+//读航线状态机
+void MissionProcess::ReadStateMachine(void)
+{
+   static uint8_t step = 0;
+
+   if(isReadMission == true) //如果收到读取航线的指令
+   {
+       if(step == 0)//第一步 发送请求
+       {
+           //MISSION_REQUEST_LIST
+           //启动定时器，等待超时
+       }
+       else if(step == 1)//等待收到Count
+       {
+            //如果收到MISSION_COUNT
+           //step ++;
+
+       }
+       else if(step ==3)//请求航点
+       {
+           //
+           //如果当前航点没有是第（n-1）航点，就正常传输MISSION_ITEM_INT，MISSION_ITEM
+           //如果当前已经是第（n-1）个，那么下一步step++,
+       }
+       else if(step == 4)//航线传输结束，发送ack
+       {
+          //发送 MISSION_ACK
+       }
+
+   }
+}
 
 
 
 
 
 
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+void MissionProcess::request_list(void)//读取整列请求
+{
+    static mavlink_message_t MAVLink_Msg;          //MAVLink协议信息(MSG)
+
+    uint8_t  MAVLink_Buf[256+20];                   //发送的缓存(注意大小, 3代表两个参数的长度)
+    uint16_t MAVLink_Len;                          //发送的长度
+    static mavlink_mission_request_list_t mission_request_list;
+
+
+
+     mission_request_list.mission_type = 0;
+     mission_request_list.target_system = vehicle.sysid;
+     mission_request_list.target_component = vehicle.compid;
+    //打包消息(到MAVLink_msg)
+    MAVLink_Len = mavlink_msg_mission_request_list_encode(2,MAV_COMP_ID_MISSIONPLANNER, &MAVLink_Msg,&mission_request_list);
+    //根据消息得到Buf和Len
+    MAVLink_Len = mavlink_msg_to_send_buffer(MAVLink_Buf, &MAVLink_Msg);
+    //通过串口发送
+    SendMessageTo(0,MAVLink_Buf, MAVLink_Len);
+
+    qDebug() << "mission request list";
+
+}
+
+void MissionProcess::count(uint16_t count)//计数值
+{
+    static mavlink_message_t MAVLink_Msg;
+    uint8_t  MAVLink_Buf[256+20];
+    uint16_t MAVLink_Len;
+    static mavlink_mission_count_t mission_count;
+
+     mission_count.count = count;
+     mission_count.mission_type = 0;
+     mission_count.target_system = vehicle.sysid;
+     mission_count.target_component = vehicle.compid;
+
+    MAVLink_Len = mavlink_msg_mission_count_encode(2,MAV_COMP_ID_MISSIONPLANNER, &MAVLink_Msg,&mission_count);
+    MAVLink_Len = mavlink_msg_to_send_buffer(MAVLink_Buf, &MAVLink_Msg);
+    SendMessageTo(0,MAVLink_Buf, MAVLink_Len);
+}
+
+void MissionProcess::request_int(uint16_t seq)//读取请求
+{
+    static mavlink_message_t MAVLink_Msg;          //MAVLink协议信息(MSG)
+
+    uint8_t  MAVLink_Buf[256+20];                   //发送的缓存(注意大小, 3代表两个参数的长度)
+    uint16_t MAVLink_Len;                          //发送的长度
+    static mavlink_mission_request_int_t mission_request_int;
+
+     mission_request_int.seq = seq;
+     mission_request_int.mission_type = 0;
+     mission_request_int.target_system = vehicle.sysid;
+     mission_request_int.target_component = vehicle.compid;
+
+    MAVLink_Len = mavlink_msg_mission_request_int_encode(2, MAV_COMP_ID_MISSIONPLANNER, &MAVLink_Msg,&mission_request_int);
+    MAVLink_Len = mavlink_msg_to_send_buffer(MAVLink_Buf, &MAVLink_Msg);
+    SendMessageTo(0,MAVLink_Buf, MAVLink_Len);
+}
+
+void MissionProcess::request(uint16_t seq)//读取请求
+{
+    static mavlink_message_t MAVLink_Msg;          //MAVLink协议信息(MSG)
+
+    uint8_t  MAVLink_Buf[256+20];                   //发送的缓存(注意大小, 3代表两个参数的长度)
+    uint16_t MAVLink_Len;                          //发送的长度
+    static mavlink_mission_request_t mission_request;
+
+     mission_request.mission_type = 0;
+     mission_request.seq = seq;
+     mission_request.target_system = vehicle.sysid;
+     mission_request.target_component = vehicle.compid;
+    MAVLink_Len = mavlink_msg_mission_request_encode(2, MAV_COMP_ID_MISSIONPLANNER, &MAVLink_Msg,&mission_request);
+    MAVLink_Len = mavlink_msg_to_send_buffer(MAVLink_Buf, &MAVLink_Msg);
+    SendMessageTo(0,MAVLink_Buf, MAVLink_Len);
+}
+
+void MissionProcess::item_int(float param1,float param2,float param3,float param4,
+                                     float x,float y,float z,
+                                     uint16_t seq,uint16_t command,
+                                     uint8_t frame,uint8_t current,
+                                     uint8_t autocontinue,uint8_t mission_type)
+{
+    static mavlink_message_t MAVLink_Msg;          //MAVLink协议信息(MSG)
+
+    uint8_t  MAVLink_Buf[256+20];                   //发送的缓存(注意大小, 3代表两个参数的长度)
+    uint16_t MAVLink_Len;                          //发送的长度
+    static mavlink_mission_item_t mission_item;
+
+    mission_item.target_system = vehicle.sysid;
+    mission_item.target_component = vehicle.compid;
+
+    mission_item.param1 = param1;
+    mission_item.param2 = param2;
+    mission_item.param3 = param3;
+    mission_item.param4 = param4;
+
+    mission_item.x = x;
+    mission_item.y = y;
+    mission_item.z = z;
+
+    mission_item.seq = seq;
+    mission_item.command = command;
+    mission_item.frame = frame;
+    mission_item.current = current;
+    mission_item.autocontinue = autocontinue;
+    mission_item.mission_type = mission_type;
+
+    MAVLink_Len = mavlink_msg_mission_item_encode(2,MAV_COMP_ID_MISSIONPLANNER, &MAVLink_Msg,&mission_item);
+    MAVLink_Len = mavlink_msg_to_send_buffer(MAVLink_Buf, &MAVLink_Msg);
+    SendMessageTo(0,MAVLink_Buf, MAVLink_Len);
+}
+
+void MissionProcess::item(float param1,float param2,float param3,float param4,
+                                     float x,float y,float z,
+                                     uint16_t seq,uint16_t command,
+                                     uint8_t frame,uint8_t current,
+                                     uint8_t autocontinue,uint8_t mission_type)
+{
+    static mavlink_message_t MAVLink_Msg;          //MAVLink协议信息(MSG)
+
+    uint8_t  MAVLink_Buf[256+20];                   //发送的缓存(注意大小, 3代表两个参数的长度)
+    uint16_t MAVLink_Len;                          //发送的长度
+    static mavlink_mission_item_t mission_item;
+
+    mission_item.target_system = vehicle.sysid;
+    mission_item.target_component = vehicle.compid;
+
+    mission_item.param1 = param1;
+    mission_item.param2 = param2;
+    mission_item.param3 = param3;
+    mission_item.param4 = param4;
+
+    mission_item.x = x;
+    mission_item.y = y;
+    mission_item.z = z;
+
+    mission_item.seq = seq;
+    mission_item.command = command;
+    mission_item.frame = frame;
+    mission_item.current = current;
+    mission_item.autocontinue = autocontinue;
+    mission_item.mission_type = mission_type;
+
+    //打包消息(到MAVLink_msg)
+    MAVLink_Len = mavlink_msg_mission_item_encode(2,MAV_COMP_ID_MISSIONPLANNER, &MAVLink_Msg,&mission_item);
+    //根据消息得到Buf和Len
+    MAVLink_Len = mavlink_msg_to_send_buffer(MAVLink_Buf, &MAVLink_Msg);
+    //通过串口发送
+    SendMessageTo(0,MAVLink_Buf, MAVLink_Len);
+}
+
+void MissionProcess::ack(void)
+{
+    static mavlink_message_t MAVLink_Msg;          //MAVLink协议信息(MSG)
+
+    uint8_t  MAVLink_Buf[256+20];                   //发送的缓存(注意大小, 3代表两个参数的长度)
+    uint16_t MAVLink_Len;                          //发送的长度
+    static mavlink_mission_ack_t mission_ack;
+
+    mission_ack.target_system = vehicle.sysid;
+    mission_ack.target_component = vehicle.compid;
+    mission_ack.mission_type = 0;
+    mission_ack.type = 0;
+
+    //打包消息(到MAVLink_msg)
+    MAVLink_Len = mavlink_msg_mission_ack_encode(2,MAV_COMP_ID_MISSIONPLANNER, &MAVLink_Msg,&mission_ack);
+    //根据消息得到Buf和Len
+    MAVLink_Len = mavlink_msg_to_send_buffer(MAVLink_Buf, &MAVLink_Msg);
+    //通过串口发送
+    SendMessageTo(0,MAVLink_Buf, MAVLink_Len);
+
+
+}
+
+void MissionProcess::current(void)
+{
+    static mavlink_message_t msg;          //MAVLink协议信息(MSG)
+
+    uint8_t  MAVLink_Buf[256+20];                   //发送的缓存(注意大小, 3代表两个参数的长度)
+    uint16_t MAVLink_Len;                          //发送的长度
+    static mavlink_mission_set_current_t mission_current;
+
+    //打包消息(到MAVLink_msg)
+    MAVLink_Len = mavlink_msg_mission_current_encode(2,MAV_COMP_ID_MISSIONPLANNER, &msg,&mission_current);
+    //根据消息得到Buf和Len
+    MAVLink_Len = mavlink_msg_to_send_buffer(MAVLink_Buf, &msg);
+    //通过串口发送
+    SendMessageTo(0,MAVLink_Buf, MAVLink_Len);
+}
+
+void MissionProcess::setcurrent(void)
+{
+    static mavlink_message_t msg;          //MAVLink协议信息(MSG)
+
+    uint8_t  MAVLink_Buf[256+20];                   //发送的缓存(注意大小, 3代表两个参数的长度)
+    uint16_t MAVLink_Len;                          //发送的长度
+    static mavlink_mission_set_current_t mission_set_current;
+
+    //打包消息(到MAVLink_msg)
+    MAVLink_Len = mavlink_msg_mission_set_current_encode(2,MAV_COMP_ID_MISSIONPLANNER, &msg,&mission_set_current);
+    //根据消息得到Buf和Len
+    MAVLink_Len = mavlink_msg_to_send_buffer(MAVLink_Buf, &msg);
+    //通过串口发送
+    SendMessageTo(0,MAVLink_Buf, MAVLink_Len);
+}
+
+void MissionProcess::clear_all(void)
+{
+    static mavlink_message_t MAVLink_Msg;          //MAVLink协议信息(MSG)
+
+    uint8_t  MAVLink_Buf[256+20];                   //发送的缓存(注意大小, 3代表两个参数的长度)
+    uint16_t MAVLink_Len;                          //发送的长度
+    static mavlink_mission_clear_all_t mission_clear_all;
+
+    mission_clear_all.target_system = vehicle.sysid;
+    mission_clear_all.target_component = vehicle.compid;
+    mission_clear_all.mission_type = 0;
+
+    //打包消息(到MAVLink_msg)
+    MAVLink_Len = mavlink_msg_mission_clear_all_encode(2,MAV_COMP_ID_MISSIONPLANNER, &MAVLink_Msg,&mission_clear_all);
+    //根据消息得到Buf和Len
+    MAVLink_Len = mavlink_msg_to_send_buffer(MAVLink_Buf, &MAVLink_Msg);
+    //通过串口发送
+    SendMessageTo(0,MAVLink_Buf, MAVLink_Len);
+}
+
+
+
+
+

MavLinkNode/missionprocess.h

@@ -2,7 +2,7 @@
 #define MISSIONPROCESS_H
 
 #include <QObject>
-//#include "mavlinknode.h"
+#include "mavlinknode.h"
 
 #include "QDebug"
 #include "QThread"
@@ -33,11 +33,20 @@ private slots:
    //线程私有接口
    void process();
 
+
+
+   //航线读取传输
+   void ReadMissionRequest();
+
+
 signals:
 
 
 private:
 
+   uint8_t sysid;
+   uint8_t compid;
+
 
     mavlink_mission_count_t mission_count;
     mavlink_mission_item_t mission_item;
@@ -52,6 +61,20 @@ private:
     quint32 running_frq = 200;//200Hz
     QThread *Missionthread = nullptr;
 
+
+
+
+    bool isRecieveCount = false;
+
+
+    bool isReadMission = false;
+
+
 };
 
+
+void SendMessageTo(uint8_t ch, QByteArray data,uint16_t len);
+
+
+
 #endif // MISSIONPROCESS_H