xu
8af733e644
详细描述 # 类型 包含: # feat：新功能（feature） # fix：修补bug # docs：文档（documentation） # style： 格式（不影响代码运行的变动） # refactor：重构（即不是新增功能，也不是修改bug的代码变动） # test：增加测试 # chore：构建过程或辅助工具的变动
865 lines
32 KiB
C#
865 lines
32 KiB
C#
using Plane.Communication;
|
||
using Plane.Protocols;
|
||
using System;
|
||
using System.Collections.Generic;
|
||
using System.Diagnostics;
|
||
using System.Linq;
|
||
using System.Net;
|
||
using System.Net.NetworkInformation;
|
||
using System.Net.Sockets;
|
||
using System.Text;
|
||
using System.Threading.Tasks;
|
||
using System.Windows.Media;
|
||
|
||
using System.Runtime.InteropServices;
|
||
using Plane.Windows.Messages;
|
||
|
||
namespace Plane.CommunicationManagement
|
||
{
|
||
//通信模块
|
||
public partial class CommModuleManager: ExceptionThrownEventSource, IDisposable
|
||
{
|
||
[DllImport("kernel32")] //返回取得字符串缓冲区的长度
|
||
private static extern int GetPrivateProfileString(string section, string key, string def, StringBuilder retVal, int size, string filePath);
|
||
|
||
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"; //"192.168.199.51"; // "192.168.199.51"; //在用版本 ----//"192.168.1.65"; //新版本
|
||
private string ServerIP = MODULE_IP;
|
||
// private const string LOCAL_IP = "192.168.199.50"; //"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;
|
||
|
||
private void LoadIPSet()
|
||
{
|
||
StringBuilder temp = new StringBuilder(255);
|
||
|
||
string path = Environment.CurrentDirectory + @"\Config.ini";
|
||
|
||
int i = GetPrivateProfileString("Default", "ServerIP", "", temp, 255, path);
|
||
ServerIP= temp.ToString();
|
||
|
||
if (ServerIP == "") ServerIP = MODULE_IP;
|
||
}
|
||
|
||
|
||
public async Task ConnectAsync()
|
||
{
|
||
LoadIPSet();
|
||
Connection = new TcpConnection(ServerIP, PORT);
|
||
await ConnectOpenAsync();
|
||
}
|
||
|
||
public void Connect()
|
||
{
|
||
LoadIPSet();
|
||
Task.Run(async () =>
|
||
{
|
||
Connection = new TcpConnection(ServerIP, PORT);
|
||
await ConnectOpenAsync();
|
||
}
|
||
);
|
||
}
|
||
|
||
public void CloseConnection()
|
||
{
|
||
CommOK = false;
|
||
Connection?.Close();
|
||
}
|
||
|
||
|
||
|
||
|
||
|
||
|
||
|
||
|
||
/*
|
||
|
||
|
||
//获取内网IP
|
||
private IPAddress GetInternalIP()
|
||
{
|
||
NetworkInterface[] nics = NetworkInterface.GetAllNetworkInterfaces();
|
||
|
||
foreach (NetworkInterface adapter in nics)
|
||
{
|
||
foreach (var uni in adapter.GetIPProperties().UnicastAddresses)
|
||
{
|
||
if (uni.Address.AddressFamily == AddressFamily.InterNetwork)
|
||
{
|
||
return uni.Address;
|
||
}
|
||
}
|
||
}
|
||
return null;
|
||
}
|
||
|
||
/// <summary>
|
||
/// 获取本地连接IP地址、无线连接IP地址
|
||
/// </summary>
|
||
/// <param name="k">0:本地连接，1:本地连接1，2:无线网络连接</param>
|
||
/// <returns></returns>
|
||
public static IPAddress GetIP(int k)
|
||
{
|
||
NetworkInterface[] interfaces = System.Net.NetworkInformation.NetworkInterface.GetAllNetworkInterfaces();
|
||
int len = interfaces.Length;
|
||
IPAddress[] mip = { IPAddress.Parse("0.0.0.0"), IPAddress.Parse("0.0.0.0"), IPAddress.Parse("0.0.0.0") };
|
||
|
||
for (int i = 0; i < len; i++)
|
||
{
|
||
NetworkInterface ni = interfaces[i];
|
||
|
||
if (ni.Name == "本地连接")
|
||
{
|
||
IPInterfaceProperties property = ni.GetIPProperties();
|
||
foreach (UnicastIPAddressInformation ip in property.UnicastAddresses)
|
||
{
|
||
if (ip.Address.AddressFamily == System.Net.Sockets.AddressFamily.InterNetwork)
|
||
{
|
||
mip[0] = ip.Address;
|
||
}
|
||
}
|
||
}
|
||
else if (ni.Name == "本地连接2")
|
||
{
|
||
IPInterfaceProperties property = ni.GetIPProperties();
|
||
foreach (UnicastIPAddressInformation ip in property.UnicastAddresses)
|
||
{
|
||
if (ip.Address.AddressFamily == System.Net.Sockets.AddressFamily.InterNetwork)
|
||
{
|
||
mip[1] = ip.Address;
|
||
}
|
||
}
|
||
}
|
||
else if (ni.Name == "无线网络连接")
|
||
{
|
||
IPInterfaceProperties property = ni.GetIPProperties();
|
||
foreach (UnicastIPAddressInformation ip in property.UnicastAddresses)
|
||
{
|
||
if (ip.Address.AddressFamily == System.Net.Sockets.AddressFamily.InterNetwork)
|
||
{
|
||
mip[2] = ip.Address;
|
||
}
|
||
}
|
||
}
|
||
}
|
||
return mip[k];
|
||
}
|
||
|
||
*/
|
||
private async Task ConnectOpenAsync()
|
||
{
|
||
CommOK = true;
|
||
//不绑定IP也可以通讯，有线网络需要设置192.168.199.X网段地址
|
||
// string locip = GetIP(0).ToString();
|
||
// bool bind = await Connection.BindIP(locip);
|
||
// if (bind)
|
||
{
|
||
try
|
||
{
|
||
if (!Connection.IsOnline)
|
||
await Connection.OpenAsync().ConfigureAwait(false);
|
||
}
|
||
catch
|
||
{ }
|
||
}
|
||
|
||
if (Connection.IsOnline)
|
||
{
|
||
Message.Connect(true);
|
||
Message.Show($"通讯基站连接成功!");
|
||
times = 0;
|
||
SendQuery();
|
||
await StartReadingPacketsAsync();
|
||
}
|
||
else
|
||
{
|
||
Message.Connect(false);
|
||
Reconnect();
|
||
}
|
||
}
|
||
|
||
//循环发送查询，保持通信正常
|
||
private void SendQuery()
|
||
{
|
||
Task.Run(async () =>
|
||
{
|
||
long lastPacketCountNum = 0;
|
||
int faulttimes = 0; //等待没有数据次数
|
||
const int revtimeout = 2000; //等待超时ms old=1200
|
||
while (CommOK)
|
||
{
|
||
if (Connection != null)
|
||
{
|
||
//发送心跳包-等待回复消息
|
||
await WriteCommPacketAsync(0, MavComm.COMM_QUERY_STATUS);
|
||
// await SendQueryStatusPacketsAsync();
|
||
await Task.Delay(revtimeout).ConfigureAwait(false);
|
||
//等待是否接受到返回的数据包，如果没有收到认为连结断开，重新联接
|
||
if (packetCountNum > lastPacketCountNum)
|
||
{
|
||
lastPacketCountNum = packetCountNum;
|
||
if (faulttimes>0)
|
||
Message.Show("收到新数据包，重置超时次数...");
|
||
faulttimes = 0;
|
||
}
|
||
else
|
||
{
|
||
faulttimes++;
|
||
Message.Show("设备回复超时("+ revtimeout + "ms)" + faulttimes+"次");
|
||
//2次没数据包返回就重连
|
||
if (faulttimes > 1)
|
||
{
|
||
Message.Show("长时间未收到设备回复数据包");
|
||
break;
|
||
}
|
||
}
|
||
}
|
||
}
|
||
Message.Show("主动断开连接，重连");
|
||
Reconnect();
|
||
}).ConfigureAwait(false);
|
||
}
|
||
|
||
/* //直接调用不用封装函数了
|
||
private async Task SendQueryStatusPacketsAsync()
|
||
{
|
||
await WriteCommPacketAsync(0, MavComm.COMM_QUERY_STATUS);
|
||
await Task.Delay(100);//100
|
||
}
|
||
*/
|
||
|
||
int times = 0;
|
||
//这里容易报空引用异常 ，没有连接通讯设备时
|
||
private void Reconnect()
|
||
{
|
||
// Message.Show($"正在重新连接...");
|
||
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)
|
||
{
|
||
if (Connection==null)
|
||
Message.Show("空连接异常");
|
||
else if (!Connection.IsOnline)
|
||
{
|
||
Message.Show("检测到设备已断开连接");
|
||
}
|
||
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: //版本13通讯模块收到飞机返回的数据
|
||
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("退出读设备数据........");
|
||
//Reconnect();
|
||
}
|
||
|
||
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);
|
||
}
|
||
|
||
public async Task UpdateCommModule()
|
||
{
|
||
MavComm.comm_update_copter_module commUpdate = new MavComm.comm_update_copter_module();
|
||
commUpdate.mav_count = (short)CommModuleCopterCount;
|
||
commUpdate.update_code = 0x7713;
|
||
await GenerateDataAsync(0, MavComm.COMM_UPDATE_COPTER_MODULE, commUpdate);
|
||
}
|
||
|
||
|
||
public async Task TestModule(short ModuleNo, short TestLong=100)
|
||
{
|
||
|
||
|
||
byte[] packet = new byte[2];
|
||
packet[0] = (byte)(ModuleNo);
|
||
packet[1] = (byte)(TestLong);
|
||
byte[] senddata = packet;
|
||
|
||
// Message.Show("长度 = " + senddata.Length);
|
||
await WriteCommPacketAsync(0, MavComm.COMM_TEST_MODULE, senddata);
|
||
// await Task.Delay(1000).ConfigureAwait(false);
|
||
|
||
}
|
||
|
||
|
||
|
||
public async Task SetModulePower(short ModuleNo, short ModulePower)
|
||
{
|
||
byte[] packet = { 0x01, 0x00, 0x03, 0x00, 0x01, 0x0A };
|
||
packet[2] = (byte)(ModuleNo & 0xFF);
|
||
packet[3] = (byte)((ModuleNo & 0xFF00) >> 8);
|
||
packet[5] = (byte)(ModulePower);
|
||
await WriteCommPacketAsync(0, MavComm.COMM_TO_MODULE, packet);
|
||
}
|
||
|
||
|
||
|
||
|
||
|
||
bool temp = false;
|
||
//测试用 灯光间隔1S闪烁
|
||
public async Task TestLED(short id, string colorString)
|
||
{
|
||
byte[] packet = DoLEDCommandAsync(51, 0, 51);
|
||
temp = !temp;
|
||
while (temp)
|
||
{
|
||
Message.Show("发送测试灯光");
|
||
await WriteCommPacketAsync(id, MavComm.COMM_DOWNLOAD_COMM, packet);
|
||
//如果是广播同时发送到广播端口
|
||
if ((id==0)&& Recomisopen)
|
||
{
|
||
await BroadcastSendAsync(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);
|
||
}
|
||
}
|
||
|
||
|
||
public async Task TestBattery(short id, float minivol)
|
||
{
|
||
|
||
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 = 5;//命令类型——测试电池电压
|
||
led.custom_bytes = new byte[24];
|
||
|
||
led.custom_bytes[0] = (byte)(int)(minivol); //整数部分
|
||
led.custom_bytes[1] = (byte)(int)((minivol-(int)minivol)*100); //小数部分 --2位
|
||
|
||
|
||
|
||
|
||
|
||
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();
|
||
}
|
||
|
||
|
||
{ //发3次
|
||
await WriteCommPacketAsync(id, MavComm.COMM_DOWNLOAD_COMM, packet);
|
||
await Task.Delay(1000).ConfigureAwait(false);
|
||
await WriteCommPacketAsync(id, MavComm.COMM_DOWNLOAD_COMM, packet);
|
||
await Task.Delay(1000).ConfigureAwait(false);
|
||
await WriteCommPacketAsync(id, MavComm.COMM_DOWNLOAD_COMM, packet);
|
||
|
||
}
|
||
}
|
||
|
||
|
||
/// <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 (messageType == MavComm.COMM_DOWNLOAD_COMM && copterId == 0)
|
||
{
|
||
short byteNum;
|
||
short length;
|
||
if (batchPacket == null)
|
||
{
|
||
byteNum = 0;
|
||
length = (short)((0x5 << 12) ^ byteNum);
|
||
batchPacket = BitConverter.GetBytes(length);
|
||
}
|
||
else
|
||
{
|
||
byteNum = (short)(batchPacket.Length / 2);
|
||
length = (short)((0x5 << 12) ^ byteNum);
|
||
batchPacket = BitConverter.GetBytes(length).Concat(batchPacket).ToArray();
|
||
}
|
||
}
|
||
*/
|
||
if (data != null && batchPacket != null) data = batchPacket.Concat(data).ToArray();
|
||
|
||
int packetlength = data == null ? 0 : data.Length;
|
||
|
||
//数据长度+10
|
||
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); //发送数据头 2字节
|
||
|
||
byte[] buffer_length;
|
||
buffer_length = BitConverter.GetBytes((Int16)(packetlength + 6));
|
||
Array.Copy(buffer_length, 0, packet, 2, 2); //数据长度 2字节
|
||
|
||
byte[] buffer_serial;
|
||
buffer_serial = BitConverter.GetBytes(serial_Number++);
|
||
Array.Copy(buffer_serial, 0, packet, 4, 2); //流水号 2字节
|
||
|
||
byte[] buffer_copterId = new byte[2];
|
||
buffer_copterId = BitConverter.GetBytes((Int16)copterId);
|
||
Array.Copy(buffer_copterId, 0, packet, 6, 2); //飞机号 2字节
|
||
|
||
byte[] buffer_messageType = new byte[2];
|
||
buffer_messageType = BitConverter.GetBytes((Int16)messageType);
|
||
Array.Copy(buffer_messageType, 0, packet, 8, 2); //命令类型 2字节
|
||
|
||
if (data != null)
|
||
Array.Copy(data, 0, packet, 10, data.Length); //数据内容 10字节开始
|
||
|
||
byte[] buffer_CRC = checkCRC16(packet); //计算所有数据的crc
|
||
|
||
byte[] buffer_packet = new byte[packet.Length + 2]; //分配新的带crc的数据包
|
||
Array.Copy(packet, buffer_packet, packet.Length);
|
||
Array.Copy(buffer_CRC, 0, buffer_packet, packet.Length, 2); //带crc的数据包
|
||
|
||
if (Connection != null && Connection.IsOnline)
|
||
{
|
||
//if (messageType==83)
|
||
// Console.WriteLine("RTCM:" + (ushort)rtcm.length + "[" + rtcm.packet[0] + "," + rtcm.packet[1] + "," + rtcm.packet[2] + "," + rtcm.packet[3] + "," + rtcm.packet[4] + "]");
|
||
// Console.WriteLine("[{0}]Send rtcm:{1}", buffer_serial[0], packetlength);
|
||
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);
|
||
|
||
|
||
//新盒子的协议--暂时不用-需要问张伟
|
||
/*
|
||
Int16 countNum = (Int16)indata.Length;
|
||
|
||
byte[] uses = new byte[] { 0, 1, 0, 0, 0, 0 };
|
||
|
||
Array.Copy(uses, 0, data, 0, 6);
|
||
|
||
Array.Copy(BitConverter.GetBytes(countNum), 0, data, 2, 2);
|
||
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();
|
||
}
|
||
}
|
||
}
|