diff --git a/PlaneGcsSdk_Shared/CommunicationManagement/CommModuleGenerateMavLink.cs b/PlaneGcsSdk_Shared/CommunicationManagement/CommModuleGenerateMavLink.cs index 582c9eb..6fc8f92 100644 --- a/PlaneGcsSdk_Shared/CommunicationManagement/CommModuleGenerateMavLink.cs +++ b/PlaneGcsSdk_Shared/CommunicationManagement/CommModuleGenerateMavLink.cs @@ -22,7 +22,7 @@ namespace Plane.CommunicationManagement private bool starttime = false; private bool rtcm_run = false; private SerialPort RecomPort; - private bool Recomisopen = false; + public bool Recomisopen = false; private IEnumerable _allcopters; @@ -551,6 +551,13 @@ namespace Plane.CommunicationManagement await vcopter.UnlockAsync(); } } + //发送到广播模块--只针对全部飞机 + //if (enrecom&&(copters == null)) + { + // if (enrecom) + + + } } @@ -1207,109 +1214,34 @@ namespace Plane.CommunicationManagement byte rtcm_Broadser = 0;//用于广播的 - - public void BroadcastGpsDataAsync(byte[] data, ushort length) + public void BroadcastbackupGpsDataAsync(byte[] packet) { - //广播发送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++) + + bool sendok=false; + try + { + RecomPort.Write(packet, 0, packet.Length); + sendok = true; + } + catch (Exception ex) + { + Windows.Messages.Message.Show("转发端口发送失败("+ ex.Message + ")，尝试新打开..."); + } + if (!sendok) { - 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); + ReOpenRtcmserial(); + if (Recomisopen) Windows.Messages.Message.Show("转发端口打开成功!"); } catch (Exception ex) { - Windows.Messages.Message.Show("转发端口发送失败" + ex.Message); - } - - // await Task.Delay(80).ConfigureAwait(false); //没有延时得话如果超过110长度，连续发可能收不到 - + Windows.Messages.Message.Show("再次打开串口失败" + ex.Message); + ReOpenRtcmserial(); + } } - - - - - /* - 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++; - */ } - - + // get sum crc 计算数据校验和 public byte checkrtrcmsum(byte[] data, ushort length) { @@ -1328,7 +1260,7 @@ namespace Plane.CommunicationManagement /// /// /// - public async Task InjectGpsDataAsync(byte[] data, ushort length) + public async Task InjectGpsDataAsync(byte[] data, ushort length,bool enrecom) { //由于通讯模块限制一次只能发110个字节 if (UseTransModule) @@ -1353,22 +1285,21 @@ namespace Plane.CommunicationManagement byte[] packet = GenerateRTKPacketAsync(MAVLink.MAVLINK_MSG_ID_GPS_INJECT_DATA, gps, gps.len); + //发送到通讯模块 await WriteCommPacketAsync(0, MavComm.COMM_DOWNLOAD_COMM, packet).ConfigureAwait(false); - + //发送到广播端口作为备用数据源 + if (enrecom) + BroadcastbackupGpsDataAsync(packet); await Task.Delay(80).ConfigureAwait(false); //没有延时得话如果超过110长度，连续发可能收不到 - // 重发一次，有序列号(target_component)飞机可以检测出来重复接收的 + // 重发一次，有序列号(target_component)飞机可以检测出来重复接收的 //需要新固件支持 await WriteCommPacketAsync(0, MavComm.COMM_DOWNLOAD_COMM, packet).ConfigureAwait(false); + //发送到广播端口作为备用数据源 + if (enrecom) + BroadcastbackupGpsDataAsync(packet); + await Task.Delay(80).ConfigureAwait(false); //没有延时得话如果超过110长度，连续发可能收不到 - - - //重发一次，有序列号(target_component)飞机可以检测出来重复接收的--需要新固件支持 - //await WriteCommPacketAsync(0, MavComm.COMM_DOWNLOAD_COMM, packet).ConfigureAwait(false); - // await Task.Delay(80).ConfigureAwait(false); //没有延时得话如果超过110长度，连续发可能收不到 - - - } } else @@ -1480,6 +1411,14 @@ namespace Plane.CommunicationManagement await Task.Delay(1); } + string last_reserialport; + public void ReOpenRtcmserial() + { + CloseResendRtcmserial(); + OpenResendRtcmserial(last_reserialport); + + } + public void CloseResendRtcmserial() { if (Recomisopen) @@ -1489,10 +1428,24 @@ namespace Plane.CommunicationManagement } Recomisopen = false; } + + public string BoardPortStatusStr + { + get { + if (Recomisopen) + return "已打开"+ last_reserialport; + else + return "端口未打开"; + } + + } + + + public bool OpenResendRtcmserial(string reserialport) { - - RecomPort= new SerialPort(reserialport); + last_reserialport = reserialport; + RecomPort = new SerialPort(reserialport); RecomPort.BaudRate = 57600; RecomPort.Parity = Parity.None; RecomPort.StopBits = StopBits.One;