From a88311a160b6882dc4b2b090526dd734a4491389 Mon Sep 17 00:00:00 2001 From: xu Date: Fri, 28 Jun 2024 18:33:12 +0800 Subject: [PATCH] =?UTF-8?q?[feat]=20=E8=B0=83=E6=95=B4=E5=B9=BF=E6=92=AD?= =?UTF-8?q?=E5=8F=91=E9=80=81=E6=96=B9=E5=BC=8F=20=20=20=E8=B0=83=E6=95=B4?= =?UTF-8?q?=E5=B9=BF=E6=92=AD=E5=8F=91=E9=80=81=E5=92=8C=E9=80=9A=E8=AE=AF?= =?UTF-8?q?=E6=A8=A1=E5=9D=97=E5=8F=91=E9=80=81=E4=B8=80=E6=A0=B7=E7=9A=84?= =?UTF-8?q?=E6=95=B0=E6=8D=AE=E4=B8=BA=E4=BA=86=E5=85=BC=E5=AE=B9=E6=9C=89?= =?UTF-8?q?=E6=A8=A1=E5=9D=97=E5=92=8C=E6=B2=A1=E6=A8=A1=E5=9D=97=E7=9A=84?= =?UTF-8?q?=EF=BC=8C=E4=B9=9F=E5=8F=AF=E4=BB=A5=E5=8F=8C=E9=80=9A=E9=81=93?= =?UTF-8?q?=E5=90=8C=E6=97=B6=E5=8F=91=E9=80=81=E6=8F=90=E9=AB=98=E5=8F=AF?= =?UTF-8?q?=E9=9D=A0=E6=80=A7=20#=20=E7=B1=BB=E5=9E=8B=20=E5=8C=85?= =?UTF-8?q?=E5=90=AB:=20#=20feat=EF=BC=9A=E6=96=B0=E5=8A=9F=E8=83=BD?= =?UTF-8?q?=EF=BC=88feature=EF=BC=89=20#=20fix=EF=BC=9A=E4=BF=AE=E8=A1=A5b?= =?UTF-8?q?ug=20#=20docs=EF=BC=9A=E6=96=87=E6=A1=A3=EF=BC=88documentation?= =?UTF-8?q?=EF=BC=89=20#=20style=EF=BC=9A=20=E6=A0=BC=E5=BC=8F=EF=BC=88?= =?UTF-8?q?=E4=B8=8D=E5=BD=B1=E5=93=8D=E4=BB=A3=E7=A0=81=E8=BF=90=E8=A1=8C?= =?UTF-8?q?=E7=9A=84=E5=8F=98=E5=8A=A8=EF=BC=89=20#=20refactor=EF=BC=9A?= =?UTF-8?q?=E9=87=8D=E6=9E=84=EF=BC=88=E5=8D=B3=E4=B8=8D=E6=98=AF=E6=96=B0?= =?UTF-8?q?=E5=A2=9E=E5=8A=9F=E8=83=BD=EF=BC=8C=E4=B9=9F=E4=B8=8D=E6=98=AF?= =?UTF-8?q?=E4=BF=AE=E6=94=B9bug=E7=9A=84=E4=BB=A3=E7=A0=81=E5=8F=98?= =?UTF-8?q?=E5=8A=A8=EF=BC=89=20#=20test=EF=BC=9A=E5=A2=9E=E5=8A=A0?= =?UTF-8?q?=E6=B5=8B=E8=AF=95=20#=20chore=EF=BC=9A=E6=9E=84=E5=BB=BA?= =?UTF-8?q?=E8=BF=87=E7=A8=8B=E6=88=96=E8=BE=85=E5=8A=A9=E5=B7=A5=E5=85=B7?= =?UTF-8?q?=E7=9A=84=E5=8F=98=E5=8A=A8?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../CommModuleGenerateMavLink.cs | 169 +++++++----------- 1 file changed, 61 insertions(+), 108 deletions(-) diff --git a/PlaneGcsSdk_Shared/CommunicationManagement/CommModuleGenerateMavLink.cs b/PlaneGcsSdk_Shared/CommunicationManagement/CommModuleGenerateMavLink.cs index 582c9eb..6fc8f92 100644 --- a/PlaneGcsSdk_Shared/CommunicationManagement/CommModuleGenerateMavLink.cs +++ b/PlaneGcsSdk_Shared/CommunicationManagement/CommModuleGenerateMavLink.cs @@ -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 _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++) + + bool sendok=false; + try + { + RecomPort.Write(packet, 0, packet.Length); + sendok = true; + } + catch (Exception ex) + { + Windows.Messages.Message.Show("转发端口发送失败("+ ex.Message + ")，尝试新打开..."); + } + if (!sendok) { - 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); - try { - RecomPort.Write(packet, 0, packet.Length); + ReOpenRtcmserial(); + if (Recomisopen) Windows.Messages.Message.Show("转发端口打开成功!"); } catch (Exception ex) { - Windows.Messages.Message.Show("转发端口发送失败" + ex.Message); - } - - // await Task.Delay(80).ConfigureAwait(false); //没有延时得话如果超过110长度，连续发可能收不到 - + Windows.Messages.Message.Show("再次打开串口失败" + ex.Message); + ReOpenRtcmserial(); + } } - - - - - /* - MAVLink.mavlink_gps_rtcm_data_t gps = new mavlink_gps_rtcm_data_t(); - var msglen = 180; - - if (length > msglen * 4) - { - 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); - } - catch (Exception ex) - { - Windows.Messages.Message.Show("转发端口发送失败" + ex.Message); - } - } - inject_seq_no++; - */ } - - + // get sum crc 计算数据校验和 public byte checkrtrcmsum(byte[] data, ushort length) { @@ -1328,7 +1260,7 @@ namespace Plane.CommunicationManagement /// /// /// - 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)飞机可以检测出来重复接收的 + // 重发一次，有序列号(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,10 +1428,24 @@ namespace Plane.CommunicationManagement } Recomisopen = false; } + + public string BoardPortStatusStr + { + get { + if (Recomisopen) + return "已打开"+ last_reserialport; + else + return "端口未打开"; + } + + } + + + public bool OpenResendRtcmserial(string reserialport) { - - RecomPort= new SerialPort(reserialport); + last_reserialport = reserialport; + RecomPort = new SerialPort(reserialport); RecomPort.BaudRate = 57600; RecomPort.Parity = Parity.None; RecomPort.StopBits = StopBits.One;