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;