group/Plane.Sdk3
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

通讯检测

Browse Source
This commit is contained in:
pxzleo 2023-05-01 13:57:32 +08:00
parent 8f2cd98bd4
commit f175def6a7
7 changed files with 125 additions and 24 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

5
PlaneGcsSdk.Contract_Shared/Copters/Constants.cs
View File

@ -23,5 +23,10 @@
/// 最高速度，单位为 m/s。
/// </summary>
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; //下降速度
}
}

14
PlaneGcsSdk_Shared/Communication/TcpConnection.cs
View File

@ -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);
}

3
PlaneGcsSdk_Shared/Communication/TcpConnectionBase.cs
View File

@ -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)

51
PlaneGcsSdk_Shared/CommunicationManagement/CommModule.cs
View File

@ -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<byte[]> 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);

68
PlaneGcsSdk_Shared/CommunicationManagement/CommModuleGenerateMavLink.cs
View File

@ -836,6 +836,30 @@ namespace Plane.CommunicationManagement
}
}
public async Task GetCommsumAsync(IEnumerable<ICopter> 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($"未开发完成！！");
}
}
/// <summary>
/// 降落
/// </summary>
@ -1097,11 +1121,11 @@ namespace Plane.CommunicationManagement
await WriteCommPacketAsync(copterId, MavComm.COMM_DOWNLOAD_COMM, packet).ConfigureAwait(false);
}
byte rtcm_tmpser = 0;
/// <summary>
///广播Rtk
///广播Rtk //-------------使用中的RTK广播函数------------
/// </summary>
/// <param name="data"></param>
/// <param name="length"></param>
@ -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;
}
}
*/
}
}

4
PlaneGcsSdk_Shared/CommunicationManagement/CommModulePacketAnalysis.cs
View File

@ -114,6 +114,10 @@ namespace Plane.CommunicationManagement
else
Message.Show($"飞机{copterId}:更新进度{buffer[1]}%");
break;
default:
// Message.Show($"Comm3返回：{type}");
break;
}
}

4
PlaneGcsSdk_Shared/Copters/FakeCopter.cs
View File

@ -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);
}