553 lines
19 KiB
C#
553 lines
19 KiB
C#
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;
|
||
|
||
/// <summary>
|
||
/// 用于判断写入任务操作超时的秒表。
|
||
/// </summary>
|
||
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<byte[]> 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<TMavCommPacket>(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);
|
||
}
|
||
}
|
||
|
||
/// <summary>
|
||
/// 生成通信模块packet并且发送
|
||
/// </summary>
|
||
/// <param name="copterId">飞机ID</param>
|
||
/// <param name="messageType">命令类型</param>
|
||
/// <param name="data">数据类型：空表示只切换模式</param>
|
||
/// <param name="batchPacket">小批量数据包</param>
|
||
/// <returns></returns>
|
||
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<bool> MissionPacket<TMavCommPacket>(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<bool> 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<bool> 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();
|
||
}
|
||
}
|
||
}
|