diff --git a/PlaneGcsSdk_Shared/CommunicationManagement/CommModule.cs b/PlaneGcsSdk_Shared/CommunicationManagement/CommModule.cs index 8bc9f02..b1c5e7e 100644 --- a/PlaneGcsSdk_Shared/CommunicationManagement/CommModule.cs +++ b/PlaneGcsSdk_Shared/CommunicationManagement/CommModule.cs @@ -1,6 +1,5 @@ using Plane.Communication; using Plane.Protocols; -using Plane.Windows.Messages; using System; using System.Collections.Generic; using System.Diagnostics; @@ -13,6 +12,7 @@ using System.Threading.Tasks; using System.Windows.Media; using System.Runtime.InteropServices; +using Plane.Windows.Messages; namespace Plane.CommunicationManagement { diff --git a/PlaneGcsSdk_Shared/CommunicationManagement/CommModuleGenerateMavLink.cs b/PlaneGcsSdk_Shared/CommunicationManagement/CommModuleGenerateMavLink.cs index a6bbf34..289115a 100644 --- a/PlaneGcsSdk_Shared/CommunicationManagement/CommModuleGenerateMavLink.cs +++ b/PlaneGcsSdk_Shared/CommunicationManagement/CommModuleGenerateMavLink.cs @@ -1,8 +1,8 @@ using Plane.Copters; using Plane.Protocols; -using Plane.Windows.Messages; using System; using System.Collections.Generic; +using System.IO.Ports; using System.Linq; using System.Text; using System.Threading.Tasks; @@ -19,6 +19,10 @@ namespace Plane.CommunicationManagement private DateTime waitRtcmTime = DateTime.Now; private bool starttime = false; private bool rtcm_run = false; + private SerialPort RecomPort; + private bool Recomisopen = false; + + private IEnumerable _allcopters; //是否使用专用传输模块 public bool UseTransModule = true; @@ -1130,7 +1134,7 @@ namespace Plane.CommunicationManagement /// /// /// - public async Task InjectGpsDataAsync(byte[] data, ushort length) + public async Task InjectGpsDataAsync(byte[] data, ushort length, bool resendtocom = false) { if (UseTransModule) { @@ -1153,6 +1157,18 @@ 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 (resendtocom&& Recomisopen) + { + try + { + RecomPort.Write(packet, 0, packet.Length); + } + catch (Exception ex) + { + // Alert.Show("转发端口打开失败" + ex.Message); + } + } await Task.Delay(50).ConfigureAwait(false); //没有延时得话如果超过110长度,连续发可能收不到 //重发一次,有序列号(target_component)飞机可以检测出来重复接收的 @@ -1278,8 +1294,41 @@ namespace Plane.CommunicationManagement await Task.Delay(1); } - - + public void CloseResendRtcmserial() + { + if (Recomisopen) + { + RecomPort.Close(); + RecomPort.Dispose(); + } + Recomisopen = false; + } + public bool OpenResendRtcmserial(string reserialport) + { + + RecomPort= new SerialPort(reserialport); + RecomPort.BaudRate = 57600; + RecomPort.Parity = Parity.None; + RecomPort.StopBits = StopBits.One; + RecomPort.DataBits = 8; + RecomPort.Handshake = Handshake.None; + + try + { + RecomPort.Open(); + Recomisopen = true; + } + catch (Exception ex) + { + // Alert.Show("转发端口打开失败" + ex.Message); + } + + return Recomisopen; + } + + + + /// /// 产生RTK包 ///