保存程序

2020-08-05 22:39:33 +08:00
parent f33888d422
commit 4d24e68e7d
14 changed files with 320 additions and 91 deletions
@@ -206,22 +206,29 @@ void MissionProcess::transmitPoint(float param1,float param2,float param3,float
 //这个函数类似中断，专门处理接收到的状态
 void MissionProcess::Parse(mavlink_message_t msg)
 {
-    if(msg.sysid == Current_sysID)
-    {
-        qDebug() << "is my mission";
-    }
-
    switch (msg.msgid) {
    case MAVLINK_MSG_ID_MISSION_REQUEST_LIST: {
       mavlink_msg_mission_request_list_decode(&msg,&mission_request_list);
    }break;
    case MAVLINK_MSG_ID_MISSION_COUNT: {
       mavlink_msg_mission_count_decode(&msg,&mission_count);
+
+       if(mission_count.target_system != Current_sysID)
+       {
+           break;//如果目标系统不是自己，那么就抛弃该指令
+       }
+
       qDebug() << "mission_count" << mission_count.count;
       mission_status.recieve.isWaitingforCount = false;
    }break;
    case MAVLINK_MSG_ID_MISSION_REQUEST_INT: {
       mavlink_msg_mission_request_int_decode(&msg,&mission_request_int);
+
+       if(mission_count.target_system != Current_sysID)
+       {
+           break;//如果目标系统不是自己，那么就抛弃该指令
+       }
+
       qDebug() << "recieve mission_request_int ";
       emit sendItemOK(mission_item_int.seq,true);
       mission_item_int.seq++;
@@ -229,6 +236,12 @@ void MissionProcess::Parse(mavlink_message_t msg)
    }break;
    case MAVLINK_MSG_ID_MISSION_REQUEST: {
       mavlink_msg_mission_request_decode(&msg,&mission_request);
+
+       if(mission_request.target_system != Current_sysID)
+       {
+           break;//如果目标系统不是自己，那么就抛弃该指令
+       }
+
       qDebug() << "recieve mission_request ";
       emit sendItemOK(mission_item_int.seq,true);
       mission_item_int.seq++;
@@ -236,6 +249,12 @@ void MissionProcess::Parse(mavlink_message_t msg)
    }break;
    case MAVLINK_MSG_ID_MISSION_ITEM_INT: {
       mavlink_msg_mission_item_int_decode(&msg,&mission_item_int);
+
+       if(mission_item_int.target_system != Current_sysID)
+       {
+           break;//如果目标系统不是自己，那么就抛弃该指令
+       }
+
       qDebug() << "recieve mission " << mission_item_int.seq;

       //把航点发出去
@@ -254,20 +273,28 @@ void MissionProcess::Parse(mavlink_message_t msg)
    }break;
    case MAVLINK_MSG_ID_MISSION_ITEM: {
       mavlink_msg_mission_item_decode(&msg,&mission_item);
+
+       if(mission_item.target_system != Current_sysID)
+       {
+           break;//如果目标系统不是自己，那么就抛弃该指令
+       }
+
       mission_status.recieve.isWaitingforItem = false;
    }break;
    case MAVLINK_MSG_ID_MISSION_ACK: {
       mavlink_msg_mission_ack_decode(&msg,&mission_ack);
+
+       if(mission_ack.target_system != Current_sysID)
+       {
+           break;//如果目标系统不是自己，那么就抛弃该指令
+       }
+
       mission_status.transmit.isWaiteforACK = false;
    }break;
    case MAVLINK_MSG_ID_MISSION_CURRENT: {
       mavlink_msg_mission_current_decode(&msg,&mission_current);
-       //if(mission_current.seq == mission_status.transmit.seq)
-       {
-           //收到设置当前点，对比如果一致，那么成功，否则失败
-           //qDebug() << "mission_current" << mission_current.seq;
-           emit currentPoint(mission_current.seq);
-       }
+
+       emit currentPoint(mission_current.seq);

    }break;
    case MAVLINK_MSG_ID_MISSION_SET_CURRENT: {
@@ -596,7 +623,7 @@ void MissionProcess::request_list(void)//读取整列请求
     mission_request_list.target_system = sysid;
     mission_request_list.target_component = compid;

-    mavlink_msg_mission_request_list_encode(Current_sysID,MAV_COMP_ID_MISSIONPLANNER, &msg,&mission_request_list);
+    mavlink_msg_mission_request_list_encode(Current_sysID,Current_CompID, &msg,&mission_request_list);
    SendMessage(msg);
 }

@@ -610,7 +637,7 @@ void MissionProcess::count(uint16_t count)//计数值
     mission_count.target_system = sysid;
     mission_count.target_component = compid;

-    mavlink_msg_mission_count_encode(Current_sysID,MAV_COMP_ID_MISSIONPLANNER, &msg,&mission_count);
+    mavlink_msg_mission_count_encode(Current_sysID,Current_CompID, &msg,&mission_count);
    SendMessage(msg);
 }

@@ -624,7 +651,7 @@ void MissionProcess::request_int(uint16_t seq)//读取请求
     mission_request_int.target_system = sysid;
     mission_request_int.target_component = compid;

-    mavlink_msg_mission_request_int_encode(Current_sysID, MAV_COMP_ID_MISSIONPLANNER, &msg,&mission_request_int);
+    mavlink_msg_mission_request_int_encode(Current_sysID, Current_CompID, &msg,&mission_request_int);
    SendMessage(msg);
 }

@@ -637,7 +664,7 @@ void MissionProcess::request(uint16_t seq)//读取请求
    mission_request.seq = seq;
    mission_request.target_system = sysid;
    mission_request.target_component = compid;
-    mavlink_msg_mission_request_encode(Current_sysID, MAV_COMP_ID_MISSIONPLANNER, &msg,&mission_request);
+    mavlink_msg_mission_request_encode(Current_sysID, Current_CompID, &msg,&mission_request);
    SendMessage(msg);
 }

@@ -669,7 +696,7 @@ void MissionProcess::item_int(float param1,float param2,float param3,float param
    mission_item_int.autocontinue = autocontinue;
    mission_item_int.mission_type = mission_type;

-    mavlink_msg_mission_item_int_encode(Current_sysID,MAV_COMP_ID_MISSIONPLANNER, &msg,&mission_item_int);
+    mavlink_msg_mission_item_int_encode(Current_sysID,Current_CompID, &msg,&mission_item_int);
    SendMessage(msg);
 }

@@ -701,7 +728,7 @@ void MissionProcess::item(float param1,float param2,float param3,float param4,
    mission_item.autocontinue = autocontinue;
    mission_item.mission_type = mission_type;

-    mavlink_msg_mission_item_encode(Current_sysID,MAV_COMP_ID_MISSIONPLANNER, &msg,&mission_item);
+    mavlink_msg_mission_item_encode(Current_sysID,Current_CompID, &msg,&mission_item);
    SendMessage(msg);
 }

@@ -715,7 +742,7 @@ void MissionProcess::ack(void)
    mission_ack.mission_type = 0;
    mission_ack.type = 0;

-    mavlink_msg_mission_ack_encode(Current_sysID,MAV_COMP_ID_MISSIONPLANNER, &msg,&mission_ack);
+    mavlink_msg_mission_ack_encode(Current_sysID,Current_CompID, &msg,&mission_ack);
    SendMessage(msg);


@@ -726,7 +753,7 @@ void MissionProcess::current(void)
    static mavlink_message_t msg;
    static mavlink_mission_current_t mission_current;

-    mavlink_msg_mission_current_encode(Current_sysID,MAV_COMP_ID_MISSIONPLANNER, &msg,&mission_current);
+    mavlink_msg_mission_current_encode(Current_sysID,Current_CompID, &msg,&mission_current);
    SendMessage(msg);
 }

@@ -740,7 +767,7 @@ void MissionProcess::setcurrent(uint16_t seq)

    mission_set_current.seq = seq;

-    mavlink_msg_mission_set_current_encode(Current_sysID,MAV_COMP_ID_MISSIONPLANNER, &msg,&mission_set_current);
+    mavlink_msg_mission_set_current_encode(Current_sysID,Current_CompID, &msg,&mission_set_current);
    SendMessage(msg);
 }

@@ -753,7 +780,7 @@ void MissionProcess::clear_all(void)
    mission_clear_all.target_component = compid;
    mission_clear_all.mission_type = 0;

-    mavlink_msg_mission_clear_all_encode(Current_sysID,MAV_COMP_ID_MISSIONPLANNER, &msg,&mission_clear_all);
+    mavlink_msg_mission_clear_all_encode(Current_sysID,Current_CompID, &msg,&mission_clear_all);
    SendMessage(msg);
 }

@@ -762,7 +789,7 @@ void MissionProcess::item_reached(void)
    static mavlink_message_t msg;
    static mavlink_mission_item_reached_t mission_item_reached;

-    mavlink_msg_mission_item_reached_encode(Current_sysID,MAV_COMP_ID_MISSIONPLANNER, &msg,&mission_item_reached);
+    mavlink_msg_mission_item_reached_encode(Current_sysID,Current_CompID, &msg,&mission_item_reached);
    SendMessage(msg);
 }

@@ -771,7 +798,7 @@ void MissionProcess::request_partial_list(void)
     static mavlink_message_t msg;
     static mavlink_mission_request_partial_list_t mission_request_partial_list;

-     mavlink_msg_mission_request_partial_list_encode(Current_sysID,MAV_COMP_ID_MISSIONPLANNER, &msg,&mission_request_partial_list);
+     mavlink_msg_mission_request_partial_list_encode(Current_sysID,Current_CompID, &msg,&mission_request_partial_list);
     SendMessage(msg);
 }

@@ -780,6 +807,6 @@ void MissionProcess::write_partial_list(void)
     static mavlink_message_t msg;
     static mavlink_mission_write_partial_list_t mission_write_partial_list;

-     mavlink_msg_mission_write_partial_list_encode(Current_sysID,MAV_COMP_ID_MISSIONPLANNER, &msg,&mission_write_partial_list);
+     mavlink_msg_mission_write_partial_list_encode(Current_sysID,Current_CompID, &msg,&mission_write_partial_list);
     SendMessage(msg);
 }