group/Plane.Sdk3
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity
f82a285f8d
Plane.Sdk3/PlaneGcsSdk_Shared/CommunicationManagement/CommModule.cs

919 lines
34 KiB
C#
Raw Normal View History

更新使用Vs2022编译 模拟飞机路线计算调整
2023-12-25 18:54:28 +08:00
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);
去掉重连提示，要不日志里面太多了
2024-01-08 20:40:10 +08:00
// Message.Show($"正在重连:次数{++times}");
更新使用Vs2022编译 模拟飞机路线计算调整
2023-12-25 18:54:28 +08:00
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;
支持紧急返航 支持返回飞机错误
2024-03-26 21:58:48 +08:00
case 4: //版本13通讯模块收到飞机返回的数据
更新使用Vs2022编译 模拟飞机路线计算调整
2023-12-25 18:54:28 +08:00
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)
{
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];
if (colorString == "") colorString = "330033";
Color color = (Color)ColorConverter.ConvertFromString("#" + colorString);
led.custom_bytes[0] = color.R;
led.custom_bytes[1] = color.G;
led.custom_bytes[2] = color.B;
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 < 14; times++)
// {
// senddata = senddata.Concat(packet).ToArray();
// }
// byte[] myByteArray = Enumerable.Repeat((byte)0x08, 15).ToArray();
//
// senddata = senddata.Concat(myByteArray).ToArray();
temp = !temp;
while (temp)
{
Message.Show("测试灯光，长度 = " + senddata.Length);
await WriteCommPacketAsync(id, MavComm.COMM_DOWNLOAD_COMM, senddata);
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();
}
}
}
Reference in New Issue Copy Permalink