From f175def6a74b70a671b35d19aed940614b7122b2 Mon Sep 17 00:00:00 2001 From: pxzleo Date: Mon, 1 May 2023 13:57:32 +0800 Subject: [PATCH] =?UTF-8?q?=E9=80=9A=E8=AE=AF=E6=A3=80=E6=B5=8B?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../Copters/Constants.cs | 5 ++ .../Communication/TcpConnection.cs | 14 ++-- .../Communication/TcpConnectionBase.cs | 3 + .../CommunicationManagement/CommModule.cs | 51 +++++++++++--- .../CommModuleGenerateMavLink.cs | 68 +++++++++++++++++-- .../CommModulePacketAnalysis.cs | 4 ++ PlaneGcsSdk_Shared/Copters/FakeCopter.cs | 4 ++ 7 files changed, 125 insertions(+), 24 deletions(-) diff --git a/PlaneGcsSdk.Contract_Shared/Copters/Constants.cs b/PlaneGcsSdk.Contract_Shared/Copters/Constants.cs index c5957b3..b4bc30e 100644 --- a/PlaneGcsSdk.Contract_Shared/Copters/Constants.cs +++ b/PlaneGcsSdk.Contract_Shared/Copters/Constants.cs @@ -23,5 +23,10 @@ /// 最高速度，单位为 m/s。 /// public const float MAX_VEL = 5f; + + public const float MAX_VEL_XY = 5.0f; //米/秒 水平速度 + public const float MAX_VEL_UP = 2.5f; //上升速度 + public const float MAX_VEL_DOWN = 1.5f; //下降速度 + } } diff --git a/PlaneGcsSdk_Shared/Communication/TcpConnection.cs b/PlaneGcsSdk_Shared/Communication/TcpConnection.cs index cc713a9..8cf256a 100644 --- a/PlaneGcsSdk_Shared/Communication/TcpConnection.cs +++ b/PlaneGcsSdk_Shared/Communication/TcpConnection.cs @@ -13,6 +13,8 @@ namespace Plane.Communication public class TcpConnection : TcpConnectionBase { private string _remoteHostname; + const int tcpReceiveBufferSize = 20 * 1024; //40 * 1024, + const int tcpReceiveTimeout = 1200; private int _remotePort; @@ -22,8 +24,8 @@ namespace Plane.Communication _remotePort = remotePort; _client = new TcpClient { - ReceiveBufferSize = 40 * 1024, - ReceiveTimeout = 1200 + ReceiveBufferSize = tcpReceiveBufferSize, + ReceiveTimeout = tcpReceiveTimeout }; } @@ -96,8 +98,8 @@ namespace Plane.Communication { _client = new TcpClient(_remoteHostname, _remotePort) { - ReceiveBufferSize = 40 * 1024, - ReceiveTimeout = 1200 + ReceiveBufferSize = tcpReceiveBufferSize, + ReceiveTimeout = tcpReceiveTimeout }; } @@ -105,8 +107,8 @@ namespace Plane.Communication { _client = new TcpClient { - ReceiveBufferSize = 40 * 1024, - ReceiveTimeout = 1200 + ReceiveBufferSize = tcpReceiveBufferSize,// 40 * 1024, + ReceiveTimeout = tcpReceiveTimeout// 1200 }; await _client.ConnectAsync(_remoteHostname, _remotePort).ConfigureAwait(false); } diff --git a/PlaneGcsSdk_Shared/Communication/TcpConnectionBase.cs b/PlaneGcsSdk_Shared/Communication/TcpConnectionBase.cs index aedcd37..1d2b28a 100644 --- a/PlaneGcsSdk_Shared/Communication/TcpConnectionBase.cs +++ b/PlaneGcsSdk_Shared/Communication/TcpConnectionBase.cs @@ -43,6 +43,9 @@ namespace Plane.Communication { try { + // bool bret ; + // bret = _client.Client != null; + // bret = bret && !((_client.Client.Poll(1000, SelectMode.SelectRead) && (_client.Client.Available == 0)) || !_client.Client.Connected); return _client.Client != null && !((_client.Client.Poll(1000, SelectMode.SelectRead) && (_client.Client.Available == 0)) || !_client.Client.Connected); } catch (ObjectDisposedException) diff --git a/PlaneGcsSdk_Shared/CommunicationManagement/CommModule.cs b/PlaneGcsSdk_Shared/CommunicationManagement/CommModule.cs index 9549ece..8bc9f02 100644 --- a/PlaneGcsSdk_Shared/CommunicationManagement/CommModule.cs +++ b/PlaneGcsSdk_Shared/CommunicationManagement/CommModule.cs @@ -195,35 +195,54 @@ namespace Plane.CommunicationManagement Task.Run(async () => { long lastPacketCountNum = 0; + int faulttimes = 0; //等待没有数据次数 + const int revtimeout = 1200; //等待超时ms while (CommOK) { if (Connection != null) { - await SendQueryStatusPacketsAsync(); - await Task.Delay(1200).ConfigureAwait(false); - + //发送心跳包-等待回复消息 + await WriteCommPacketAsync(0, MavComm.COMM_QUERY_STATUS); + // await SendQueryStatusPacketsAsync(); + await Task.Delay(revtimeout).ConfigureAwait(false); //1200 + //等待是否接受到返回的数据包，如果没有收到认为连结断开，重新联接 if (packetCountNum > lastPacketCountNum) + { lastPacketCountNum = packetCountNum; + if (faulttimes>0) + Message.Show("收到新数据包，重置超时次数..."); + faulttimes = 0; + } else - break; + { + faulttimes++; + Message.Show("设备回复超时("+ revtimeout + "ms)" + faulttimes+"次"); + //超过3次没数据包返回就重连 + if (faulttimes > 3) + { + Message.Show("长时间未收到设备回复数据包"); + break; + } + } } } - Message.Show("通信断开"); - + Message.Show("主动断开连接，重连"); Reconnect(); }).ConfigureAwait(false); } - private async Task SendQueryStatusPacketsAsync() + /* //直接调用不用封装函数了 + private async Task SendQueryStatusPacketsAsync() { await WriteCommPacketAsync(0, MavComm.COMM_QUERY_STATUS); - await Task.Delay(100); + await Task.Delay(100);//100 } + */ int times = 1; private void Reconnect() { - //Message.Show($"正在重新连接..."); + // Message.Show($"正在重新连接..."); Task.Run(async () => { CloseConnection(); @@ -243,6 +262,12 @@ namespace Plane.CommunicationManagement { if (Connection == null || !Connection.IsOnline) { + if (Connection==null) + Message.Show("空连接异常"); + else if (!Connection.IsOnline) + { + Message.Show("检测到设备已断开连接"); + } break; } @@ -301,7 +326,8 @@ namespace Plane.CommunicationManagement } } }).ConfigureAwait(false); - Message.Show("连接断开"); + Message.Show("退出读设备数据........"); + //Reconnect(); } private async Task ReadPacketAsync() @@ -680,7 +706,10 @@ namespace Plane.CommunicationManagement 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); diff --git a/PlaneGcsSdk_Shared/CommunicationManagement/CommModuleGenerateMavLink.cs b/PlaneGcsSdk_Shared/CommunicationManagement/CommModuleGenerateMavLink.cs index 5b458ca..a6bbf34 100644 --- a/PlaneGcsSdk_Shared/CommunicationManagement/CommModuleGenerateMavLink.cs +++ b/PlaneGcsSdk_Shared/CommunicationManagement/CommModuleGenerateMavLink.cs @@ -836,6 +836,30 @@ namespace Plane.CommunicationManagement } } + + public async Task GetCommsumAsync(IEnumerable copters = null) + { + if (UseTransModule) + { + short copterId = 0; + byte[] batchPacket = null; + GetCopterIds(copters, out copterId, out batchPacket); + + MAVLink.mavlink_sversion_request req = new MAVLink.mavlink_sversion_request(); + req.version = 1; + byte[] packet = GeneratePacket(MAVLink.MAVLINK_MSG_ID_AUTOPILOT_VERSION_REQUEST, req); + await WriteCommPacketAsync((short)copterId, MavComm.COMM_DOWNLOAD_COMM, packet, batchPacket).ConfigureAwait(false); + } + else + { + + Windows.Messages.Message.Show($"未开发完成！！"); + } + + } + + + /// /// 降落 /// @@ -1097,11 +1121,11 @@ namespace Plane.CommunicationManagement await WriteCommPacketAsync(copterId, MavComm.COMM_DOWNLOAD_COMM, packet).ConfigureAwait(false); } - + byte rtcm_tmpser = 0; /// - ///广播Rtk + ///广播Rtk //-------------使用中的RTK广播函数------------ /// /// /// @@ -1119,13 +1143,33 @@ namespace Plane.CommunicationManagement datalen = Math.Min(length - a * msglen, msglen); gps.data = new byte[msglen]; Array.Copy(data, a * msglen, gps.data, 0, datalen); + //gps.data[0] = rtcm_tmpser++; gps.len = (byte)datalen; - gps.target_component = 1; + gps.target_component = rtcm_tmpser++; gps.target_system = 1; + + Console.WriteLine(" ["+ gps.target_component + "]总长: "+ length +" 发送第 "+ (a+1) + " 个包,长度: " + (ushort)datalen ); + byte[] packet = GenerateRTKPacketAsync(MAVLink.MAVLINK_MSG_ID_GPS_INJECT_DATA, gps, gps.len); + await WriteCommPacketAsync(0, MavComm.COMM_DOWNLOAD_COMM, packet).ConfigureAwait(false); + await Task.Delay(50).ConfigureAwait(false); //没有延时得话如果超过110长度，连续发可能收不到 + + //重发一次，有序列号(target_component)飞机可以检测出来重复接收的 + //需要新固件支持 + // await WriteCommPacketAsync(0, MavComm.COMM_DOWNLOAD_COMM, packet).ConfigureAwait(false); + // await Task.Delay(50).ConfigureAwait(false); //没有延时得话如果超过110长度，连续发可能收不到 + + /* + //重发一次，有序列号(target_component)飞机可以检测出来重复接收的--需要新固件支持 + await WriteCommPacketAsync(0, MavComm.COMM_DOWNLOAD_COMM, packet).ConfigureAwait(false); + await Task.Delay(50).ConfigureAwait(false); //没有延时得话如果超过110长度，连续发可能收不到 + */ + + } - }else + } + else { if (_allcopters != null) foreach (var vcopter in _allcopters) @@ -1144,7 +1188,7 @@ namespace Plane.CommunicationManagement public async Task InjectGpsRTCMDataAsync(byte[] data, int length) { MAVLink.mavlink_gps_rtcm_data_t gps = new MAVLink.mavlink_gps_rtcm_data_t(); - var msglen = 180; + var msglen = 180; //飞机端是180，之前也是180和通讯盒可能也有关系 // if (length > msglen * 4) // log.Error("Message too large " + length); @@ -1152,6 +1196,9 @@ namespace Plane.CommunicationManagement // number of packets we need, including a termination packet if needed var nopackets = (length % msglen) == 0 ? length / msglen + 1 : (length / msglen) + 1; + // if (nopackets>1) + // Plane.Windows.Messages.Message.Show(" RTCM_Mut: " + length); + if (nopackets >= 4) nopackets = 4; @@ -1182,12 +1229,18 @@ namespace Plane.CommunicationManagement // set the length gps.len = (byte)copy; - + byte[] packet = GeneratePacket(MAVLink.MAVLINK_MSG_ID_GPS_RTCM_DATA, gps); + + // packet[2] + //Plane.Windows.Messages.Message.Show("["+ packet[2]+"]RTCM:"+ gps.len); + + // Console.WriteLine("RTCM:" + gps.len+ "[" + data[0]+","+ data[1] + "," + data[2] + "," + data[3] + "," + data[4]+"]"); + //Windows.Messages.Message.Show($"{DateTime.Now.ToString("HH:mm:ss:fff")} 单次长度 = {packet.Length}"); //老版本在这发送，新版本在StartRtcmLoop里面一次性打包4个180字节发送了 await WriteCommPacketAsync(0, MavComm.COMM_DOWNLOAD_COMM, packet).ConfigureAwait(false); - + /* //长RTCP数据发送还没开发完 lock (lock_rtcm) { rtcm_packets.Add(packet); @@ -1197,6 +1250,7 @@ namespace Plane.CommunicationManagement starttime = true; } } + */ } } diff --git a/PlaneGcsSdk_Shared/CommunicationManagement/CommModulePacketAnalysis.cs b/PlaneGcsSdk_Shared/CommunicationManagement/CommModulePacketAnalysis.cs index d1a1c09..5eb2799 100644 --- a/PlaneGcsSdk_Shared/CommunicationManagement/CommModulePacketAnalysis.cs +++ b/PlaneGcsSdk_Shared/CommunicationManagement/CommModulePacketAnalysis.cs @@ -114,6 +114,10 @@ namespace Plane.CommunicationManagement else Message.Show($"飞机{copterId}:更新进度{buffer[1]}%"); break; + + default: + // Message.Show($"Comm3返回：{type}"); + break; } } diff --git a/PlaneGcsSdk_Shared/Copters/FakeCopter.cs b/PlaneGcsSdk_Shared/Copters/FakeCopter.cs index 459c39a..132aba8 100644 --- a/PlaneGcsSdk_Shared/Copters/FakeCopter.cs +++ b/PlaneGcsSdk_Shared/Copters/FakeCopter.cs @@ -109,9 +109,13 @@ namespace Plane.Copters get { return UPDATE_INTERVAL; } set { + //最大移动距离 MAX_MOVE_IN_INTERVAL = MAX_VEL * value / 1000; + //快速模式最大移动距离 MAX_MOVE_IN_INTERVAL_FAST = MAX_MOVE_IN_INTERVAL * 4; + //速度缩放后快速速度距离 _scaledFastMaxMoveInInterval = MAX_MOVE_IN_INTERVAL_FAST* _speedScale; + //速度缩放后速度距离 _scaledMaxMoveInInterval = MAX_MOVE_IN_INTERVAL* _speedScale; Set(nameof(sim_update_int), ref UPDATE_INTERVAL, value); }