From 17e4c8bd97cf9883903f2f55e95d38cb0068d1c0 Mon Sep 17 00:00:00 2001 From: pxzleo Date: Wed, 10 Jan 2024 16:15:33 +0800 Subject: [PATCH] =?UTF-8?q?1=E4=BF=AE=E5=A4=8D=E5=BC=80=E7=BD=91=E7=BB=9CR?= =?UTF-8?q?TK=E5=90=8E=EF=BC=8C=E7=95=8C=E9=9D=A2=E5=8D=A1=E9=A1=BFbug,rtk?= =?UTF-8?q?loop=E5=A2=9E=E5=8A=A0=E5=85=B3=E9=94=AE=E5=BB=B6=E8=BF=9F=202?= =?UTF-8?q?=E5=B0=86now=E6=94=B9=E4=B8=BAutcnow=EF=BC=8C=E5=87=8F=E5=B0=91?= =?UTF-8?q?=E7=B3=BB=E7=BB=9F=E5=8D=A0=E7=94=A8?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../Formation/RtcmManager.cs | 1403 +++++++++-------- Plane.FormationCreator/Util/CommNTRIP.cs | 861 +++++----- 2 files changed, 1133 insertions(+), 1131 deletions(-) diff --git a/Plane.FormationCreator/Formation/RtcmManager.cs b/Plane.FormationCreator/Formation/RtcmManager.cs index 1624730..601c936 100644 --- a/Plane.FormationCreator/Formation/RtcmManager.cs +++ b/Plane.FormationCreator/Formation/RtcmManager.cs @@ -1,700 +1,703 @@ -using GalaSoft.MvvmLight; -using Microsoft.Practices.ServiceLocation; -using Plane.CommunicationManagement; -using Plane.FormationCreator.ViewModels; -using Plane.Util; -using Plane.Windows.IniHelper; -using Plane.Windows.Messages; -using System; -using System.Collections; -using System.Collections.Generic; -using System.Collections.ObjectModel; -using System.Linq; -using System.Text; -using System.Threading; -using System.Threading.Tasks; -using System.Windows.Forms; -using System.Windows.Media; -using System.Windows.Threading; - -namespace Plane.FormationCreator.Formation -{ - public class RtcmManager: ObservableObject - { - private rtcm3 rtcm = new rtcm3(); - // ubx detection - private static Ubx ubx_m8p = new Ubx(); - static nmea nmea = new nmea(); - - private Hashtable msgseen = new Hashtable(); - - private ICommsSerial comPort; - private ICommsSerial RecomPort; - private CommModuleManager _commModuleManager = CommModuleManager.Instance; - public ObservableCollection rtcmInfoList { get; } = new ObservableCollection(); - private ControlPanelViewModel ControlPanelVM = ServiceLocator.Current.GetInstance(); - private CopterManager _copterManager = ServiceLocator.Current.GetInstance(); - private bool _rtcmthreadrun = false; - private bool _smallrtcmdata = false; //减少传输数据--用于带宽不够的通讯模块-对数传广播无效 - private bool _enrecom = false;//是否转发到另外串口 - - // rtcm发送类型0:直接发送， - // 1：1秒只发一种卫星，1秒发GPS,第2秒发北斗，第3秒发格洛纳斯，其他数据随来随发 - - private int _resendtocom = 1; - const int MSG_GPS = 1074; - const int MSG_GLONASS = 1084; - const int MSG_Beidou = 1124; - const int MSG_BasePos = 1005; - const int MSG_REV = 1033; - const int MSG_GLONASS_PHASE = 1230; - - - public string rtcm_typename(int msgtype) - { - string typename = "Rtcm" + msgtype; - switch (msgtype) - { - case MSG_GPS: - typename = "GPS"; - break; - case MSG_GLONASS: - typename = "GLONASS"; - break; - case MSG_GLONASS_PHASE: - typename = "GLONASS相位差"; - break; - case MSG_Beidou: - typename = "北斗"; - break; - //位置5秒发一次就可以了 - case MSG_BasePos: - typename = "基站位置"; - break; - case MSG_REV: //这个没必要发 - typename = "版本"; - break; - } - return typename; - - } - public bool Rtcmthreadrun - { - get { return _rtcmthreadrun; } - set { - Set(nameof(Rtcmthreadrun), ref _rtcmthreadrun, value); - - if (value) - ControlPanelVM.RTKState = "RTK发送中..."; - else - ControlPanelVM.RTKState = "RTK未发送"; - - - - } - } - - public bool SmallRtcmData - { - get { return _smallrtcmdata; } - set - { - Set(nameof(SmallRtcmData), ref _smallrtcmdata, value); - } - } - - //接收速率 - private int bps; - //发送速率 - private int bpsusefull; - - private int _bps = 0; - public int Bps - { - get { return _bps; } - set { Set(nameof(Bps), ref _bps, value); } - } - - private int _bpsusefull = 0; - public int Bpsusefull - { - get { return _bpsusefull; } - set { Set(nameof(Bpsusefull), ref _bpsusefull, value); } - } - - private string _messages_seen = ""; - public string Messages_seen - { - get { return _messages_seen; } - set { Set(nameof(Messages_seen), ref _messages_seen, value); } - } - - - private DateTime baseTime = DateTime.Now; - private DateTime gpsTime = DateTime.Now; - private DateTime glonassTime = DateTime.Now; - private DateTime beidouTime = DateTime.Now; - - private bool _baseState = false; - public bool BaseState - { - get { return _baseState; } - set { Set(nameof(BaseState), ref _baseState, value); } - } - - private bool _gpsState = false; - public bool GpsState - { - get { return _gpsState; } - set { Set(nameof(GpsState), ref _gpsState, value); } - } - - private bool _glonassState = false; - public bool GlonassState - { - get { return _glonassState; } - set { Set(nameof(GlonassState), ref _glonassState, value); } - } - - private bool _beidouState = false; - public bool BeidouState - { - get { return _beidouState; } - set { Set(nameof(BeidouState), ref _beidouState, value); } - } - - private double _stationLat; - public double StationLat - { - get { return _stationLat; } - set { Set(nameof(StationLat), ref _stationLat, value); } - } - - private double _stationLng; - public double StationLng - { - get { return _stationLng; } - set { Set(nameof(StationLng), ref _stationLng, value); } - } - - private double _stationAlt; - public double StationAlt - { - get { return _stationAlt; } - set { Set(nameof(StationAlt), ref _stationAlt, value); } - } - - private string _stationTime; - public string StationTime - { - get { return _stationTime; } - set { Set(nameof(StationTime), ref _stationTime, value); } - } - - private DispatcherTimer dispatcherTimer = null; - public RtcmManager() - { - rtcm.ObsMessage += Rtcm_ObsMessage; - dispatcherTimer = new DispatcherTimer(); - dispatcherTimer.Tick += new EventHandler(OnTimedEvent); - dispatcherTimer.Interval = new TimeSpan(0, 0, 1); - dispatcherTimer.Start(); - } - - //用于刷新界面，显示消息状态，基站和卫星是否有效等---1HZ - private void OnTimedEvent(object sender, EventArgs e) - { - BaseState = baseTime > DateTime.Now; - GpsState = gpsTime > DateTime.Now; - GlonassState = glonassTime > DateTime.Now; - BeidouState = beidouTime > DateTime.Now; - - Bps = bps; - Bpsusefull = bpsusefull; - bps = 0; - bpsusefull = 0; - - StringBuilder sb = new StringBuilder(); - try - { - foreach (var item in msgseen.Keys) - { - sb.Append(item + "=" + msgseen[item] + " "); - } - } - catch - {} - Messages_seen = sb.ToString(); - } - - - //显示卫星信号强度条 - private void Rtcm_ObsMessage(object sender, EventArgs e) - { - List obs = sender as List; - if (obs.Count == 0) return; - - List partRtcmInfos; - partRtcmInfos = rtcmInfoList.Where(o => o.Sys == obs[0].sys).ToList(); - while (partRtcmInfos.Count() < obs.Count) - { - RtcmInfo newInfo = new RtcmInfo(); - rtcmInfoList.Add(newInfo); - partRtcmInfos.Add(newInfo); - - } - - while (partRtcmInfos.Count() > obs.Count) - { - RtcmInfo lastInfo = partRtcmInfos[partRtcmInfos.Count - 1]; - rtcmInfoList.Remove(lastInfo); - partRtcmInfos.Remove(lastInfo); - } - - if (partRtcmInfos.Count() == obs.Count) - { - for (int i = 0; i < obs.Count; i++) - { - partRtcmInfos[i].Sys = obs[i].sys; - partRtcmInfos[i].Prn = obs[i].prn; - partRtcmInfos[i].Snr = obs[i].snr; - } - } - - } - - public string[] GetPortNames() - { - return SerialPort.GetPortNames(); - } - - public async Task Open(string CMB_serialport, string reserialport="") - { - if (CMB_serialport == "") return; - FlightTaskManager _flightTaskManager = ServiceLocator.Current.GetInstance(); - - switch (CMB_serialport) - { - case "千寻": - //检测原点 - // FlightTaskManager _flightTaskManager = ServiceLocator.Current.GetInstance(); - if (_flightTaskManager.OriginLat == 0 && _flightTaskManager.OriginLng == 0) - { - Alert.Show("作为参照的原点未设置，无法发送RTCM！", "提示"); - return; - } - //ini里面的固定千寻账号 - //千寻账号 - string messagetips = "格式：http://差分账号:差分密码@域名:端口/坐标系\r\n例如：http://account:password@rtk.ntrip.qxwz.com:8002/RTCM32_GGB"; - string url = ""; - IniFiles inifilse = new IniFiles(); - url = inifilse.IniReadvalue("Default", "RTKurl"); - if (PlaneMessageBox.OnShow(messagetips, "发送RTK", ref url) == false) return; - inifilse.IniWritevalue("Default", "RTKurl", url); - - //构造 - comPort = new CommsNTRIP(url, _flightTaskManager.OriginLat, _flightTaskManager.OriginLng); - - break; - - case "魔方基站": - //检测原点 - if (_flightTaskManager.OriginLat == 0 && _flightTaskManager.OriginLng == 0) - { - Alert.Show("作为参照的原点未设置，无法发送RTCM！", "提示"); - return; - } - /* //ini里面的固定千寻账号 - //千寻账号 - string messagetips = "格式：http://差分账号:差分密码@域名:端口/坐标系\r\n例如：http://account:password@rtk.ntrip.qxwz.com:8002/RTCM32_GGB"; - string url = ""; - IniFiles inifilse = new IniFiles(); - url = inifilse.IniReadvalue("Default", "RTKurl"); - if (PlaneMessageBox.OnShow(messagetips, "发送RTK", ref url) == false) return; - inifilse.IniWritevalue("Default", "RTKurl", url); - */ - //通过网络得到千寻账号 - if (_copterManager.RTK_URL == "") - { - //得到千寻账号 - _copterManager.GetRTKURL(); - //等到返回5秒 - DateTime Tthen = DateTime.Now; - do - { - Application.DoEvents(); - } while ((_copterManager.RTK_URL == "") || (Tthen.AddSeconds(2) > DateTime.Now)); - } - if (_copterManager.RTK_URL != "") - { - //构造 - comPort = new CommsNTRIP(_copterManager.RTK_URL, _flightTaskManager.OriginLat, _flightTaskManager.OriginLng); - } - break; - default: - comPort = new SerialPort(); - comPort.PortName = CMB_serialport; - comPort.BaudRate = 57600; - break; - } - msgseen.Clear(); - try - { - comPort.Open(); - } - catch (Exception ex) - { - Alert.Show("数据端口打开失败:"+ ex.Message, "提示"); - } - - if (comPort.IsOpen) - { - Rtcmthreadrun = true; - _enrecom = false; - if (reserialport != "") - { - _enrecom=_commModuleManager.OpenResendRtcmserial(reserialport); - - } - await RtcmLoop(); - } - } - - public async Task Close(string CMB_serialport) - { - //如果需要关闭转发端口 - _commModuleManager.CloseResendRtcmserial(); - await _commModuleManager.CloseRtcmLoop(); - Rtcmthreadrun = false; - comPort.Close(); - switch (CMB_serialport) - { - case "魔方基站": - { - //释放千寻账号 - _copterManager.ReleaseRTKURL(); - - } - break; - } - - BaseState = false; - GpsState = false; - GlonassState = false; - BeidouState = false; - rtcmInfoList.Clear(); - _copterManager.RTK_URL = ""; - - - } - private static readonly DateTime Jan1st1970 = new DateTime(1970, 1, 1, 0, 0, 0, DateTimeKind.Utc); - - public static long CurrentTimeMillis() - { - return (long)(DateTime.UtcNow - Jan1st1970).TotalMilliseconds; - } - - - - //发送RTK数据线程循环 - private async Task RtcmLoop() - { - int reconnecttimeout = 10; - DateTime lastrecv = DateTime.MinValue; - long last_send_pos = CurrentTimeMillis()- 5000;//5秒发一次就可以了 - bool Ensend = false; - //新版本打包发送模式 - //await _commModuleManager.StartRtcmLoop(); - while (Rtcmthreadrun) - { - // Console.WriteLine(string.Format("{0:T} 1", DateTime.Now)); - - try - { - try - { - if ((DateTime.Now - lastrecv).TotalSeconds > reconnecttimeout || !comPort.IsOpen) - { - if (comPort is CommsNTRIP) - { - } - else - { - comPort.Close(); - comPort.Open(); - } - // reset timer - lastrecv = DateTime.Now; - } - } - catch { } - - if (!comPort.IsOpen) - { - return; - } - /////////////////////////////////重写发送RTCM数据部分---23/10/18 by panxu - /// - // limit to 180 byte packet if using new packet - byte[] buffer = new byte[180]; - while (comPort.BytesToRead > 0) - { - int read = comPort.Read(buffer, 0, Math.Min(buffer.Length, comPort.BytesToRead)); - Console.WriteLine(string.Format("{0:T} - read:{1:D}", DateTime.Now, read)); - if (read > 0) - lastrecv = DateTime.Now; - bps += read; //接收速率bps,byte需要*8到bit - // check for valid rtcm/sbp/ubx packets - for (int a = 0; a < read; a++) - { - int seenmsg = -1; - // rtcm and not can - if ((seenmsg = rtcm.Read(buffer[a])) > 0) - { - // Console.WriteLine(string.Format("{0:T}------------RTCM Send {1:D}", DateTime.Now, rtcm.length)); - ubx_m8p.resetParser(); - nmea.resetParser(); - string msgshowname = rtcm_typename(seenmsg); - //广播数据直接发送不受小带宽数据选项影响 - if (_enrecom) - { - Console.WriteLine(DateTime.Now.ToString("HH:mm:ss") + "--广播Rtcm长度: " + (ushort)rtcm.length + " 类型: " + msgshowname + " (" + seenmsg + ")"); - _commModuleManager.BroadcastGpsDataAsync(rtcm.packet, (ushort)rtcm.length); - } - - //直接发送 - if (!SmallRtcmData) - { - //发送到飞机 - Console.WriteLine(DateTime.Now.ToString("HH:mm:ss") + "--通讯模块发送Rtcm长度: " + (ushort)rtcm.length + " 类型: " + msgshowname + " (" + seenmsg + ")"); - await _commModuleManager.InjectGpsDataAsync(rtcm.packet, (ushort)rtcm.length); - //累加消息数量，用于界面显示 - bpsusefull += rtcm.length; - } - else - //为了减少发送数据量： - //1：1秒只发一种卫星，1秒发GPS,第2秒发北斗，第3秒发格洛纳斯，其他数据随来随发 - - { - - Ensend = false; //是否发送 - //第1秒是gps,第2秒是GLONASS，第3秒是 Beidou - //1秒内可能发送多个同一种类型的数据，看数据大小和原始频率 - long curr_s = CurrentTimeMillis() / 1000; - int sendtype = (int)(curr_s % 3); - switch (seenmsg) - { - case MSG_GPS: - if (sendtype == 0) //第1秒是gps, - Ensend = true; - break; - - case MSG_GLONASS: - case MSG_GLONASS_PHASE: - if (sendtype == 1) //第2秒是GLONASS， - Ensend = true; - break; - case MSG_Beidou: - if (sendtype == 2) //第3秒是 Beidou - Ensend = true; - break; - //位置4秒发一次就可以了 - case MSG_BasePos: - { - long curr_send_pos = CurrentTimeMillis(); - if ((curr_send_pos - last_send_pos) > 4000) - { - last_send_pos = curr_send_pos; - Ensend = true; - } - } - break; - case MSG_REV: //这个没必要发 - Ensend = false; - break; - default: //其他类型数据随到随发 - Ensend = true; - break; - } - if (Ensend) - { - Console.WriteLine(DateTime.Now.ToString("HH:mm:ss") + "--通讯模块发送Rtcm长度: " + (ushort)rtcm.length + " 类型: " + msgshowname + " (" + seenmsg + ")"); - await _commModuleManager.InjectGpsDataAsync(rtcm.packet, (ushort)rtcm.length); - //累加消息数量，用于界面显示 - bpsusefull += rtcm.length; - } - - - } - - - - - string msgname = rtcm_typename(seenmsg); - if (!msgseen.ContainsKey(msgname)) - msgseen[msgname] = 0; - msgseen[msgname] = (int)msgseen[msgname] + 1; - //检测基站定位状态和卫星定位状态，用于在界面上显示 - await ExtractBasePos(seenmsg); - } - - // ubx - if ((seenmsg = ubx_m8p.Read(buffer[a])) > 0) - { - Console.WriteLine(string.Format("{0:T}----------------------------ubx_m8p ", DateTime.Now)); - rtcm.resetParser(); - nmea.resetParser(); - /* - //Ubx协议数据不用显示 - string msgname = "Ubx" + seenmsg.ToString("X4"); - if (!msgseen.ContainsKey(msgname)) - msgseen[msgname] = 0; - msgseen[msgname] = (int)msgseen[msgname] + 1; - */ - } - // nmea - if ((seenmsg = nmea.Read(buffer[a])) > 0) - { - Console.WriteLine(string.Format("{0:T}----------------------------nmea ", DateTime.Now)); - rtcm.resetParser(); - ubx_m8p.resetParser(); - /* - //NMEA协议数据不用显示 - string msgname = "NMEA"; - if (!msgseen.ContainsKey(msgname)) - msgseen[msgname] = 0; - msgseen[msgname] = (int)msgseen[msgname] + 1; - */ - } - - } - ////////////////////////////解析/发送RTCM结束 - await Task.Delay(10); - } - } - catch (Exception ex) - { - Plane.Windows.Messages.Message.Show(ex.Message); - } - } - } - - - private async Task ExtractBasePos(int seen) - { - try - { - if (seen == 1005) - { - var basepos = new rtcm3.type1005(); - basepos.Read(rtcm.packet); - var pos = basepos.ecefposition; - double[] baseposllh = new double[3]; - rtcm3.ecef2pos(pos, ref baseposllh); - - - StationLat = baseposllh[0] * rtcm3.R2D; - StationLng = baseposllh[1] * rtcm3.R2D; - StationAlt = baseposllh[2]; - StationTime = DateTime.Now.ToString("HH:mm:ss"); - - } - else if (seen == 1006) - { - var basepos = new rtcm3.type1006(); - basepos.Read(rtcm.packet); - var pos = basepos.ecefposition; - double[] baseposllh = new double[3]; - rtcm3.ecef2pos(pos, ref baseposllh); - - StationLat = baseposllh[0] * rtcm3.R2D; - StationLng = baseposllh[1] * rtcm3.R2D; - StationAlt = baseposllh[2]; - StationTime = DateTime.Now.ToString("HH:mm:ss"); - } - - switch (seen) - { - case 1001: - case 1002: - case 1003: - case 1004: - case 1071: - case 1072: - case 1073: - case 1074: - case 1075: - case 1076: - case 1077: - gpsTime = DateTime.Now.AddSeconds(5); - break; - case 1005: - case 1006: - case 4072: // ublox moving base - baseTime = DateTime.Now.AddSeconds(20); - break; - case 1009: - case 1010: - case 1011: - case 1012: - case 1081: - case 1082: - case 1083: - case 1084: - case 1085: - case 1086: - case 1087: - glonassTime = DateTime.Now.AddSeconds(5); - break; - case 1121: - case 1122: - case 1123: - case 1124: - case 1125: - case 1126: - case 1127: - beidouTime = DateTime.Now.AddSeconds(5); - break; - default: - break; - } - } - catch (Exception ex) - { - Plane.Windows.Messages.Message.Show(ex.Message); - } - await Task.Delay(1); - - } - } - - public class RtcmInfo: ObservableObject - { - private char sys; - private byte prn; - private byte snr; - - public RtcmInfo() - { - - } - - public char Sys - { - get { return sys; } - set { Set(nameof(Sys), ref sys, value); } - } - - public byte Prn - { - get { return prn; } - set { Set(nameof(Prn), ref prn, value); } - } - - public byte Snr - { - get { return snr; } - set { Set(nameof(Snr), ref snr, value); } - } - } -} +using GalaSoft.MvvmLight; +using Microsoft.Practices.ServiceLocation; +using Plane.CommunicationManagement; +using Plane.FormationCreator.ViewModels; +using Plane.Util; +using Plane.Windows.IniHelper; +using Plane.Windows.Messages; +using System; +using System.Collections; +using System.Collections.Generic; +using System.Collections.ObjectModel; +using System.Linq; +using System.Text; +using System.Threading; +using System.Threading.Tasks; +using System.Windows.Forms; +using System.Windows.Media; +using System.Windows.Threading; + +namespace Plane.FormationCreator.Formation +{ + public class RtcmManager: ObservableObject + { + private rtcm3 rtcm = new rtcm3(); + // ubx detection + private static Ubx ubx_m8p = new Ubx(); + static nmea nmea = new nmea(); + + private Hashtable msgseen = new Hashtable(); + + private ICommsSerial comPort; + private ICommsSerial RecomPort; + private CommModuleManager _commModuleManager = CommModuleManager.Instance; + public ObservableCollection rtcmInfoList { get; } = new ObservableCollection(); + private ControlPanelViewModel ControlPanelVM = ServiceLocator.Current.GetInstance(); + private CopterManager _copterManager = ServiceLocator.Current.GetInstance(); + private bool _rtcmthreadrun = false; + private bool _smallrtcmdata = false; //减少传输数据--用于带宽不够的通讯模块-对数传广播无效 + private bool _enrecom = false;//是否转发到另外串口 + + // rtcm发送类型0:直接发送， + // 1：1秒只发一种卫星，1秒发GPS,第2秒发北斗，第3秒发格洛纳斯，其他数据随来随发 + + private int _resendtocom = 1; + const int MSG_GPS = 1074; + const int MSG_GLONASS = 1084; + const int MSG_Beidou = 1124; + const int MSG_BasePos = 1005; + const int MSG_REV = 1033; + const int MSG_GLONASS_PHASE = 1230; + + + public string rtcm_typename(int msgtype) + { + string typename = "Rtcm" + msgtype; + switch (msgtype) + { + case MSG_GPS: + typename = "GPS"; + break; + case MSG_GLONASS: + typename = "GLONASS"; + break; + case MSG_GLONASS_PHASE: + typename = "GLONASS相位差"; + break; + case MSG_Beidou: + typename = "北斗"; + break; + //位置5秒发一次就可以了 + case MSG_BasePos: + typename = "基站位置"; + break; + case MSG_REV: //这个没必要发 + typename = "版本"; + break; + } + return typename; + + } + public bool Rtcmthreadrun + { + get { return _rtcmthreadrun; } + set { + Set(nameof(Rtcmthreadrun), ref _rtcmthreadrun, value); + + if (value) + ControlPanelVM.RTKState = "RTK发送中..."; + else + ControlPanelVM.RTKState = "RTK未发送"; + + + + } + } + + public bool SmallRtcmData + { + get { return _smallrtcmdata; } + set + { + Set(nameof(SmallRtcmData), ref _smallrtcmdata, value); + } + } + + //接收速率 + private int bps; + //发送速率 + private int bpsusefull; + + private int _bps = 0; + public int Bps + { + get { return _bps; } + set { Set(nameof(Bps), ref _bps, value); } + } + + private int _bpsusefull = 0; + public int Bpsusefull + { + get { return _bpsusefull; } + set { Set(nameof(Bpsusefull), ref _bpsusefull, value); } + } + + private string _messages_seen = ""; + public string Messages_seen + { + get { return _messages_seen; } + set { Set(nameof(Messages_seen), ref _messages_seen, value); } + } + + + private DateTime baseTime = DateTime.Now; + private DateTime gpsTime = DateTime.Now; + private DateTime glonassTime = DateTime.Now; + private DateTime beidouTime = DateTime.Now; + + private bool _baseState = false; + public bool BaseState + { + get { return _baseState; } + set { Set(nameof(BaseState), ref _baseState, value); } + } + + private bool _gpsState = false; + public bool GpsState + { + get { return _gpsState; } + set { Set(nameof(GpsState), ref _gpsState, value); } + } + + private bool _glonassState = false; + public bool GlonassState + { + get { return _glonassState; } + set { Set(nameof(GlonassState), ref _glonassState, value); } + } + + private bool _beidouState = false; + public bool BeidouState + { + get { return _beidouState; } + set { Set(nameof(BeidouState), ref _beidouState, value); } + } + + private double _stationLat; + public double StationLat + { + get { return _stationLat; } + set { Set(nameof(StationLat), ref _stationLat, value); } + } + + private double _stationLng; + public double StationLng + { + get { return _stationLng; } + set { Set(nameof(StationLng), ref _stationLng, value); } + } + + private double _stationAlt; + public double StationAlt + { + get { return _stationAlt; } + set { Set(nameof(StationAlt), ref _stationAlt, value); } + } + + private string _stationTime; + public string StationTime + { + get { return _stationTime; } + set { Set(nameof(StationTime), ref _stationTime, value); } + } + + private DispatcherTimer dispatcherTimer = null; + public RtcmManager() + { + rtcm.ObsMessage += Rtcm_ObsMessage; + dispatcherTimer = new DispatcherTimer(); + dispatcherTimer.Tick += new EventHandler(OnTimedEvent); + dispatcherTimer.Interval = new TimeSpan(0, 0, 1); + dispatcherTimer.Start(); + } + + //用于刷新界面，显示消息状态，基站和卫星是否有效等---1HZ + private void OnTimedEvent(object sender, EventArgs e) + { + BaseState = baseTime > DateTime.Now; + GpsState = gpsTime > DateTime.Now; + GlonassState = glonassTime > DateTime.Now; + BeidouState = beidouTime > DateTime.Now; + + Bps = bps; + Bpsusefull = bpsusefull; + bps = 0; + bpsusefull = 0; + + StringBuilder sb = new StringBuilder(); + try + { + foreach (var item in msgseen.Keys) + { + sb.Append(item + "=" + msgseen[item] + " "); + } + } + catch + {} + Messages_seen = sb.ToString(); + } + + + //显示卫星信号强度条 + private void Rtcm_ObsMessage(object sender, EventArgs e) + { + List obs = sender as List; + if (obs.Count == 0) return; + + List partRtcmInfos; + partRtcmInfos = rtcmInfoList.Where(o => o.Sys == obs[0].sys).ToList(); + while (partRtcmInfos.Count() < obs.Count) + { + RtcmInfo newInfo = new RtcmInfo(); + rtcmInfoList.Add(newInfo); + partRtcmInfos.Add(newInfo); + + } + + while (partRtcmInfos.Count() > obs.Count) + { + RtcmInfo lastInfo = partRtcmInfos[partRtcmInfos.Count - 1]; + rtcmInfoList.Remove(lastInfo); + partRtcmInfos.Remove(lastInfo); + } + + if (partRtcmInfos.Count() == obs.Count) + { + for (int i = 0; i < obs.Count; i++) + { + partRtcmInfos[i].Sys = obs[i].sys; + partRtcmInfos[i].Prn = obs[i].prn; + partRtcmInfos[i].Snr = obs[i].snr; + } + } + + } + + public string[] GetPortNames() + { + return SerialPort.GetPortNames(); + } + + public async Task Open(string CMB_serialport, string reserialport="") + { + if (CMB_serialport == "") return; + FlightTaskManager _flightTaskManager = ServiceLocator.Current.GetInstance(); + + switch (CMB_serialport) + { + case "千寻": + //检测原点 + // FlightTaskManager _flightTaskManager = ServiceLocator.Current.GetInstance(); + if (_flightTaskManager.OriginLat == 0 && _flightTaskManager.OriginLng == 0) + { + Alert.Show("作为参照的原点未设置，无法发送RTCM！", "提示"); + return; + } + //ini里面的固定千寻账号 + //千寻账号 + string messagetips = "格式：http://差分账号:差分密码@域名:端口/坐标系\r\n例如：http://account:password@rtk.ntrip.qxwz.com:8002/RTCM32_GGB"; + string url = ""; + IniFiles inifilse = new IniFiles(); + url = inifilse.IniReadvalue("Default", "RTKurl"); + if (PlaneMessageBox.OnShow(messagetips, "发送RTK", ref url) == false) return; + inifilse.IniWritevalue("Default", "RTKurl", url); + + //构造 + comPort = new CommsNTRIP(url, _flightTaskManager.OriginLat, _flightTaskManager.OriginLng); + + break; + + case "魔方基站": + //检测原点 + if (_flightTaskManager.OriginLat == 0 && _flightTaskManager.OriginLng == 0) + { + Alert.Show("作为参照的原点未设置，无法发送RTCM！", "提示"); + return; + } + /* //ini里面的固定千寻账号 + //千寻账号 + string messagetips = "格式：http://差分账号:差分密码@域名:端口/坐标系\r\n例如：http://account:password@rtk.ntrip.qxwz.com:8002/RTCM32_GGB"; + string url = ""; + IniFiles inifilse = new IniFiles(); + url = inifilse.IniReadvalue("Default", "RTKurl"); + if (PlaneMessageBox.OnShow(messagetips, "发送RTK", ref url) == false) return; + inifilse.IniWritevalue("Default", "RTKurl", url); + */ + //通过网络得到千寻账号 + if (_copterManager.RTK_URL == "") + { + //得到千寻账号 + _copterManager.GetRTKURL(); + //等到返回5秒 + DateTime Tthen = DateTime.UtcNow; + do + { + Application.DoEvents(); + } while ((_copterManager.RTK_URL == "") || (Tthen.AddSeconds(2) > DateTime.UtcNow)); + } + if (_copterManager.RTK_URL != "") + { + //构造 + comPort = new CommsNTRIP(_copterManager.RTK_URL, _flightTaskManager.OriginLat, _flightTaskManager.OriginLng); + } + break; + default: + comPort = new SerialPort(); + comPort.PortName = CMB_serialport; + comPort.BaudRate = 57600; + break; + } + msgseen.Clear(); + try + { + comPort.Open(); + } + catch (Exception ex) + { + Alert.Show("数据端口打开失败:"+ ex.Message, "提示"); + } + + if (comPort.IsOpen) + { + Rtcmthreadrun = true; + _enrecom = false; + if (reserialport != "") + { + _enrecom=_commModuleManager.OpenResendRtcmserial(reserialport); + + } + await RtcmLoop(); + } + } + + public async Task Close(string CMB_serialport) + { + //如果需要关闭转发端口 + _commModuleManager.CloseResendRtcmserial(); + await _commModuleManager.CloseRtcmLoop(); + Rtcmthreadrun = false; + comPort.Close(); + switch (CMB_serialport) + { + case "魔方基站": + { + //释放千寻账号 + _copterManager.ReleaseRTKURL(); + + } + break; + } + + BaseState = false; + GpsState = false; + GlonassState = false; + BeidouState = false; + rtcmInfoList.Clear(); + _copterManager.RTK_URL = ""; + + + } + private static readonly DateTime Jan1st1970 = new DateTime(1970, 1, 1, 0, 0, 0, DateTimeKind.Utc); + + public static long CurrentTimeMillis() + { + return (long)(DateTime.UtcNow - Jan1st1970).TotalMilliseconds; + } + + + + //发送RTK数据线程循环 + private async Task RtcmLoop() + { + int reconnecttimeout = 10; + DateTime lastrecv = DateTime.MinValue; + DateTime unow = DateTime.UtcNow; + long last_send_pos = CurrentTimeMillis()- 5000;//5秒发一次就可以了 + bool Ensend = false; + //新版本打包发送模式 + //await _commModuleManager.StartRtcmLoop(); + while (Rtcmthreadrun) + { + // Console.WriteLine(string.Format("{0:T} 1", DateTime.Now)); + + try + { + try + { + unow=DateTime.UtcNow; + if ((unow - lastrecv).TotalSeconds > reconnecttimeout || !comPort.IsOpen) + { + if (comPort is CommsNTRIP) + { + } + else + { + comPort.Close(); + comPort.Open(); + } + // reset timer + lastrecv = unow; + } + } + catch { } + + if (!comPort.IsOpen) + { + return; + } + /////////////////////////////////重写发送RTCM数据部分---23/10/18 by panxu + /// + // limit to 180 byte packet if using new packet + byte[] buffer = new byte[180]; + while (comPort.BytesToRead > 0) + { + int read = comPort.Read(buffer, 0, Math.Min(buffer.Length, comPort.BytesToRead)); + //Console.WriteLine(string.Format("{0:T} - read:{1:D}", DateTime.UtcNow, read)); + if (read > 0) + lastrecv = DateTime.UtcNow; + bps += read; //接收速率bps,byte需要*8到bit + // check for valid rtcm/sbp/ubx packets + for (int a = 0; a < read; a++) + { + int seenmsg = -1; + // rtcm and not can + if ((seenmsg = rtcm.Read(buffer[a])) > 0) + { + // Console.WriteLine(string.Format("{0:T}------------RTCM Send {1:D}", DateTime.Now, rtcm.length)); + ubx_m8p.resetParser(); + nmea.resetParser(); + string msgshowname = rtcm_typename(seenmsg); + //广播数据直接发送不受小带宽数据选项影响 + if (_enrecom) + { + // Console.WriteLine(DateTime.UtcNow.ToString("HH:mm:ss") + "--广播Rtcm长度: " + (ushort)rtcm.length + " 类型: " + msgshowname + " (" + seenmsg + ")"); + _commModuleManager.BroadcastGpsDataAsync(rtcm.packet, (ushort)rtcm.length); + } + + //直接发送 + if (!SmallRtcmData) + { + //发送到飞机 + // Console.WriteLine(DateTime.UtcNow.ToString("HH:mm:ss") + "--通讯模块发送Rtcm长度: " + (ushort)rtcm.length + " 类型: " + msgshowname + " (" + seenmsg + ")"); + await _commModuleManager.InjectGpsDataAsync(rtcm.packet, (ushort)rtcm.length); + //累加消息数量，用于界面显示 + bpsusefull += rtcm.length; + } + else + //为了减少发送数据量： + //1：1秒只发一种卫星，1秒发GPS,第2秒发北斗，第3秒发格洛纳斯，其他数据随来随发 + + { + + Ensend = false; //是否发送 + //第1秒是gps,第2秒是GLONASS，第3秒是 Beidou + //1秒内可能发送多个同一种类型的数据，看数据大小和原始频率 + long curr_s = CurrentTimeMillis() / 1000; + int sendtype = (int)(curr_s % 3); + switch (seenmsg) + { + case MSG_GPS: + if (sendtype == 0) //第1秒是gps, + Ensend = true; + break; + + case MSG_GLONASS: + case MSG_GLONASS_PHASE: + if (sendtype == 1) //第2秒是GLONASS， + Ensend = true; + break; + case MSG_Beidou: + if (sendtype == 2) //第3秒是 Beidou + Ensend = true; + break; + //位置4秒发一次就可以了 + case MSG_BasePos: + { + long curr_send_pos = CurrentTimeMillis(); + if ((curr_send_pos - last_send_pos) > 4000) + { + last_send_pos = curr_send_pos; + Ensend = true; + } + } + break; + case MSG_REV: //这个没必要发 + Ensend = false; + break; + default: //其他类型数据随到随发 + Ensend = true; + break; + } + if (Ensend) + { + //Console.WriteLine(DateTime.UtcNow.ToString("HH:mm:ss") + "--通讯模块发送Rtcm长度: " + (ushort)rtcm.length + " 类型: " + msgshowname + " (" + seenmsg + ")"); + await _commModuleManager.InjectGpsDataAsync(rtcm.packet, (ushort)rtcm.length); + //累加消息数量，用于界面显示 + bpsusefull += rtcm.length; + } + + + } + + + + + string msgname = rtcm_typename(seenmsg); + if (!msgseen.ContainsKey(msgname)) + msgseen[msgname] = 0; + msgseen[msgname] = (int)msgseen[msgname] + 1; + //检测基站定位状态和卫星定位状态，用于在界面上显示 + await ExtractBasePos(seenmsg); + } + + // ubx + if ((seenmsg = ubx_m8p.Read(buffer[a])) > 0) + { + //Console.WriteLine(string.Format("{0:T}----------------------------ubx_m8p ", DateTime.UtcNow)); + rtcm.resetParser(); + nmea.resetParser(); + /* + //Ubx协议数据不用显示 + string msgname = "Ubx" + seenmsg.ToString("X4"); + if (!msgseen.ContainsKey(msgname)) + msgseen[msgname] = 0; + msgseen[msgname] = (int)msgseen[msgname] + 1; + */ + } + // nmea + if ((seenmsg = nmea.Read(buffer[a])) > 0) + { + //Console.WriteLine(string.Format("{0:T}----------------------------nmea ", DateTime.UtcNow)); + rtcm.resetParser(); + ubx_m8p.resetParser(); + /* + //NMEA协议数据不用显示 + string msgname = "NMEA"; + if (!msgseen.ContainsKey(msgname)) + msgseen[msgname] = 0; + msgseen[msgname] = (int)msgseen[msgname] + 1; + */ + } + + } + ////////////////////////////解析/发送RTCM结束 + await Task.Delay(20); + } + await Task.Delay(20); //新增加关键延迟，要不然界面会出现卡死现象 + } + catch (Exception ex) + { + Plane.Windows.Messages.Message.Show(ex.Message); + } + } + } + + + private async Task ExtractBasePos(int seen) + { + try + { + if (seen == 1005) + { + var basepos = new rtcm3.type1005(); + basepos.Read(rtcm.packet); + var pos = basepos.ecefposition; + double[] baseposllh = new double[3]; + rtcm3.ecef2pos(pos, ref baseposllh); + + + StationLat = baseposllh[0] * rtcm3.R2D; + StationLng = baseposllh[1] * rtcm3.R2D; + StationAlt = baseposllh[2]; + StationTime = DateTime.Now.ToString("HH:mm:ss"); + + } + else if (seen == 1006) + { + var basepos = new rtcm3.type1006(); + basepos.Read(rtcm.packet); + var pos = basepos.ecefposition; + double[] baseposllh = new double[3]; + rtcm3.ecef2pos(pos, ref baseposllh); + + StationLat = baseposllh[0] * rtcm3.R2D; + StationLng = baseposllh[1] * rtcm3.R2D; + StationAlt = baseposllh[2]; + StationTime = DateTime.Now.ToString("HH:mm:ss"); + } + + switch (seen) + { + case 1001: + case 1002: + case 1003: + case 1004: + case 1071: + case 1072: + case 1073: + case 1074: + case 1075: + case 1076: + case 1077: + gpsTime = DateTime.Now.AddSeconds(5); + break; + case 1005: + case 1006: + case 4072: // ublox moving base + baseTime = DateTime.Now.AddSeconds(20); + break; + case 1009: + case 1010: + case 1011: + case 1012: + case 1081: + case 1082: + case 1083: + case 1084: + case 1085: + case 1086: + case 1087: + glonassTime = DateTime.Now.AddSeconds(5); + break; + case 1121: + case 1122: + case 1123: + case 1124: + case 1125: + case 1126: + case 1127: + beidouTime = DateTime.Now.AddSeconds(5); + break; + default: + break; + } + } + catch (Exception ex) + { + Plane.Windows.Messages.Message.Show(ex.Message); + } + await Task.Delay(1); + + } + } + + public class RtcmInfo: ObservableObject + { + private char sys; + private byte prn; + private byte snr; + + public RtcmInfo() + { + + } + + public char Sys + { + get { return sys; } + set { Set(nameof(Sys), ref sys, value); } + } + + public byte Prn + { + get { return prn; } + set { Set(nameof(Prn), ref prn, value); } + } + + public byte Snr + { + get { return snr; } + set { Set(nameof(Snr), ref snr, value); } + } + } +} diff --git a/Plane.FormationCreator/Util/CommNTRIP.cs b/Plane.FormationCreator/Util/CommNTRIP.cs index d54efc1..8436d26 100644 --- a/Plane.FormationCreator/Util/CommNTRIP.cs +++ b/Plane.FormationCreator/Util/CommNTRIP.cs @@ -1,431 +1,430 @@ -using System; -using System.Collections.Generic; -using System.ComponentModel; -using System.Reflection; -using System.Text; -using System.IO.Ports; -using System.Threading; -using System.Net; // dns, ip address -using System.Net.Sockets; // tcplistner -using System.IO; -using System.Runtime.InteropServices; -using System.Text.RegularExpressions; - -namespace Plane.Util -{ - public class CommsNTRIP : ICommsSerial, IDisposable - { - public TcpClient client = new TcpClient(); - IPEndPoint RemoteIpEndPoint = new IPEndPoint(IPAddress.Any, 0); - private Uri remoteUri; - - public double lat = 0; - public double lng = 0; - public double alt = 0; - - int retrys = 3; - - private string host; - - public int WriteBufferSize { get; set; } - public int WriteTimeout { get; set; } - public bool RtsEnable { get; set; } - private string url; - - public Stream BaseStream - { - get { return client.GetStream(); } - } - - public CommsNTRIP(string url, double lat, double lng) - { - this.url = url; - this.lat = lat; - this.lng = lng; - ReadTimeout = 500; - } - - public void toggleDTR() - { - } - - public string Port { get; set; } - - public int ReadTimeout - { - get; // { return client.ReceiveTimeout; } - set; // { client.ReceiveTimeout = value; } - } - - public int ReadBufferSize { get; set; } - - public int BaudRate { get; set; } - public StopBits StopBits { get; set; } - public Parity Parity { get; set; } - public int DataBits { get; set; } - - public string PortName { get; set; } - - public int BytesToRead - { - get - { - /*Console.WriteLine(DateTime.Now.Millisecond + " tcp btr " + (client.Available + rbuffer.Length - rbufferread));*/ - SendNMEA(); - return (int)client.Available; - } - } - - public int BytesToWrite - { - get { return 0; } - } - - public bool IsOpen - { - get - { - try - { - return client.Client.Connected; - } - catch - { - return false; - } - } - } - - public bool DtrEnable { get; set; } - - public void Open() - { - if (client.Client.Connected) - { - return; - } - - - /*string url = "";*/ - - - int count = url.Split('@').Length - 1; - - if (count > 1) - { - var regex = new Regex("@"); - url = regex.Replace(url, "%40", 1); - } - - url = url.Replace("ntrip://", "http://"); - - remoteUri = new Uri(url); - - doConnect(); - } - - private byte[] TcpKeepAlive(bool On_Off, uint KeepaLiveTime, uint KeepaLiveInterval) - { - byte[] InValue = new byte[12]; - - Array.ConstrainedCopy(BitConverter.GetBytes(Convert.ToUInt32(On_Off)), 0, InValue, 0, 4); - Array.ConstrainedCopy(BitConverter.GetBytes(KeepaLiveTime), 0, InValue, 4, 4); - Array.ConstrainedCopy(BitConverter.GetBytes(KeepaLiveInterval), 0, InValue, 8, 4); - - return InValue; - } - - private void doConnect() - { - string usernamePassword = remoteUri.UserInfo; - string userpass2 = Uri.UnescapeDataString(usernamePassword); - string auth = "Authorization: Basic " + - Convert.ToBase64String(new ASCIIEncoding().GetBytes(userpass2)) + "\r\n"; - - if (usernamePassword == "") - auth = ""; - - host = remoteUri.Host; - Port = remoteUri.Port.ToString(); - - client = new TcpClient(host, int.Parse(Port)); - client.Client.IOControl(IOControlCode.KeepAliveValues, TcpKeepAlive(true, 36000000, 3000), null); - - NetworkStream ns = client.GetStream(); - - StreamWriter sw = new StreamWriter(ns); - StreamReader sr = new StreamReader(ns); - - string line = "GET " + remoteUri.PathAndQuery + " HTTP/1.0\r\n" - + "User-Agent: NTRIP MissionPlanner/1.0\r\n" - + auth - + "Connection: close\r\n\r\n"; - - sw.Write(line); - - sw.Flush(); - - line = sr.ReadLine(); - - if (!line.Contains("200")) - { - client.Dispose(); - - client = new TcpClient(); - - throw new Exception("Bad ntrip Responce\n\n" + line); - } - - // vrs may take up to 60+ seconds to respond - SendNMEA(); - - VerifyConnected(); - } - - DateTime _lastnmea = DateTime.MinValue; - - private void SendNMEA() - { - if (lat != 0 || lng != 0) - { - if (_lastnmea.AddSeconds(30) < DateTime.Now) - { - double latdms = (int)lat + ((lat - (int)lat) * .6f); - double lngdms = (int)lng + ((lng - (int)lng) * .6f); - - var line = string.Format(System.Globalization.CultureInfo.InvariantCulture, - "$GP{0},{1:HHmmss.ff},{2},{3},{4},{5},{6},{7},{8},{9},{10},{11},{12},{13},{14}", "GGA", - DateTime.Now.ToUniversalTime(), Math.Abs(latdms * 100).ToString("0000.00"), lat < 0 ? "S" : "N", - Math.Abs(lngdms * 100).ToString("00000.00"), lng < 0 ? "W" : "E", 1, 10, - 1, alt.ToString("0.00"), "M", 0, "M", "0.0", "0"); - - string checksum = GetChecksum(line); - WriteLine(line + "*" + checksum); - - - _lastnmea = DateTime.Now; - } - } - - } - - // Calculates the checksum for a sentence - string GetChecksum(string sentence) - { - // Loop through all chars to get a checksum - int Checksum = 0; - foreach (char Character in sentence.ToCharArray()) - { - switch (Character) - { - case '$': - // Ignore the dollar sign - break; - case '*': - // Stop processing before the asterisk - continue; - default: - // Is this the first value for the checksum? - if (Checksum == 0) - { - // Yes. Set the checksum to the value - Checksum = Convert.ToByte(Character); - } - else - { - // No. XOR the checksum with this character's value - Checksum = Checksum ^ Convert.ToByte(Character); - } - break; - } - } - // Return the checksum formatted as a two-character hexadecimal - return Checksum.ToString("X2"); - } - - void VerifyConnected() - { - if (!IsOpen) - { - try - { - client.Dispose(); - client = new TcpClient(); - } - catch { } - - // this should only happen if we have established a connection in the first place - if (client != null && retrys > 0) - { - doConnect(); - retrys--; - } - - throw new Exception("网络RTK基站通讯已关闭!"); - } - } - - public int Read(byte[] readto, int offset, int length) - { - VerifyConnected(); - - SendNMEA(); - - try - { - if (length < 1) { return 0; } - - return client.Client.Receive(readto, offset, length, SocketFlags.Partial); - /* - byte[] temp = new byte[length]; - clientbuf.Read(temp, 0, length); - - temp.CopyTo(readto, offset); - - return length;*/ - } - catch { throw new Exception("ntrip Socket Closed"); } - } - - public int ReadByte() - { - VerifyConnected(); - int count = 0; - while (this.BytesToRead == 0) - { - System.Threading.Thread.Sleep(1); - if (count > ReadTimeout) - throw new Exception("ntrip Timeout on read"); - count++; - } - byte[] buffer = new byte[1]; - Read(buffer, 0, 1); - return buffer[0]; - } - - public int ReadChar() - { - return ReadByte(); - } - - public string ReadExisting() - { - VerifyConnected(); - byte[] data = new byte[client.Available]; - if (data.Length > 0) - Read(data, 0, data.Length); - - string line = Encoding.ASCII.GetString(data, 0, data.Length); - - return line; - } - - public void WriteLine(string line) - { - VerifyConnected(); - line = line + "\r\n"; - Write(line); - } - - public void Write(string line) - { - VerifyConnected(); - byte[] data = new System.Text.ASCIIEncoding().GetBytes(line); - Write(data, 0, data.Length); - } - - public void Write(byte[] write, int offset, int length) - { - VerifyConnected(); - try - { - client.Client.Send(write, length, SocketFlags.None); - } - catch { }//throw new Exception("Comport / Socket Closed"); } - } - - public void DiscardInBuffer() - { - VerifyConnected(); - int size = (int)client.Available; - byte[] crap = new byte[size]; - Read(crap, 0, size); - } - - public string ReadLine() - { - byte[] temp = new byte[4000]; - int count = 0; - int timeout = 0; - - while (timeout <= 100) - { - if (!this.IsOpen) { break; } - if (this.BytesToRead > 0) - { - byte letter = (byte)this.ReadByte(); - - temp[count] = letter; - - if (letter == '\n') // normal line - { - break; - } - - - count++; - if (count == temp.Length) - break; - timeout = 0; - } - else - { - timeout++; - System.Threading.Thread.Sleep(5); - } - } - - Array.Resize(ref temp, count + 1); - - return Encoding.ASCII.GetString(temp, 0, temp.Length); - } - - public void Close() - { - try - { - if (client.Client != null && client.Client.Connected) - { - client.Client.Dispose(); - client.Dispose(); - } - } - catch { } - - try - { - client.Dispose(); - } - catch { } - - client = new TcpClient(); - } - - protected virtual void Dispose(bool disposing) - { - if (disposing) - { - // dispose managed resources - this.Close(); - client = null; - } - // free native resources - } - - public void Dispose() - { - Dispose(true); - GC.SuppressFinalize(this); - } - } -} +using System; +using System.Collections.Generic; +using System.ComponentModel; +using System.Reflection; +using System.Text; +using System.IO.Ports; +using System.Threading; +using System.Net; // dns, ip address +using System.Net.Sockets; // tcplistner +using System.IO; +using System.Runtime.InteropServices; +using System.Text.RegularExpressions; + +namespace Plane.Util +{ + public class CommsNTRIP : ICommsSerial, IDisposable + { + public TcpClient client = new TcpClient(); + IPEndPoint RemoteIpEndPoint = new IPEndPoint(IPAddress.Any, 0); + private Uri remoteUri; + + public double lat = 0; + public double lng = 0; + public double alt = 0; + + int retrys = 3; + + private string host; + + public int WriteBufferSize { get; set; } + public int WriteTimeout { get; set; } + public bool RtsEnable { get; set; } + private string url; + + public Stream BaseStream + { + get { return client.GetStream(); } + } + + public CommsNTRIP(string url, double lat, double lng) + { + this.url = url; + this.lat = lat; + this.lng = lng; + ReadTimeout = 500; + } + + public void toggleDTR() + { + } + + public string Port { get; set; } + + public int ReadTimeout + { + get; // { return client.ReceiveTimeout; } + set; // { client.ReceiveTimeout = value; } + } + + public int ReadBufferSize { get; set; } + + public int BaudRate { get; set; } + public StopBits StopBits { get; set; } + public Parity Parity { get; set; } + public int DataBits { get; set; } + + public string PortName { get; set; } + + public int BytesToRead + { + get + { + /*Console.WriteLine(DateTime.Now.Millisecond + " tcp btr " + (client.Available + rbuffer.Length - rbufferread));*/ + SendNMEA(); + return (int)client.Available; + } + } + + public int BytesToWrite + { + get { return 0; } + } + + public bool IsOpen + { + get + { + try + { + return client.Client.Connected; + } + catch + { + return false; + } + } + } + + public bool DtrEnable { get; set; } + + public void Open() + { + if (client.Client.Connected) + { + return; + } + + + /*string url = "";*/ + + + int count = url.Split('@').Length - 1; + + if (count > 1) + { + var regex = new Regex("@"); + url = regex.Replace(url, "%40", 1); + } + + url = url.Replace("ntrip://", "http://"); + + remoteUri = new Uri(url); + + doConnect(); + } + + private byte[] TcpKeepAlive(bool On_Off, uint KeepaLiveTime, uint KeepaLiveInterval) + { + byte[] InValue = new byte[12]; + + Array.ConstrainedCopy(BitConverter.GetBytes(Convert.ToUInt32(On_Off)), 0, InValue, 0, 4); + Array.ConstrainedCopy(BitConverter.GetBytes(KeepaLiveTime), 0, InValue, 4, 4); + Array.ConstrainedCopy(BitConverter.GetBytes(KeepaLiveInterval), 0, InValue, 8, 4); + + return InValue; + } + + private void doConnect() + { + string usernamePassword = remoteUri.UserInfo; + string userpass2 = Uri.UnescapeDataString(usernamePassword); + string auth = "Authorization: Basic " + + Convert.ToBase64String(new ASCIIEncoding().GetBytes(userpass2)) + "\r\n"; + + if (usernamePassword == "") + auth = ""; + + host = remoteUri.Host; + Port = remoteUri.Port.ToString(); + + client = new TcpClient(host, int.Parse(Port)); + client.Client.IOControl(IOControlCode.KeepAliveValues, TcpKeepAlive(true, 36000000, 3000), null); + + NetworkStream ns = client.GetStream(); + + StreamWriter sw = new StreamWriter(ns); + StreamReader sr = new StreamReader(ns); + + string line = "GET " + remoteUri.PathAndQuery + " HTTP/1.0\r\n" + + "User-Agent: NTRIP MissionPlanner/1.0\r\n" + + auth + + "Connection: close\r\n\r\n"; + + sw.Write(line); + + sw.Flush(); + + line = sr.ReadLine(); + + if (!line.Contains("200")) + { + client.Dispose(); + + client = new TcpClient(); + + throw new Exception("Bad ntrip Responce\n\n" + line); + } + + // vrs may take up to 60+ seconds to respond + SendNMEA(); + + VerifyConnected(); + } + + DateTime _lastnmea = DateTime.MinValue; + + private void SendNMEA() + { + if (lat != 0 || lng != 0) + { + DateTime currentUtcTime = DateTime.UtcNow; + if (_lastnmea.AddSeconds(30) < currentUtcTime) + { + double latdms = (int)lat + ((lat - (int)lat) * .6f); + double lngdms = (int)lng + ((lng - (int)lng) * .6f); + + var line = string.Format(System.Globalization.CultureInfo.InvariantCulture, + "$GP{0},{1:HHmmss.ff},{2},{3},{4},{5},{6},{7},{8},{9},{10},{11},{12},{13},{14}", "GGA", + currentUtcTime, Math.Abs(latdms * 100).ToString("0000.00"), lat < 0 ? "S" : "N", + Math.Abs(lngdms * 100).ToString("00000.00"), lng < 0 ? "W" : "E", 1, 10, + 1, alt.ToString("0.00"), "M", 0, "M", "0.0", "0"); + + string checksum = GetChecksum(line); + WriteLine(line + "*" + checksum); + _lastnmea = currentUtcTime; + } + } + + } + + // Calculates the checksum for a sentence + string GetChecksum(string sentence) + { + // Loop through all chars to get a checksum + int Checksum = 0; + foreach (char Character in sentence.ToCharArray()) + { + switch (Character) + { + case '$': + // Ignore the dollar sign + break; + case '*': + // Stop processing before the asterisk + continue; + default: + // Is this the first value for the checksum? + if (Checksum == 0) + { + // Yes. Set the checksum to the value + Checksum = Convert.ToByte(Character); + } + else + { + // No. XOR the checksum with this character's value + Checksum = Checksum ^ Convert.ToByte(Character); + } + break; + } + } + // Return the checksum formatted as a two-character hexadecimal + return Checksum.ToString("X2"); + } + + void VerifyConnected() + { + if (!IsOpen) + { + try + { + client.Dispose(); + client = new TcpClient(); + } + catch { } + + // this should only happen if we have established a connection in the first place + if (client != null && retrys > 0) + { + doConnect(); + retrys--; + } + + throw new Exception("网络RTK基站通讯已关闭!"); + } + } + + public int Read(byte[] readto, int offset, int length) + { + VerifyConnected(); + + SendNMEA(); + + try + { + if (length < 1) { return 0; } + + return client.Client.Receive(readto, offset, length, SocketFlags.Partial); + /* + byte[] temp = new byte[length]; + clientbuf.Read(temp, 0, length); + + temp.CopyTo(readto, offset); + + return length;*/ + } + catch { throw new Exception("ntrip Socket Closed"); } + } + + public int ReadByte() + { + VerifyConnected(); + int count = 0; + while (this.BytesToRead == 0) + { + System.Threading.Thread.Sleep(1); + if (count > ReadTimeout) + throw new Exception("ntrip Timeout on read"); + count++; + } + byte[] buffer = new byte[1]; + Read(buffer, 0, 1); + return buffer[0]; + } + + public int ReadChar() + { + return ReadByte(); + } + + public string ReadExisting() + { + VerifyConnected(); + byte[] data = new byte[client.Available]; + if (data.Length > 0) + Read(data, 0, data.Length); + + string line = Encoding.ASCII.GetString(data, 0, data.Length); + + return line; + } + + public void WriteLine(string line) + { + VerifyConnected(); + line = line + "\r\n"; + Write(line); + } + + public void Write(string line) + { + VerifyConnected(); + byte[] data = new System.Text.ASCIIEncoding().GetBytes(line); + Write(data, 0, data.Length); + } + + public void Write(byte[] write, int offset, int length) + { + VerifyConnected(); + try + { + client.Client.Send(write, length, SocketFlags.None); + } + catch { }//throw new Exception("Comport / Socket Closed"); } + } + + public void DiscardInBuffer() + { + VerifyConnected(); + int size = (int)client.Available; + byte[] crap = new byte[size]; + Read(crap, 0, size); + } + + public string ReadLine() + { + byte[] temp = new byte[4000]; + int count = 0; + int timeout = 0; + + while (timeout <= 100) + { + if (!this.IsOpen) { break; } + if (this.BytesToRead > 0) + { + byte letter = (byte)this.ReadByte(); + + temp[count] = letter; + + if (letter == '\n') // normal line + { + break; + } + + + count++; + if (count == temp.Length) + break; + timeout = 0; + } + else + { + timeout++; + System.Threading.Thread.Sleep(5); + } + } + + Array.Resize(ref temp, count + 1); + + return Encoding.ASCII.GetString(temp, 0, temp.Length); + } + + public void Close() + { + try + { + if (client.Client != null && client.Client.Connected) + { + client.Client.Dispose(); + client.Dispose(); + } + } + catch { } + + try + { + client.Dispose(); + } + catch { } + + client = new TcpClient(); + } + + protected virtual void Dispose(bool disposing) + { + if (disposing) + { + // dispose managed resources + this.Close(); + client = null; + } + // free native resources + } + + public void Dispose() + { + Dispose(true); + GC.SuppressFinalize(this); + } + } +}