保存程序

MavLinkNode/commandprocess.cpp

@@ -153,10 +153,14 @@ void commandprocess::WriteCmd_long(float param1, float param2,  float param3, fl
 //这个函数类似中断，专门处理接收到的状态
 void commandprocess::Parse(mavlink_message_t msg)
 {
+
+
     mavlink_command_int_t command_int;
     mavlink_command_long_t command_long;
     mavlink_command_ack_t command_ack;
 
+
+
     switch (msg.msgid) {
     case MAVLINK_MSG_ID_COMMAND_INT://几乎不可能收到
          mavlink_msg_command_int_decode(&msg,&command_int);
@@ -167,6 +171,11 @@ void commandprocess::Parse(mavlink_message_t msg)
     case MAVLINK_MSG_ID_COMMAND_ACK:
          mavlink_msg_command_ack_decode(&msg,&command_ack);
 
+         if(command_ack.target_system != Current_sysID)
+         {
+             break;//如果目标系统不是自己，那么就抛弃该指令
+         }
+
          if(command_ack.result == MAV_RESULT_ACCEPTED)
          {
              status.transmit.isWaitingforACK = false;
@@ -417,7 +426,7 @@ void commandprocess::ack(uint16_t command,uint8_t result,uint8_t progress,int32_
     command_ack.progress = progress;
     command_ack.result_param2 = result_param2;
 
-    mavlink_msg_command_ack_encode(Current_sysID,MAV_COMP_ID_MISSIONPLANNER, &msg,&command_ack);
+    mavlink_msg_command_ack_encode(Current_sysID,Current_CompID, &msg,&command_ack);
     SendMessage(msg);
 }