1加入RTCM数据广播到指定端口功能

2临时修改协议头到通用的FE，后面要改回来

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++;
+            
+        }
+
+
+
+
         /// <summary>
         ///广播Rtk               //-------------使用中的RTK广播函数------------
         /// </summary>
         /// <param name="data"></param>
         /// <param name="length"></param>
         /// <returns></returns>
-        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)飞机可以检测出来重复接收的

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
+        //协议头 飞控4.0以后 254 - 240  Mavlink头 FE 改为 F0 
-        //public const byte MAVLINK_STX = 254;
+        public const byte MAVLINK_STX = 254;
-        public const byte MAVLINK_STX = 240;
+        //public const byte MAVLINK_STX = 240;
 
         public const byte MAVLINK_ENDIAN = MAVLINK_LITTLE_ENDIAN;