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;