diff --git a/PlaneGcsSdk_Shared/Communication/TcpConnection.cs b/PlaneGcsSdk_Shared/Communication/TcpConnection.cs index 58c086f..a4f3736 100644 --- a/PlaneGcsSdk_Shared/Communication/TcpConnection.cs +++ b/PlaneGcsSdk_Shared/Communication/TcpConnection.cs @@ -1,6 +1,7 @@ #if !NETFX_CORE using System; +using System.Net; using System.Net.Sockets; using System.Threading.Tasks; @@ -48,6 +49,23 @@ namespace Plane.Communication _stream = _client.GetStream(); } + public async Task BindIP(string ip) + { + bool bind = false; + try + { + IPAddress IPLocal = IPAddress.Parse(ip); + _client.Client.Bind(new IPEndPoint(IPLocal, 0)); + bind = true; + } + catch + { + bind = false; + } + await Task.Delay(10).ConfigureAwait(false); + return bind; + } + public override async Task OpenAsync() { if (!IsOpen) @@ -56,13 +74,16 @@ namespace Plane.Communication { await _client.ConnectAsync(_remoteHostname, _remotePort).ConfigureAwait(false); } + //屏蔽掉异常处理 + //CreateClientAndConnectAsync会new一个TcpClient并且重新连接 + //之前设置的TcpClient中Socket Bind会无效，在多网卡工作时会导致断线重连的时间特别长 catch (SocketException) { - await CreateClientAndConnectAsync(); + //await CreateClientAndConnectAsync(); } catch (ObjectDisposedException) { - await CreateClientAndConnectAsync(); + //await CreateClientAndConnectAsync(); } _isBroken = false; } diff --git a/PlaneGcsSdk_Shared/CommunicationManagement/CommConnection.cs b/PlaneGcsSdk_Shared/CommunicationManagement/CommConnection.cs new file mode 100644 index 0000000..a936856 --- /dev/null +++ b/PlaneGcsSdk_Shared/CommunicationManagement/CommConnection.cs @@ -0,0 +1,42 @@ +using Plane.Communication; +using System; +using System.Collections.Generic; +using System.Text; +using System.Threading.Tasks; + +namespace Plane.CommunicationManagement +{ + public class CommConnection : IConnection + { + public bool IsOpen { get; protected set; } + + public event EventHandler ExceptionThrown; + + public int BytesToRead() + { + return 0; + } + + public void Close() + { + IsOpen = false; + } + + public Task OpenAsync() + { + IsOpen = true; + return TaskUtils.CompletedTask; + } + + public async Task ReadAsync(byte[] buffer, int offset, int count) + { + await TaskUtils.CompletedTask; + return 0; + } + + public async Task WriteAsync(byte[] buffer, int offset, int count) + { + await TaskUtils.CompletedTask; + } + } +} diff --git a/PlaneGcsSdk_Shared/CommunicationManagement/CommModule.cs b/PlaneGcsSdk_Shared/CommunicationManagement/CommModule.cs new file mode 100644 index 0000000..77773a6 --- /dev/null +++ b/PlaneGcsSdk_Shared/CommunicationManagement/CommModule.cs @@ -0,0 +1,552 @@ +using Plane.Communication; +using Plane.Protocols; +using Plane.Windows.Messages; +using System; +using System.Collections.Generic; +using System.Diagnostics; +using System.Linq; +using System.Text; +using System.Threading.Tasks; + +namespace Plane.CommunicationManagement +{ + //通信模块 + public partial class CommModuleManager: ExceptionThrownEventSource, IDisposable + { + private static CommModuleManager _Instance = new CommModuleManager(); + public static CommModuleManager Instance { get { return _Instance; } } + + public TcpConnection Connection = null; + public bool CommOK = false; + private const string MODULE_IP = "192.168.199.51"; + private const string LOCAL_IP = "192.168.199.50"; + private const int PORT = 9551; + private bool _disposed; + public int CommModuleCopterCount = 0; + public int CommModuleCurMode = 0; + private long packetCountNum = 0; + + /// + /// 用于判断写入任务操作超时的秒表。 + /// + private Stopwatch _writeMissionStopwatch; + + public async Task ConnectAsync() + { + Connection = new TcpConnection(MODULE_IP, PORT); + await ConnectOpenAsync(); + } + + public void Connect() + { + Task.Run(async () => + { + Connection = new TcpConnection(MODULE_IP, PORT); + await ConnectOpenAsync(); + } + ); + } + + public void CloseConnection() + { + CommOK = false; + Connection?.Close(); + } + + private async Task ConnectOpenAsync() + { + CommOK = true; + bool bind = await Connection.BindIP(LOCAL_IP); + if (bind) + { + try + { + await Connection.OpenAsync().ConfigureAwait(false); + } + catch + { } + } + + if (Connection.IsOnline) + { + SendQuery(); + await StartReadingPacketsAsync(); + } + else + { + Reconnect(); + } + } + + //循环发送查询，保持通信正常 + private void SendQuery() + { + Task.Run(async () => + { + long lastPacketCountNum = 0; + while (CommOK) + { + if (Connection != null) + { + await SendQueryStatusPacketsAsync(); + await Task.Delay(1200).ConfigureAwait(false); + + if (packetCountNum > lastPacketCountNum) + lastPacketCountNum = packetCountNum; + else + break; + } + } + Message.Show("通信断开"); + + Reconnect(); + }).ConfigureAwait(false); + } + + private async Task SendQueryStatusPacketsAsync() + { + await WriteCommPacketAsync(0, MavComm.COMM_QUERY_STATUS); + await Task.Delay(100); + } + + int times = 1; + private void Reconnect() + { + Task.Run(async () => + { + CloseConnection(); + await Task.Delay(250).ConfigureAwait(false); + Message.Show($"正在重连:次数{times++}"); + await Task.Delay(250).ConfigureAwait(false); + await ConnectAsync(); + + }).ConfigureAwait(false); + } + + private async Task StartReadingPacketsAsync() + { + await Task.Run(async () => + { + while (CommOK) + { + if (Connection == null || !Connection.IsOnline) + { + break; + } + + var packet = await ReadPacketAsync().ConfigureAwait(false); + if (packet != null) + { + packetCountNum++; + short serialNum = BitConverter.ToInt16(packet, 4); + short copterNum = BitConverter.ToInt16(packet, 6); + short commandType = BitConverter.ToInt16(packet, 8); + byte[] dealData; + switch (commandType) + { + case MavComm.COMM_DOWNLOAD_MODE: + dealData = packet.Skip(10).ToArray(); + AnalyzeMissionStartPacket(copterNum, dealData); + break; + + case MavComm.COMM_GET_COPTERS_COUNT: + dealData = packet.Skip(16).ToArray(); + AnalyzeCopterInfosPacket(dealData); + break; + + case MavComm.COMM_NOTIFICATION: + short messageType = BitConverter.ToInt16(packet, 10); + dealData = packet.Skip(12).ToArray(); + switch (messageType) + { + case (short)MavComm.MessageType.STR: + AnalyzeStringPacket(copterNum, dealData); + break; + + case (short)MavComm.MessageType.SCAN2: + AnalyzeComm2RetrunPacket(copterNum, dealData); + break; + + case (short)MavComm.MessageType.SCAN3: + AnalyzeComm3RetrunPacket(copterNum, dealData); + break; + case 4: + AnalyzeComm4RetrunPacket(copterNum, dealData); + break; + } + break; + case 0: + int packetLength = packet.Length; + short errorId = BitConverter.ToInt16(packet, packetLength - 6); + short copterCount = BitConverter.ToInt16(packet, packetLength - 4); + short curMode = BitConverter.ToInt16(packet, packetLength - 2); + string msg = string.Format("错误Id={0}, 检测飞机总数={1}，工作模式={2} 流水号={3}", errorId, copterCount, curMode, serialNum); + CommModuleCopterCount = copterCount; + CommModuleCurMode = curMode; + //Message.Show(msg); + break; + } + } + } + }).ConfigureAwait(false); + Message.Show("连接断开"); + } + + private async Task ReadPacketAsync() + { + int readnumber = 0; + if (Connection == null || !Connection.IsOnline) + { + return null; + } + try + { + byte[] bufferHaed = new byte[2]; + readnumber = await Connection.ReadAsync(bufferHaed, 0, 2); + if (bufferHaed[0] == 0x13 && bufferHaed[1] == 0x77) + { + byte[] bufferLength = new byte[2]; + readnumber = await Connection.ReadAsync(bufferLength, 0, 2); + short datalength = BitConverter.ToInt16(bufferLength, 0); + //Console.WriteLine("datalength = "+ datalength); + + byte[] bufferData = new byte[datalength]; + readnumber = await Connection.ReadAsync(bufferData, 0, datalength); + +// foreach (byte b in bufferData) +// { +// Console.Write(b.ToString("X2")); +// Console.Write(" "); +// } +// Console.WriteLine(""); + + byte[] needCRCData = new byte[datalength + 4]; + Array.Copy(bufferHaed, 0, needCRCData, 0, 2); + Array.Copy(bufferLength, 0, needCRCData, 2, 2); + Array.Copy(bufferData, 0, needCRCData, 4, datalength); + + byte[] crc = checkCRC16(needCRCData); + byte[] bufferCRC16 = new byte[2]; + readnumber = await Connection.ReadAsync(bufferCRC16, 0, 2); + + if (crc[0] == bufferCRC16[0] && crc[1] == bufferCRC16[1]) + { + return needCRCData; + } + } + } + catch + { + //Console.WriteLine("错误"); + return null; + } + return null; + } + + + + short serial_Number = 1; + + public async Task GenerateDataAsync(short copterId, short messageType, TMavCommPacket indata) + { + byte[] data; + data = MavlinkUtil.StructureToByteArray(indata); + await WriteCommPacketAsync(copterId, messageType, data); + } + + bool temp = false; + //测试用 灯光间隔1S闪烁 + public async Task TestLED(short id) + { + 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 = 4; + led.custom_bytes = new byte[24]; + led.custom_bytes[0] = 80; + led.custom_bytes[1] = 0; + led.custom_bytes[2] = 0; + led.custom_bytes[3] = 3; + 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(0, MavComm.COMM_DOWNLOAD_COMM, packet); + await Task.Delay(1000).ConfigureAwait(false); + } + } + + /// + /// 生成通信模块packet并且发送 + /// + /// 飞机ID + /// 命令类型 + /// 数据类型：空表示只切换模式 + /// 小批量数据包 + /// + public async Task WriteCommPacketAsync(short copterId, short messageType, byte[] data = null, byte[] batchPacket = null) + { + if (data != null && batchPacket != null) data = batchPacket.Concat(data).ToArray(); + + int packetlength = data == null ? 0 : data.Length; + + byte[] packet = new byte[packetlength + 10]; + + byte[] buffer_header = new byte[2]; + buffer_header = BitConverter.GetBytes(MavComm.COMM_SEND_PACKET_HEADER); + Array.Copy(buffer_header, 0, packet, 0, 2); //数据头 + + byte[] buffer_length; + buffer_length = BitConverter.GetBytes((Int16)(packetlength + 6)); + Array.Copy(buffer_length, 0, packet, 2, 2); //数据长度 + + byte[] buffer_serial; + buffer_serial = BitConverter.GetBytes(serial_Number++); + Array.Copy(buffer_serial, 0, packet, 4, 2); //流水号 + + byte[] buffer_copterId = new byte[2]; + buffer_copterId = BitConverter.GetBytes((Int16)copterId); + Array.Copy(buffer_copterId, 0, packet, 6, 2); //飞机号 + + byte[] buffer_messageType = new byte[2]; + buffer_messageType = BitConverter.GetBytes((Int16)messageType); + Array.Copy(buffer_messageType, 0, packet, 8, 2); //命令类型 + + if (data != null) + Array.Copy(data, 0, packet, 10, data.Length); //数据内容 + + byte[] buffer_CRC = checkCRC16(packet); + + byte[] buffer_packet = new byte[packet.Length + 2]; + Array.Copy(packet, buffer_packet, packet.Length); + Array.Copy(buffer_CRC, 0, buffer_packet, packet.Length, 2); + + if (Connection != null && Connection.IsOnline) + { + try + { + await Connection.WriteAsync(buffer_packet, 0, buffer_packet.Length); + } + catch + { + + } + } + + } + + public async Task MissionPacket(short copterId, byte messageType, TMavCommPacket[] indata) + { + int dataLength = 6 + 2 + indata.Length * 32; + byte[] data = new byte[dataLength]; + + byte[] uses = new byte[] { 0, 0, 1, 0, 0, 0 }; + Array.Copy(uses, 0, data, 0, 6); + + Int16 countNum = (Int16)indata.Length; + Array.Copy(BitConverter.GetBytes(countNum), 0, data, 6, 2); + + int offset = 8; + + foreach (TMavCommPacket MavPacket in indata) + { + byte[] MavPacketData = MavlinkUtil.StructureToByteArray(MavPacket); + byte[] tempData = new byte[32]; + + //张伟只需要32个有用数据 0-27， 30-31， 34-35 + Array.Copy(MavPacketData, 0, tempData, 0, 28); + Array.Copy(MavPacketData, 30, tempData, 28, 2); + Array.Copy(MavPacketData, 34, tempData, 30, 2); + + Array.Copy(tempData, 0, data, offset, 32); + offset += 32; + } + + await WriteCommPacketAsync(copterId, messageType, data); + + if (_writeMissionStopwatch == null) _writeMissionStopwatch = Stopwatch.StartNew(); + else _writeMissionStopwatch.Restart(); + + //先延时5秒判断通信模块是否返回错误ID，0为正确。 如果已经错误了就不需要等待下发了。 + MissionStartCopterId = 0; MissionErrorId = -1; + ErrorCode = 0; + + if (await AwaitCommMissionStartAsync(copterId, 5000)) + { + _writeMissionStopwatch.Restart(); + MissionSendCopterId = 0; + //等待通信模块地面站数据下发后，才能开始下个飞机的航点写入 + //下发不代表写入成功 + return await AwaitCommMissionRequestAsync(copterId, 10000).ConfigureAwait(false); + } + else + return false; + } + + public async Task AwaitCommMissionStartAsync(short copterId, int millisecondsTimeout) + { + while (MissionStartCopterId != copterId) + { + if (_writeMissionStopwatch.ElapsedMilliseconds >= millisecondsTimeout) + { + return false; + } + await Task.Delay(50).ConfigureAwait(false); + } + if (MissionErrorId == 0) + return true; + else + { + //Message.Show($"{copterId}:返回错误--{MissionErrorId}"); + return false; + } + } + + public async Task AwaitCommMissionRequestAsync(short copterId, int millisecondsTimeout) + { + while (MissionSendCopterId != copterId) + { + if (_writeMissionStopwatch.ElapsedMilliseconds >= millisecondsTimeout || ErrorCode != 0) + { + return false; + } + await Task.Delay(50).ConfigureAwait(false); + } + return true; + } + + private long GetCreatetime() + { + DateTime DateStart = new DateTime(2000, 1, 1, 0, 0, 0); + return Convert.ToInt64((DateTime.Now - DateStart).TotalMilliseconds); + } + + public static byte[] CRC16(byte[] data) + { + int len = data.Length; + if (len > 0) + { + ushort crc = 0xFFFF; + + for (int i = 0; i < len; i++) + { + crc = (ushort)(crc ^ (data[i])); + for (int j = 0; j < 8; j++) + { + crc = (crc & 1) != 0 ? (ushort)((crc >> 1) ^ 0xA001) : (ushort)(crc >> 1); + } + } + byte hi = (byte)((crc & 0xFF00) >> 8); //高位置 + byte lo = (byte)(crc & 0x00FF); //低位置 + return new byte[] { hi, lo }; + } + return new byte[] { 0, 0 }; + } + + public byte[] CRCCalc(byte[] crcbuf) + { + //计算并填写CRC校验码 + int crc = 0xffff; + int len = crcbuf.Length; + for (int n = 0; n < len; n++) + { + byte i; + crc = crc ^ crcbuf[n]; + for (i = 0; i < 8; i++) + { + int TT; + TT = crc & 1; + crc = crc >> 1; + crc = crc & 0x7fff; + if (TT == 1) + { + crc = crc ^ 0xa001; + } + crc = crc & 0xffff; + } + + } + byte[] redata = new byte[2]; + redata[1] = (byte)((crc >> 8) & 0xff); + redata[0] = (byte)((crc & 0xff)); + return redata; + } + + public byte[] checkCRC16(byte[] puchMsg) + { + + int usDataLen = puchMsg.Length; + + ushort iTemp = 0x0; + ushort flag = 0x0; + + for (int i = 0; i < usDataLen; i++) + { + iTemp ^= puchMsg[i]; + for (int j = 0; j < 8; j++) + { + flag = (ushort)(iTemp & 0x01); + iTemp >>= 1; + if (flag == 1) + { + iTemp ^= 0xa001; + } + } + } + byte[] ret = BitConverter.GetBytes(iTemp); + return ret; + } + + public void Dispose() + { + if (!CommOK) + { + return; + } + CommOK = false; + CloseConnection(); + } + } +} diff --git a/PlaneGcsSdk_Shared/CommunicationManagement/CommModuleGenerateMavLink.cs b/PlaneGcsSdk_Shared/CommunicationManagement/CommModuleGenerateMavLink.cs new file mode 100644 index 0000000..e04c0f6 --- /dev/null +++ b/PlaneGcsSdk_Shared/CommunicationManagement/CommModuleGenerateMavLink.cs @@ -0,0 +1,602 @@ +using Plane.Copters; +using Plane.Protocols; +using System; +using System.Collections.Generic; +using System.Linq; +using System.Text; +using System.Threading.Tasks; +using static Plane.Copters.PlaneCopter; + +namespace Plane.CommunicationManagement +{ + public partial class CommModuleManager + { + public int packetcount = 0; + + + /// + /// 设置通道 + /// + /// + /// + /// + /// + /// + /// + public async Task SetChannelsAsync(IEnumerable copters = null, ushort? ch1 = null, ushort? ch2 = null, ushort? ch3 = null, ushort? ch4 = null) + { + short copterId = 0; + byte[] batchPacket = null; + GetCopterIds(copters, out copterId, out batchPacket); + + byte[] packet = SetChannels(ch1, ch2, ch3, ch4); + await WriteCommPacketAsync(copterId, MavComm.COMM_DOWNLOAD_COMM, packet, batchPacket).ConfigureAwait(false); + } + + + private byte[] SetChannels(ushort? ch1 = null, ushort? ch2 = null, ushort? ch3 = null, ushort? ch4 = null, ushort? ch5 = null, ushort? ch6 = null, ushort? ch7 = null, ushort? ch8 = null) + { + MAVLink.mavlink_rc_channels_override_t rc = new MAVLink.mavlink_rc_channels_override_t(); + rc.target_component = 1; + rc.target_system = 1; + rc.chan1_raw = ch1 ?? 1500; + rc.chan2_raw = ch2 ?? 1500; + rc.chan3_raw = ch3 ?? 1500; + rc.chan4_raw = ch4 ?? 1500; + rc.chan5_raw = 1500; + rc.chan6_raw = 1500; + rc.chan7_raw = 1500; + rc.chan8_raw = 1500; + return GeneratePacket(MAVLink.MAVLINK_MSG_ID_RC_CHANNELS_OVERRIDE, rc); + } + + public async Task WriteMissionListAsync(short id, List missions) + { + missions.Insert(0, PLCopter.PRE_TAKE_OFF_MISSION); + List mission_list = new List(); + foreach (IMission mission in missions) + { + var frame = mission.Command == FlightCommand.Waypoint && mission.Sequence == 0 ? MAVLink.MAV_FRAME.GLOBAL : MAVLink.MAV_FRAME.GLOBAL_RELATIVE_ALT; + + var req = new MAVLink.mavlink_mission_item_t(); + + req.target_system = 1; + req.target_component = 1; + + req.command = (byte)mission.Command; + + req.current = 0; + req.autocontinue = 1; + + req.frame = (byte)frame; + if (mission.Command == FlightCommand.LEDControl) + { + req.x = mission.R; + req.y = mission.G; + req.z = mission.B; + } + else + { + req.x = (float)mission.Latitude; + req.y = (float)mission.Longitude; + req.z = mission.Altitude; + } + + req.param1 = mission.Param1; + req.param2 = mission.Param2; + req.param3 = mission.Param3; + req.param4 = mission.Param4; + + req.seq = mission.Sequence; + mission_list.Add(req); + } + + bool result = await MissionPacket(id, (byte)MavComm.COMM_DOWNLOAD_MODE, mission_list.ToArray()); + + lock(MissinLocker) + { + if (missionWriteState.ContainsKey(id)) + { + missionWriteState[id].StateReturn = MissionErrorId; + missionWriteState[id].ErrorCode = ErrorCode; + missionWriteState[id].SendAchieved = result; + missionWriteState[id].WriteSucceed = false; + } + else + { + CommWriteMissinState state = new CommWriteMissinState(result); + state.StateReturn = MissionErrorId; + state.ErrorCode = ErrorCode; + missionWriteState.Add(id, state); + } + return result; + } + } + + /// + /// 开始任务 + /// + public async Task DoMissionStartAsync(int hour_utc, int minute_utc, int second_utc, double Missionlng, double Missionlat) + { + byte[] packet = DoCommandAsync(MAVLink.MAV_CMD.MISSION_START, hour_utc, minute_utc, second_utc, (float)Missionlng, (float)Missionlat, 0, 0); + await WriteCommPacketAsync(0, MavComm.COMM_DOWNLOAD_COMM, packet).ConfigureAwait(false); + } + + /// + /// 暂停任务 + /// + public async Task DoMissionPauseAsync() + { + byte[] packet = DoCommandAsync(MAVLink.MAV_CMD.MISSION_START, 0, 0, 0, 0, 0, 1, 0); + await WriteCommPacketAsync(0, MavComm.COMM_DOWNLOAD_COMM, packet).ConfigureAwait(false); + } + + /// + /// 恢复任务 + /// + public async Task DoMissionResumeAsync(int hour_utc, int minute_utc, int second_utc) + { + byte[] packet = DoCommandAsync(MAVLink.MAV_CMD.MISSION_START, hour_utc, minute_utc, second_utc, 0, 0 , 2, 0); + await WriteCommPacketAsync(0, MavComm.COMM_DOWNLOAD_COMM, packet).ConfigureAwait(false); + } + /// + /// 解锁 + /// + /// + /// + public async Task UnlockAsync(IEnumerable copters = null) + { + short copterId = 0; + byte[] batchPacket = null; + GetCopterIds(copters,out copterId, out batchPacket); + + await SetChannelsAsync(copters, 1500, 1500, 1100, 1500); + + + byte[] packet; + packet = SetModeAsync(FlightMode.ALT_HOLD); + await WriteCommPacketAsync(copterId, MavComm.COMM_DOWNLOAD_COMM, packet, batchPacket).ConfigureAwait(false); + + await Task.Delay(100).ConfigureAwait(false); + + packet = DoARMAsync(true); + await WriteCommPacketAsync(copterId, MavComm.COMM_DOWNLOAD_COMM, packet, batchPacket).ConfigureAwait(false); + } + + /// + /// LED闪烁白灯 + /// + /// + /// + public void LED_FlickerAsync(IEnumerable copters = null) + { + Task.Run(async () => + { + short copterId = 0; + byte[] batchPacket = null; + GetCopterIds(copters, out copterId, out batchPacket); + + byte[] packet = DoLEDCommandAsync(); + await WriteCommPacketAsync(copterId, MavComm.COMM_DOWNLOAD_COMM, packet, batchPacket).ConfigureAwait(false); + }); + } + + + /// + /// 设置参数 + /// + /// + /// + /// + /// + public async Task SetParamAsync(string paramname, float value, IEnumerable copters = null) + { + short copterId = 0; + byte[] batchPacket = null; + GetCopterIds(copters, out copterId, out batchPacket); + + byte[] packet = SetParam2Async(paramname, value); + int packetNum = packet[2]; + await WriteCommPacketAsync(copterId, MavComm.COMM_DOWNLOAD_COMM, packet, batchPacket).ConfigureAwait(false); + return packetNum; + } + + /// + /// 起飞测试 + /// + /// + /// + /// + public async Task TakeOffAsync(float alt, IEnumerable copters = null) + { + short copterId = 0; + byte[] batchPacket = null; + GetCopterIds(copters, out copterId, out batchPacket); + + byte[] packet; + packet = SetModeAsync(FlightMode.GUIDED); + await WriteCommPacketAsync(copterId, MavComm.COMM_DOWNLOAD_COMM, packet, batchPacket).ConfigureAwait(false); + + await Task.Delay(10).ConfigureAwait(false); + await SetChannelsAsync(copters, 1500, 1500, 1500, 1500); + await Task.Delay(10).ConfigureAwait(false); + + packet = DoCommandAsync(MAVLink.MAV_CMD.TAKEOFF, 0, 0, 0, 0, 0, 0, alt); + await WriteCommPacketAsync(copterId, MavComm.COMM_DOWNLOAD_COMM, packet, batchPacket).ConfigureAwait(false); + } + + /// + /// 返航 + /// + /// + /// + public async Task ReturnToLaunchAsync(IEnumerable copters = null) + { + short copterId = 0; + byte[] batchPacket = null; + GetCopterIds(copters, out copterId, out batchPacket); + + byte[] packet = SetModeAsync(FlightMode.RTL); + await WriteCommPacketAsync((short)copterId, MavComm.COMM_DOWNLOAD_COMM, packet, batchPacket).ConfigureAwait(false); + } + + /// + /// 获取版本 + /// + /// + /// + public async Task GetVersionsAsync(IEnumerable copters = null) + { + short copterId = 0; + byte[] batchPacket = null; + GetCopterIds(copters, out copterId, out batchPacket); + + MAVLink.mavlink_sversion_request req = new MAVLink.mavlink_sversion_request(); + req.version = 0; + byte[] packet = GeneratePacket(MAVLink.MAVLINK_MSG_ID_AUTOPILOT_VERSION_REQUEST, req); + await WriteCommPacketAsync((short)copterId, MavComm.COMM_DOWNLOAD_COMM, packet, batchPacket).ConfigureAwait(false); + } + /// + /// 降落 + /// + /// + public async Task LandAsync(IEnumerable copters = null) + { + short copterId = 0; + byte[] batchPacket = null; + GetCopterIds(copters, out copterId, out batchPacket); + + byte[] packet = SetModeAsync(FlightMode.LAND); + await WriteCommPacketAsync((short)copterId, MavComm.COMM_DOWNLOAD_COMM, packet, batchPacket).ConfigureAwait(false); + } + + /// + /// 加锁 + /// + /// + /// + public async Task LockAsync(IEnumerable copters = null) + { + short copterId = 0; + byte[] batchPacket = null; + GetCopterIds(copters, out copterId, out batchPacket); + + byte[] packet = DoARMAsync(false); + await WriteCommPacketAsync((short)copterId, MavComm.COMM_DOWNLOAD_COMM, packet, batchPacket).ConfigureAwait(false); + } + + /// + /// 测试电机 + /// + /// + /// + public async Task MotorTestAsync(int motor, IEnumerable copters = null) + { + short copterId = 0; + byte[] batchPacket = null; + GetCopterIds(copters, out copterId, out batchPacket); + + byte[] packet = DoMotorTestAsync(motor, MAVLink.MOTOR_TEST_THROTTLE_TYPE.MOTOR_TEST_THROTTLE_PERCENT, 5, 5); + await WriteCommPacketAsync((short)copterId, MavComm.COMM_DOWNLOAD_COMM, packet, batchPacket); + } + + /// + /// 悬停 + /// + /// + public async Task HoverAsync(IEnumerable copters = null) + { + short copterId = 0; + byte[] batchPacket = null; + GetCopterIds(copters, out copterId, out batchPacket); + + await SetChannelsAsync(copters, 1500, 1500, 1500, 1500); + + byte[] packet = SetModeAsync(FlightMode.LOITER); + await WriteCommPacketAsync((short)copterId, MavComm.COMM_DOWNLOAD_COMM, packet, batchPacket).ConfigureAwait(false); + } + + /// + /// 手动 + /// + /// + /// + public async Task FloatAsync(IEnumerable copters = null) + { + short copterId = 0; + byte[] batchPacket = null; + GetCopterIds(copters, out copterId, out batchPacket); + + byte[] packet = SetModeAsync(FlightMode.ALT_HOLD); + await WriteCommPacketAsync((short)copterId, MavComm.COMM_DOWNLOAD_COMM, packet, batchPacket).ConfigureAwait(false); + } + + private byte[] SetModeAsync(FlightMode mode) + { + return SetModeAsync(mode.ToAC2Mode()); + } + + private byte[] DoARMAsync(bool armit) + { + return DoCommandAsync(MAVLink.MAV_CMD.COMPONENT_ARM_DISARM, armit ? 1 : 0, 21196, 0, 0, 0, 0, 0); + } + + private byte[] DoMotorTestAsync(int motor, MAVLink.MOTOR_TEST_THROTTLE_TYPE thr_type, int throttle, int timeout) + { + return DoCommandAsync(MAVLink.MAV_CMD.DO_MOTOR_TEST, (float)motor, (float)(byte)thr_type, (float)throttle, (float)timeout, 0, 0, 0); + } + + private byte[] DoCommandAsync(MAVLink.MAV_CMD actionid, float p1, float p2, float p3, float p4, float p5, float p6, float p7) + { + MAVLink.mavlink_command_long_t req = new MAVLink.mavlink_command_long_t(); + + req.target_system = 1; + req.target_component = 1; + if (actionid == MAVLink.MAV_CMD.COMPONENT_ARM_DISARM) + { + req.target_component = (byte)MAVLink.MAV_COMPONENT.MAV_COMP_ID_SYSTEM_CONTROL; + } + req.command = (ushort)actionid; + req.param1 = p1; + req.param2 = p2; + req.param3 = p3; + req.param4 = p4; + req.param5 = p5; + req.param6 = p6; + req.param7 = p7; + + return GeneratePacket(MAVLink.MAVLINK_MSG_ID_COMMAND_LONG, req); ; + } + + private byte[] GeneratePacket(byte messageType, TMavlinkPacket indata) + { + byte[] data; + data = MavlinkUtil.StructureToByteArray(indata); + byte[] packet = new byte[data.Length + 6 + 2]; + packet[0] = MAVLink.MAVLINK_STX; + packet[1] = (byte)(data.Length); + packet[2] = (byte)packetcount; + packetcount++; + packet[3] = 255; // this is always 255 - MYGCS + packet[4] = (byte)MAVLink.MAV_COMPONENT.MAV_COMP_ID_MISSIONPLANNER; + packet[5] = messageType; + + 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[messageType], 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; + + return packet; + } + + + private byte[] SetModeAsync(ac2modes modein) + { + try + { + MAVLink.mavlink_set_mode_t mode = new MAVLink.mavlink_set_mode_t(); + + if (TranslateMode(modein, ref mode)) + { + return SetModeAsync(mode); + } + } + catch { } + return null; + } + + private byte[] SetModeAsync(MAVLink.mavlink_set_mode_t mode, MAVLink.MAV_MODE_FLAG base_mode = 0) + { + mode.base_mode |= (byte)base_mode; + return GeneratePacket(MAVLink.MAVLINK_MSG_ID_SET_MODE, mode); + } + + private bool TranslateMode(ac2modes modein, ref MAVLink.mavlink_set_mode_t mode) + { + mode.target_system = 1; + switch (modein) + { + case ac2modes.STABILIZE: + case ac2modes.AUTO: + case ac2modes.RTL: + case ac2modes.LOITER: + case ac2modes.ACRO: + case ac2modes.ALT_HOLD: + case ac2modes.CIRCLE: + case ac2modes.POSITION: + case ac2modes.LAND: + case ac2modes.OF_LOITER: + case ac2modes.GUIDED: + mode.base_mode = (byte)MAVLink.MAV_MODE_FLAG.CUSTOM_MODE_ENABLED; + mode.custom_mode = (uint)modein; + break; + default: + return false; + } + return true; + } + + + /// + ///广播Rtk + /// + /// + /// + /// + public async Task InjectGpsDataAsync(byte[] data, ushort length) + { + MAVLink.mavlink_gps_inject_data_t gps = new MAVLink.mavlink_gps_inject_data_t(); + var msglen = 110; + int datalen = 0; + var len = (length % msglen) == 0 ? length / msglen : (length / msglen) + 1; + for (int a = 0; a < len; a++) + { + datalen = Math.Min(length - a * msglen, msglen); + gps.data = new byte[msglen]; + Array.Copy(data, a * msglen, gps.data, 0, datalen); + gps.len = (byte)datalen; + gps.target_component = 1; + gps.target_system = 1; + byte[] packet = GenerateRTKPacketAsync(MAVLink.MAVLINK_MSG_ID_GPS_INJECT_DATA, gps, gps.len); + await WriteCommPacketAsync(0, MavComm.COMM_DOWNLOAD_COMM, packet).ConfigureAwait(false); + } + } + + public byte[] GenerateRTKPacketAsync(byte messageType, TMavlinkPacket indata, int rtklength) + { + byte[] data; + int rtkdataleng = rtklength + 3; + + data = MavlinkUtil.StructureToByteArray(indata); + + byte[] packet = new byte[rtkdataleng + 6 + 2]; + + packet[0] = MAVLink.MAVLINK_STX; + packet[1] = (byte)(rtkdataleng); + packet[2] = (byte)packetcount; + packetcount++; + packet[3] = 255; // this is always 255 - MYGCS + packet[4] = (byte)MAVLink.MAV_COMPONENT.MAV_COMP_ID_MISSIONPLANNER; + packet[5] = messageType; + + int i = 6; + foreach (byte b in data) + { + if (i < rtkdataleng + 6) + packet[i] = b; + else break; + i++; + } + + ushort checksum = MavlinkCRC.crc_calculate(packet, packet[1] + 6); + + checksum = MavlinkCRC.crc_accumulate(MAVLink.MAVLINK_MESSAGE_CRCS[messageType], 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; + + return packet; + } + + + private byte[] SetParam2Async(string paramname, float value) + { + // param type is set here, however it is always sent over the air as a float 100int = 100f. + var req = new MAVLink.mavlink_param_set_t { target_system = 1, target_component = 1, param_type = 4 }; + + byte[] temp = Encoding.GetEncoding("ASCII").GetBytes(paramname); + + Array.Resize(ref temp, 16); + req.param_id = temp; + req.param_value = (value); + + return GeneratePacket((byte)MAVLink.MAVLINK_MSG_ID_PARAM_SET, req); + } + + private byte[] DoLEDCommandAsync() + { + 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 = 4; + led.custom_bytes = new byte[24]; + led.custom_bytes[0] = 255; + led.custom_bytes[1] = 255; + led.custom_bytes[2] = 255; + led.custom_bytes[3] = 3; + + return GeneratePacket(MAVLink.MAVLINK_MSG_ID_LED_CONTROL, led); + } + + private byte[] BatchCopterIdData(IEnumerable copters) + { + byte[] BatchByte = null; + List copterList = copters.OrderBy(c=>int.Parse(c.Id)).ToList(); + for (int i = 0; i < copterList.Count; i++) + { + byte[] tempByte; + int id = int.Parse(copterList[i].Id); + int count = 1; + while(i+1 < copterList.Count && int.Parse(copterList[i + 1].Id) == id + count) + { + count++; + i++; + } + + if (count == 1) + { + short curId = (short)(0 << 12 ^ id); + tempByte = BitConverter.GetBytes(curId); + } + else + { + short beginId = (short)(2 << 12 ^ id); + tempByte = BitConverter.GetBytes(beginId); + + short numCount = (short)(4 << 12 ^ count); + tempByte = tempByte.Concat(BitConverter.GetBytes(numCount)).ToArray(); + } + + if (BatchByte == null) + BatchByte = tempByte; + else + BatchByte = BatchByte.Concat(tempByte).ToArray(); + } + return BatchByte; + } + + private void GetCopterIds(IEnumerable copters, out short copterId, out byte[] batchPacket) + { + copterId = 0; + batchPacket = null; + + if (copters != null) + { + if (copters.Count() == 1) + copterId = short.Parse(copters.FirstOrDefault().Id); + else + { + batchPacket = BatchCopterIdData(copters); + } + } + } + } +} diff --git a/PlaneGcsSdk_Shared/CommunicationManagement/CommModulePacketAnalysis.cs b/PlaneGcsSdk_Shared/CommunicationManagement/CommModulePacketAnalysis.cs new file mode 100644 index 0000000..981a9e9 --- /dev/null +++ b/PlaneGcsSdk_Shared/CommunicationManagement/CommModulePacketAnalysis.cs @@ -0,0 +1,195 @@ +using Plane.Communication; +using Plane.Protocols; +using Plane.Windows.Messages; +using System; +using System.Collections.Generic; +using System.Diagnostics; +using System.Linq; +using System.Text; +using System.Threading.Tasks; + +namespace Plane.CommunicationManagement +{ + public partial class CommModuleManager + { + private int MissionStartCopterId = 0; + private int MissionSendCopterId = 0; + private int MissionErrorId = -1; + private int ErrorCode = 0; + public event EventHandler CommunicationReceived; + public event EventHandler CommunicationCopterDisconnect; + public event EventHandler CommunicationConnected; + private Dictionary missionWriteState = new Dictionary(); + private static readonly object MissinLocker = new object(); + + public Dictionary MissionWriteState + { + get {return missionWriteState; } + } + + public void ClearMissionWriteState() + { + missionWriteState.Clear(); + } + + private void AnalyzeMissionStartPacket(short copterId, byte[] buffer) + { + short errorId = BitConverter.ToInt16(buffer, 0); + MissionStartCopterId = copterId; + MissionErrorId = errorId; + if(errorId != 0) + Message.Show($"{copterId}:返回 = {errorId}"); + } + + private void AnalyzeStringPacket(short copterId, byte[] buffer) + { + string msg = System.Text.Encoding.Default.GetString(buffer).Replace("tcpserver", "flicube"); + Message.Show(msg); + } + + private void AnalyzeComm2RetrunPacket(short copterId, byte[] buffer) + { + ushort ret = BitConverter.ToUInt16(buffer, 0); + switch (ret) + { + case MavComm.SEARCH_FINDCOPTER: + Message.Show(copterId.ToString() + "---飞机开始相应"); + break; + + case MavComm.SEARCH_COMPLETED: + Message.Show(copterId.ToString() + "---设置成功"); + break; + + case MavComm.SEARCH_OUTTIME: + Message.Show("超时无响应"); + break; + + case MavComm.MISSION_SEND_COMPLETED: + MissionSendCopterId = copterId; + break; + + case MavComm.P2P_SEND_FAILED: + Message.Show("点对点通信发送失败"); + break; + default: + if (ret > MavComm.ERROR_CODE_START && ret <= MavComm.ERROR_CODE_END) + { + ErrorCode = ret; + Message.Show($"{copterId}--错误码：{ret}"); + } + + break; + } + } + + private void AnalyzeComm3RetrunPacket(short copterId, byte[] buffer) + { + byte type = buffer[0]; + byte connectRet; + switch (type) + { + case 0x01: + //connectRet = buffer[1]; + //if (connectRet == 0x0) //飞机连接上 不管飞控和通信模块的连接 + CommunicationConnected?.Invoke(this, new CommunicationConnectEventArgs(copterId)); + break; + case 0x02: + connectRet = buffer[1]; + if (connectRet == 0x0) //飞机断开 + CommunicationCopterDisconnect?.Invoke(this, new CommunicationCopterDisconnectEventArgs(copterId)); + break; + + case 0x03: + SaveMissionWriteStat(copterId, buffer[1]); + break; + } + } + + private void SaveMissionWriteStat(short copterId, byte wirteMissRet) + { + Task.Run(async () => + { + lock (MissinLocker) + { + if (wirteMissRet == 0x20) + { + if (missionWriteState.ContainsKey(copterId)) + missionWriteState[copterId].WriteSucceed = true; + Message.Show($"{copterId}:航点写入成功"); + } + if (wirteMissRet > 0x20 && wirteMissRet < 0x30) + { + if (missionWriteState.ContainsKey(copterId)) + missionWriteState[copterId].WriteSucceed = false; + Message.Show($"{copterId}:航点写入失败"); + } + } + await Task.Delay(10).ConfigureAwait(false); + } + ); + + } + + private void AnalyzeCopterInfosPacket(byte[] buffer) + { + ushort beginNum = BitConverter.ToUInt16(buffer, 0); + ushort endNum = BitConverter.ToUInt16(buffer, 2); + int count = endNum - beginNum + 1; + byte[] copter_packets = buffer.Skip(4).ToArray(); + if (copter_packets.Length != count * 4) + { + return; + } + + int offset = 0; + for (int i = beginNum; i <= endNum; i++) + { + byte[] onePacket = copter_packets.Skip(offset).Take(4).ToArray(); + UInt16 state = BitConverter.ToUInt16(onePacket, 0); + short copterId = (short)i; + switch (state >> 13) + { + //0B000 + case 0x0000 >> 13: + CommunicationCopterDisconnect?.Invoke(this, new CommunicationCopterDisconnectEventArgs(copterId)); + break; + //0B100 + case 0x8000 >> 13: + break; + //0B110 + case 0xC000 >> 13: + //0B111 + case 0xE000 >> 13: + CommunicationConnected?.Invoke(this, new CommunicationConnectEventArgs(copterId)); + break; + } + offset += 4; + } + } + + private void AnalyzeComm4RetrunPacket(short copterId, byte[] buffer) + { + byte[] packet = buffer.Take(28).ToArray(); + byte[] state_packet = buffer.Skip(28).ToArray(); + UInt16 state = BitConverter.ToUInt16(state_packet,0); + byte version = state_packet[3]; + switch (state >> 13) + { + //0B000 + case 0x0000 >> 13: + CommunicationCopterDisconnect?.Invoke(this, new CommunicationCopterDisconnectEventArgs(copterId)); + break; + //0B100 + case 0x8000 >> 13: + break; + //0B110 + case 0xC000 >> 13: + //0B111 + case 0xE000 >> 13: + CommunicationReceived?.Invoke(this, new CommunicationReceiveCopterInfoEventArgs(copterId, packet, version)); + break; + } + + } + } +} diff --git a/PlaneGcsSdk_Shared/CommunicationManagement/CommWriteMissinState.cs b/PlaneGcsSdk_Shared/CommunicationManagement/CommWriteMissinState.cs new file mode 100644 index 0000000..9c9d5a3 --- /dev/null +++ b/PlaneGcsSdk_Shared/CommunicationManagement/CommWriteMissinState.cs @@ -0,0 +1,20 @@ +using System; +using System.Collections.Generic; +using System.Text; + +namespace Plane.CommunicationManagement +{ + public class CommWriteMissinState + { + public CommWriteMissinState(bool sendAchieved) + { + this.SendAchieved = sendAchieved; + } + public int StateReturn = 0; + public int ErrorCode = 0; + public bool SendAchieved = false; + public bool WriteSucceed = false; + + public bool IsFailed { get { return StateReturn != 0 || ErrorCode != 0 || !SendAchieved || !WriteSucceed; } } + } +} diff --git a/PlaneGcsSdk_Shared/CommunicationManagement/CommunicationReceiveCopterInfoEventArgs.cs b/PlaneGcsSdk_Shared/CommunicationManagement/CommunicationReceiveCopterInfoEventArgs.cs new file mode 100644 index 0000000..cc51048 --- /dev/null +++ b/PlaneGcsSdk_Shared/CommunicationManagement/CommunicationReceiveCopterInfoEventArgs.cs @@ -0,0 +1,40 @@ +using System; +using System.Collections.Generic; +using System.Text; + +namespace Plane.CommunicationManagement +{ + public class CommunicationReceiveCopterInfoEventArgs + { + public CommunicationReceiveCopterInfoEventArgs(int id, byte[] package, byte commModuleVersion) + { + this.Id = id; + this.Package = package; + this.CommModuleVersion = commModuleVersion; + } + + public int Id { get; private set; } + public byte[] Package { get; private set; } + public byte CommModuleVersion { get; private set; } + } + + public class CommunicationConnectEventArgs + { + public CommunicationConnectEventArgs(int id) + { + this.Id = id; + } + + public int Id { get; private set; } + } + + public class CommunicationCopterDisconnectEventArgs + { + public CommunicationCopterDisconnectEventArgs(int id) + { + this.Id = id; + } + + public int Id { get; private set; } + } +} diff --git a/PlaneGcsSdk_Shared/Protocols/MavCommTypes.cs b/PlaneGcsSdk_Shared/Protocols/MavCommTypes.cs new file mode 100644 index 0000000..180863d --- /dev/null +++ b/PlaneGcsSdk_Shared/Protocols/MavCommTypes.cs @@ -0,0 +1,148 @@ +using System; +using System.Collections.Generic; +using System.Runtime.InteropServices; +using System.Text; + +namespace Plane.Protocols +{ + /* + * + * 通信模块协议 + * + */ + public class MavComm + { + /// + /// 发送数据时的数据包头 + /// + public const ushort COMM_SEND_PACKET_HEADER = 0x9551; + /// + /// 接受数据时的数据包头 + /// + public const ushort COMM_RECEIVE_PACKET_HEADER = 0x7713; + + + #region 命令类型 2字节 + //-----------------命令类型----------------- + /// + /// 查询状态 + /// + public const short COMM_QUERY_STATUS = 0x00; + + /// + /// 变更飞机总数及参与者 + /// + public const short COMM_SET_MAV_COUNT = 0x10; + + /// + /// 获取飞机详细信息 + /// + public const short COMM_GET_COPTERS_COUNT = 0x20; + + /// + /// 时间同步，不改变当前模式 + /// + public const short COMM_ASYN_TIME = 0x30; + + /// + /// 进入空闲模式 + /// + public const short COMM_IDLE_MODE = 0x50; + + /// + /// 进入搜索模式，命名编号targetID memcpy(pdata,input,2); + /// + public const short COMM_SEARCH_MODE = 0x51; + + /// + /// 进入航点下载模式 (写航点) + /// + public const short COMM_DOWNLOAD_MODE = 0x52; + + /// + /// 下载模式通信 + /// + public const short COMM_DOWNLOAD_COMM = 0x53; + + /// + /// 进入飞行模式（无负载） + /// + public const short COMM_FLIGHT_MODE = 0x54; + + /// + /// 进入飞行模式（有负载数据） + /// + public const short COMM_FLIGHT_LOAD_MODE = 0x55; + + /// + /// 通信模块 + /// + public const short COMM_NOTIFICATION = 0x1234; + + #endregion + + public enum CommMode + { + IDLE = 0, + SEARCH = 1, + DOWNLOAD = 3, + FLIGHT = 4 + } + + public enum MessageType + { + STR = 0, + SCAN2 = 2, + SCAN3 = 3 + } + + //search 搜索模式相关 + public const ushort SEARCH_COMPLETED = 0x0502; //搜索模式 扫描成功 + public const ushort FREQUENCY_HOPPING_COMPLETED = 0x0503; //搜索模式 跳频表更新成功 + public const ushort SEARCH_FINDCOPTER = 0x0401; //搜索模式 找到指定目标 + public const ushort FREQUENCY_HOPPING_ERROR = 0x0100; //搜索模式 跳频表更新错误 + public const ushort SEARCH_OUTTIME = 0xFFFF; //搜索模式 没有人应答 超时时间20S + + + public const ushort MISSION_SEND_COMPLETED = 0x0503; //航点传输到飞控成功，可开始传输下个飞机航点信息, + //需要再后续等待一个成功消息，才算完成 + + public const ushort P2P_SEND_FAILED = 0x0600; //点对点通信发送失败 + + public const ushort ERROR_CODE_START = 0x0100; + public const ushort ERROR_CODE_END = 0x03FF; + + + [StructLayout(LayoutKind.Sequential, Pack = 1, Size = 2)] + public struct comm_set_mav_count + { + public Int16 mav_count; //飞机总数 + }; + + + [StructLayout(LayoutKind.Sequential, Pack = 1, Size = 8)] + public struct comm_asyn_time + { + public Int64 time_stamp; + }; + + [StructLayout(LayoutKind.Sequential, Pack = 1, Size = 28)] + public struct comm_copter_info + { + public Int32 control_mode; + public UInt32 lat; + public UInt32 lng; + public Int32 retain; + public Int32 alt; + + public Int16 gps_status; + + public byte lock_status; + public byte gps_num_sats; + public Int16 battery_voltage; + public Int16 yaw; + + }; + + } +}