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); + } + } + /// /// 生成通信模块packet并且发送 /// 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 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; + } + /// /// 读取参数