[feat] 调整广播发送方式
调整广播发送和通讯模块发送一样的数据为了兼容有模块和没模块的，也可以双通道同时发送提高可靠性 # 类型 包含: # feat：新功能（feature） # fix：修补bug # docs：文档（documentation） # style： 格式（不影响代码运行的变动） # refactor：重构（即不是新增功能，也不是修改bug的代码变动） # test：增加测试 # chore：构建过程或辅助工具的变动
This commit is contained in:
parent
e909b7a3d3
commit
a88311a160
@ -22,7 +22,7 @@ namespace Plane.CommunicationManagement
|
||||
private bool starttime = false;
|
||||
private bool rtcm_run = false;
|
||||
private SerialPort RecomPort;
|
||||
private bool Recomisopen = false;
|
||||
public bool Recomisopen = false;
|
||||
|
||||
|
||||
private IEnumerable<ICopter> _allcopters;
|
||||
@ -551,6 +551,13 @@ namespace Plane.CommunicationManagement
|
||||
await vcopter.UnlockAsync();
|
||||
}
|
||||
}
|
||||
//发送到广播模块--只针对全部飞机
|
||||
//if (enrecom&&(copters == null))
|
||||
{
|
||||
// if (enrecom)
|
||||
|
||||
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
@ -1207,109 +1214,34 @@ namespace Plane.CommunicationManagement
|
||||
byte rtcm_Broadser = 0;//用于广播的
|
||||
|
||||
|
||||
|
||||
public void BroadcastGpsDataAsync(byte[] data, ushort length)
|
||||
public void BroadcastbackupGpsDataAsync(byte[] packet)
|
||||
{
|
||||
//广播发送RTCM数据采用专用数据可以一次发180个字节
|
||||
if (!Recomisopen)
|
||||
return;
|
||||
MAVLink.mavlink_gps_inject_data_t gps = new MAVLink.mavlink_gps_inject_data_t();
|
||||
var msglen = 110;
|
||||
int datalen = 0;
|
||||
var len = (length % msglen) == 0 ? length / msglen : (length / msglen) + 1;
|
||||
for (int a = 0; a < len; a++)
|
||||
{
|
||||
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_Broadser++;
|
||||
gps.len = (byte)datalen;
|
||||
gps.target_component = rtcm_Broadser++;
|
||||
//实测一旦收到数据都是正确的，张伟已经做过crc检验了，为了兼容性不再做了
|
||||
//如果需要做，飞控里面需要把注释的代码放开，并且不再兼容之前的固件
|
||||
gps.target_system = 1;// checkrtrcmsum(gps.data, (ushort)datalen);
|
||||
|
||||
// Console.WriteLine(" ["+ gps.target_component + "]总长: "+ length +" 发送第 "+ (a+1) + " 个包,长度: " + (ushort)datalen );
|
||||
|
||||
byte[] packet = GenerateRTKPacketAsync(MAVLink.MAVLINK_MSG_ID_GPS_INJECT_DATA, gps, gps.len);
|
||||
|
||||
bool sendok=false;
|
||||
try
|
||||
{
|
||||
RecomPort.Write(packet, 0, packet.Length);
|
||||
sendok = true;
|
||||
}
|
||||
catch (Exception ex)
|
||||
{
|
||||
Windows.Messages.Message.Show("转发端口发送失败" + ex.Message);
|
||||
Windows.Messages.Message.Show("转发端口发送失败("+ ex.Message + ")，尝试新打开...");
|
||||
}
|
||||
|
||||
// await Task.Delay(80).ConfigureAwait(false); //没有延时得话如果超过110长度，连续发可能收不到
|
||||
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
/*
|
||||
MAVLink.mavlink_gps_rtcm_data_t gps = new mavlink_gps_rtcm_data_t();
|
||||
var msglen = 180;
|
||||
|
||||
if (length > msglen * 4)
|
||||
if (!sendok)
|
||||
{
|
||||
Windows.Messages.Message.Show($"RTCM数据太长:" + length);
|
||||
return;
|
||||
}
|
||||
|
||||
// number of packets we need, including a termination packet if needed
|
||||
var nopackets = (length % msglen) == 0 ? length / msglen + 1 : (length / msglen) + 1;
|
||||
|
||||
if (nopackets >= 4)
|
||||
nopackets = 4;
|
||||
|
||||
// flags = isfrag(1)/frag(2)/seq(5)
|
||||
|
||||
for (int a = 0; a < nopackets; a++)
|
||||
{
|
||||
// check if its a fragment
|
||||
if (nopackets > 1)
|
||||
gps.flags = 1;
|
||||
else
|
||||
gps.flags = 0;
|
||||
|
||||
// add fragment number
|
||||
gps.flags += (byte)((a & 0x3) << 1);
|
||||
|
||||
// add seq number
|
||||
gps.flags += (byte)((inject_seq_no & 0x1f) << 3);
|
||||
|
||||
// create the empty buffer
|
||||
gps.data = new byte[msglen];
|
||||
|
||||
// calc how much data we are copying
|
||||
int copy = Math.Min(length - a * msglen, msglen);
|
||||
|
||||
// copy the data
|
||||
Array.Copy(data, a * msglen, gps.data, 0, copy);
|
||||
|
||||
// set the length
|
||||
gps.len = (byte)copy;
|
||||
byte[] packet = GenerateRTKPacketAsync(MAVLink.MAVLINK_MSG_ID_GPS_RTCM_DATA, gps, gps.len);
|
||||
|
||||
//Console.WriteLine(string.Format("{0:T} rtcm send :{1:D}", DateTime.Now, packet.Length));
|
||||
|
||||
try
|
||||
{
|
||||
RecomPort.Write(packet, 0, packet.Length);
|
||||
ReOpenRtcmserial();
|
||||
if (Recomisopen) Windows.Messages.Message.Show("转发端口打开成功!");
|
||||
}
|
||||
catch (Exception ex)
|
||||
{
|
||||
Windows.Messages.Message.Show("转发端口发送失败" + ex.Message);
|
||||
Windows.Messages.Message.Show("再次打开串口失败" + ex.Message);
|
||||
ReOpenRtcmserial();
|
||||
}
|
||||
}
|
||||
inject_seq_no++;
|
||||
*/
|
||||
}
|
||||
|
||||
|
||||
// get sum crc 计算数据校验和
|
||||
public byte checkrtrcmsum(byte[] data, ushort length)
|
||||
{
|
||||
@ -1328,7 +1260,7 @@ namespace Plane.CommunicationManagement
|
||||
/// <param name="data"></param>
|
||||
/// <param name="length"></param>
|
||||
/// <returns></returns>
|
||||
public async Task InjectGpsDataAsync(byte[] data, ushort length)
|
||||
public async Task InjectGpsDataAsync(byte[] data, ushort length,bool enrecom)
|
||||
{
|
||||
//由于通讯模块限制一次只能发110个字节
|
||||
if (UseTransModule)
|
||||
@ -1353,22 +1285,21 @@ namespace Plane.CommunicationManagement
|
||||
|
||||
byte[] packet = GenerateRTKPacketAsync(MAVLink.MAVLINK_MSG_ID_GPS_INJECT_DATA, gps, gps.len);
|
||||
|
||||
//发送到通讯模块
|
||||
await WriteCommPacketAsync(0, MavComm.COMM_DOWNLOAD_COMM, packet).ConfigureAwait(false);
|
||||
|
||||
//发送到广播端口作为备用数据源
|
||||
if (enrecom)
|
||||
BroadcastbackupGpsDataAsync(packet);
|
||||
await Task.Delay(80).ConfigureAwait(false); //没有延时得话如果超过110长度，连续发可能收不到
|
||||
|
||||
// 重发一次，有序列号(target_component)飞机可以检测出来重复接收的
|
||||
//需要新固件支持
|
||||
await WriteCommPacketAsync(0, MavComm.COMM_DOWNLOAD_COMM, packet).ConfigureAwait(false);
|
||||
//发送到广播端口作为备用数据源
|
||||
if (enrecom)
|
||||
BroadcastbackupGpsDataAsync(packet);
|
||||
|
||||
await Task.Delay(80).ConfigureAwait(false); //没有延时得话如果超过110长度，连续发可能收不到
|
||||
|
||||
|
||||
//重发一次，有序列号(target_component)飞机可以检测出来重复接收的--需要新固件支持
|
||||
//await WriteCommPacketAsync(0, MavComm.COMM_DOWNLOAD_COMM, packet).ConfigureAwait(false);
|
||||
// await Task.Delay(80).ConfigureAwait(false); //没有延时得话如果超过110长度，连续发可能收不到
|
||||
|
||||
|
||||
|
||||
}
|
||||
}
|
||||
else
|
||||
@ -1480,6 +1411,14 @@ namespace Plane.CommunicationManagement
|
||||
await Task.Delay(1);
|
||||
}
|
||||
|
||||
string last_reserialport;
|
||||
public void ReOpenRtcmserial()
|
||||
{
|
||||
CloseResendRtcmserial();
|
||||
OpenResendRtcmserial(last_reserialport);
|
||||
|
||||
}
|
||||
|
||||
public void CloseResendRtcmserial()
|
||||
{
|
||||
if (Recomisopen)
|
||||
@ -1489,9 +1428,23 @@ namespace Plane.CommunicationManagement
|
||||
}
|
||||
Recomisopen = false;
|
||||
}
|
||||
|
||||
public string BoardPortStatusStr
|
||||
{
|
||||
get {
|
||||
if (Recomisopen)
|
||||
return "已打开"+ last_reserialport;
|
||||
else
|
||||
return "端口未打开";
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
|
||||
|
||||
public bool OpenResendRtcmserial(string reserialport)
|
||||
{
|
||||
|
||||
last_reserialport = reserialport;
|
||||
RecomPort = new SerialPort(reserialport);
|
||||
RecomPort.BaudRate = 57600;
|
||||
RecomPort.Parity = Parity.None;
|
||||
|
||||
Loading…
Reference in New Issue
Block a user