From 14d102277573f8e3f070d72b4a3889a914b0dc50 Mon Sep 17 00:00:00 2001 From: pxzleo Date: Thu, 19 Oct 2023 22:14:37 +0800 Subject: [PATCH] =?UTF-8?q?1=E5=8A=A0=E5=85=A5RTCM=E6=95=B0=E6=8D=AE?= =?UTF-8?q?=E5=B9=BF=E6=92=AD=E5=88=B0=E6=8C=87=E5=AE=9A=E7=AB=AF=E5=8F=A3?= =?UTF-8?q?=E5=8A=9F=E8=83=BD?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit 2临时修改协议头到通用的FE，后面要改回来 --- .../CommModuleGenerateMavLink.cs | 88 ++++++++++++++++--- PlaneGcsSdk_Shared/Protocols/MAVLinkTypes.cs | 6 +- 2 files changed, 77 insertions(+), 17 deletions(-) diff --git a/PlaneGcsSdk_Shared/CommunicationManagement/CommModuleGenerateMavLink.cs b/PlaneGcsSdk_Shared/CommunicationManagement/CommModuleGenerateMavLink.cs index 289115a..71195dc 100644 --- a/PlaneGcsSdk_Shared/CommunicationManagement/CommModuleGenerateMavLink.cs +++ b/PlaneGcsSdk_Shared/CommunicationManagement/CommModuleGenerateMavLink.cs @@ -8,6 +8,7 @@ using System.Text; using System.Threading.Tasks; using System.Windows.Media; using static Plane.Copters.PlaneCopter; +using static Plane.Protocols.MAVLink; namespace Plane.CommunicationManagement { @@ -1128,14 +1129,84 @@ namespace Plane.CommunicationManagement byte rtcm_tmpser = 0; + + public void BroadcastGpsDataAsync(byte[] data, ushort length) + { + //广播发送RTCM数据采用专用数据可以一次发180个字节 + if (!Recomisopen) + return; + + 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++; + + } + + + + /// ///广播Rtk //-------------使用中的RTK广播函数------------ /// /// /// /// - public async Task InjectGpsDataAsync(byte[] data, ushort length, bool resendtocom = false) + public async Task InjectGpsDataAsync(byte[] data, ushort length) { + //由于通讯模块限制一次只能发110个字节 if (UseTransModule) { MAVLink.mavlink_gps_inject_data_t gps = new MAVLink.mavlink_gps_inject_data_t(); @@ -1152,23 +1223,12 @@ namespace Plane.CommunicationManagement gps.target_component = rtcm_tmpser++; gps.target_system = 1; - Console.WriteLine(" ["+ gps.target_component + "]总长: "+ length +" 发送第 "+ (a+1) + " 个包,长度: " + (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); 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)飞机可以检测出来重复接收的 diff --git a/PlaneGcsSdk_Shared/Protocols/MAVLinkTypes.cs b/PlaneGcsSdk_Shared/Protocols/MAVLinkTypes.cs index 19002d8..2ffad51 100644 --- a/PlaneGcsSdk_Shared/Protocols/MAVLinkTypes.cs +++ b/PlaneGcsSdk_Shared/Protocols/MAVLinkTypes.cs @@ -17,9 +17,9 @@ namespace Plane.Protocols public const int MAVLINK_LITTLE_ENDIAN = 1; public const int MAVLINK_BIG_ENDIAN = 0; - //ɿ4.0Ժ 254 - 240 - //public const byte MAVLINK_STX = 254; - public const byte MAVLINK_STX = 240; + //Эͷ ɿ4.0Ժ 254 - 240 Mavlinkͷ FE Ϊ F0 + public const byte MAVLINK_STX = 254; + //public const byte MAVLINK_STX = 240; public const byte MAVLINK_ENDIAN = MAVLINK_LITTLE_ENDIAN;