From e607ab4c7d140b443945367e047ca8c97f5defc4 Mon Sep 17 00:00:00 2001
From: zxd <zxdaa217@qq.com>
Date: Tue, 9 Jul 2019 14:49:24 +0800
Subject: [PATCH] =?UTF-8?q?=E6=B7=BB=E5=8A=A0=E6=B5=8B=E8=AF=95=E6=8B=89?=
 =?UTF-8?q?=E7=83=9F=E7=9A=84=E5=8A=9F=E8=83=BD=20=E6=B7=BB=E5=8A=A0?=
 =?UTF-8?q?=E5=A4=9A=E5=8F=82=E6=95=B0=E5=90=8C=E6=97=B6=E5=86=99=E5=85=A5?=
 =?UTF-8?q?=EF=BC=88=E7=94=A8=E4=BA=8E=E4=B8=80=E9=94=AE=E5=85=B3=E9=97=AD?=
 =?UTF-8?q?=E6=B5=8B=E8=AF=95=E7=81=AF=E5=85=89=EF=BC=89?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

---
 .../CommunicationManagement/CommModule.cs     | 64 ++++++++++++++++++-
 .../CommModuleGenerateMavLink.cs              | 19 ++++++
 2 files changed, 82 insertions(+), 1 deletion(-)

diff --git a/PlaneGcsSdk_Shared/CommunicationManagement/CommModule.cs b/PlaneGcsSdk_Shared/CommunicationManagement/CommModule.cs
index 6928324..cfb6856 100644
--- a/PlaneGcsSdk_Shared/CommunicationManagement/CommModule.cs
+++ b/PlaneGcsSdk_Shared/CommunicationManagement/CommModule.cs
@@ -331,11 +331,73 @@ namespace Plane.CommunicationManagement
             temp = !temp;
             while (temp)
             {
-                await WriteCommPacketAsync(0, MavComm.COMM_DOWNLOAD_COMM, packet);
+                await WriteCommPacketAsync(id, MavComm.COMM_DOWNLOAD_COMM, packet);
                 await Task.Delay(1000).ConfigureAwait(false);
             } 
         }
 
+        public async Task TestFire(short id, int channel)
+        {
+
+            MAVLink.mavlink_msg_id_led_control led = new MAVLink.mavlink_msg_id_led_control();
+            led.target_system = 1;
+            led.target_component = 1;//(byte)MAVLink.MAV_COMPONENT.MAV_COMP_ID_SYSTEM_CONTROL;;
+            led.instance = 0;
+            led.pattern = 0;
+            led.custom_len = 1;
+            led.custom_bytes = new byte[24];
+
+            led.custom_bytes[0] = (byte)channel;
+            
+            byte[] data;
+            data = MavlinkUtil.StructureToByteArray(led);
+
+            byte[] packet = new byte[data.Length + 6 + 2];
+            packet[0] = MAVLink.MAVLINK_STX;
+
+            packet[1] = (byte)(data.Length);
+            packet[2] = 1;
+            packet[3] = 255; // this is always 255 - MYGCS
+
+            packet[4] = (byte)MAVLink.MAV_COMPONENT.MAV_COMP_ID_MISSIONPLANNER;
+
+            packet[5] = MAVLink.MAVLINK_MSG_ID_LED_CONTROL;
+            int i = 6;
+            foreach (byte b in data)
+            {
+                packet[i] = b;
+                i++;
+            }
+
+            ushort checksum = MavlinkCRC.crc_calculate(packet, packet[1] + 6);
+            checksum = MavlinkCRC.crc_accumulate(MAVLink.MAVLINK_MESSAGE_CRCS[MAVLink.MAVLINK_MSG_ID_LED_CONTROL], checksum);
+
+            byte ck_a = (byte)(checksum & 0xFF); ///< High byte
+            byte ck_b = (byte)(checksum >> 8); ///< Low byte
+
+            packet[i] = ck_a;
+            i += 1;
+            packet[i] = ck_b;
+            i += 1;
+
+
+            //await WriteCommPacketAsync(id, MavComm.COMM_DOWNLOAD_COMM, packet);
+            byte[] senddata = packet;
+
+            for (int times = 0; times < 3; times++)
+            {
+                senddata = senddata.Concat(packet).ToArray();
+            }
+
+
+            temp = !temp;
+            while (temp)
+            {
+                await WriteCommPacketAsync(id, MavComm.COMM_DOWNLOAD_COMM, packet);
+                await Task.Delay(1000).ConfigureAwait(false);
+            }
+        }
+
         /// <summary>
         /// 生成通信模块packet并且发送
         /// </summary>
diff --git a/PlaneGcsSdk_Shared/CommunicationManagement/CommModuleGenerateMavLink.cs b/PlaneGcsSdk_Shared/CommunicationManagement/CommModuleGenerateMavLink.cs
index 059555c..286897d 100644
--- a/PlaneGcsSdk_Shared/CommunicationManagement/CommModuleGenerateMavLink.cs
+++ b/PlaneGcsSdk_Shared/CommunicationManagement/CommModuleGenerateMavLink.cs
@@ -198,6 +198,25 @@ namespace Plane.CommunicationManagement
             return packetNum;
         }
 
+        public async Task<int> SetMultipleParamAsync(params string[] param)
+        {
+            if (param.Length % 2 == 1)
+                return 0;
+
+            byte[] packet = null;
+            int packetNum = 0;
+            for (int i = 0; i < param.Length; i += 2)
+            {
+                string paramname = param[i];
+                float value = float.Parse(param[i + 1]);
+                byte[] data = SetParam2Async(paramname, value);
+                packetNum = data[2];
+                packet = packet == null ? data : packet.Concat(data).ToArray();
+            }
+            await WriteCommPacketAsync(0, MavComm.COMM_DOWNLOAD_COMM, packet).ConfigureAwait(false);
+            return packetNum;
+        }
+
 
         /// <summary>
         /// 读取参数