diff --git a/PlaneGcsSdk_Shared/CommunicationManagement/CommModule.cs b/PlaneGcsSdk_Shared/CommunicationManagement/CommModule.cs index 9a2214f..8316c7a 100644 --- a/PlaneGcsSdk_Shared/CommunicationManagement/CommModule.cs +++ b/PlaneGcsSdk_Shared/CommunicationManagement/CommModule.cs @@ -243,6 +243,7 @@ namespace Plane.CommunicationManagement */ int times = 0; + //这里容易报空引用异常 ,没有连接通讯设备时 private void Reconnect() { // Message.Show($"正在重新连接..."); diff --git a/PlaneGcsSdk_Shared/CommunicationManagement/CommModuleGenerateMavLink.cs b/PlaneGcsSdk_Shared/CommunicationManagement/CommModuleGenerateMavLink.cs index 086a252..d1ecd89 100644 --- a/PlaneGcsSdk_Shared/CommunicationManagement/CommModuleGenerateMavLink.cs +++ b/PlaneGcsSdk_Shared/CommunicationManagement/CommModuleGenerateMavLink.cs @@ -1203,7 +1203,9 @@ namespace Plane.CommunicationManagement } - byte rtcm_tmpser = 0; + byte rtcm_tmpser = 0; //用于通讯模块的 + byte rtcm_Broadser = 0;//用于广播的 + public void BroadcastGpsDataAsync(byte[] data, ushort length) @@ -1211,7 +1213,43 @@ namespace Plane.CommunicationManagement //广播发送RTCM数据采用专用数据可以一次发180个字节 if (!Recomisopen) return; - + MAVLink.mavlink_gps_inject_data_t gps = new MAVLink.mavlink_gps_inject_data_t(); + var msglen = 110; + int datalen = 0; + var len = (length % msglen) == 0 ? length / msglen : (length / msglen) + 1; + for (int a = 0; a < len; a++) + { + datalen = Math.Min(length - a * msglen, msglen); + gps.data = new byte[msglen]; + Array.Copy(data, a * msglen, gps.data, 0, datalen); + //gps.data[0] = rtcm_Broadser++; + gps.len = (byte)datalen; + gps.target_component = rtcm_Broadser++; + //实测一旦收到数据都是正确的,张伟已经做过crc检验了,为了兼容性不再做了 + //如果需要做,飞控里面需要把注释的代码放开,并且不再兼容之前的固件 + gps.target_system = 1;// checkrtrcmsum(gps.data, (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); + + try + { + RecomPort.Write(packet, 0, packet.Length); + } + catch (Exception ex) + { + Windows.Messages.Message.Show("转发端口发送失败" + ex.Message); + } + + // await Task.Delay(80).ConfigureAwait(false); //没有延时得话如果超过110长度,连续发可能收不到 + + } + + + + + /* MAVLink.mavlink_gps_rtcm_data_t gps = new mavlink_gps_rtcm_data_t(); var msglen = 180; @@ -1268,7 +1306,7 @@ namespace Plane.CommunicationManagement } } inject_seq_no++; - + */ }