diff --git a/Plane.FormationCreator/Formation/RtcmManager.cs b/Plane.FormationCreator/Formation/RtcmManager.cs index 361b99d..1624730 100644 --- a/Plane.FormationCreator/Formation/RtcmManager.cs +++ b/Plane.FormationCreator/Formation/RtcmManager.cs @@ -22,6 +22,10 @@ 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; @@ -31,17 +35,20 @@ namespace Plane.FormationCreator.Formation 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 _rtcmsendtype = 1; + 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) { @@ -54,6 +61,9 @@ namespace Plane.FormationCreator.Formation case MSG_GLONASS: typename = "GLONASS"; break; + case MSG_GLONASS_PHASE: + typename = "GLONASS相位差"; + break; case MSG_Beidou: typename = "北斗"; break; @@ -84,7 +94,18 @@ namespace Plane.FormationCreator.Formation } } + public bool SmallRtcmData + { + get { return _smallrtcmdata; } + set + { + Set(nameof(SmallRtcmData), ref _smallrtcmdata, value); + } + } + + //接收速率 private int bps; + //发送速率 private int bpsusefull; private int _bps = 0; @@ -180,7 +201,7 @@ namespace Plane.FormationCreator.Formation dispatcherTimer.Start(); } - //用于刷新界面，显示消息状态，基站和卫星是否有效等 + //用于刷新界面，显示消息状态，基站和卫星是否有效等---1HZ private void OnTimedEvent(object sender, EventArgs e) { BaseState = baseTime > DateTime.Now; @@ -386,6 +407,8 @@ namespace Plane.FormationCreator.Formation //await _commModuleManager.StartRtcmLoop(); while (Rtcmthreadrun) { + // Console.WriteLine(string.Format("{0:T} 1", DateTime.Now)); + try { try @@ -405,97 +428,151 @@ namespace Plane.FormationCreator.Formation } } catch { } - + if (!comPort.IsOpen) { return; } - - //rtcm size=180 - byte[] buffer = new byte[180];// new byte[180]; - //一次读取180个字节 - int read = comPort.Read(buffer, 0, Math.Min(buffer.Length, comPort.BytesToRead)); - if (read > 0) + /////////////////////////////////重写发送RTCM数据部分---23/10/18 by panxu + /// + // limit to 180 byte packet if using new packet + byte[] buffer = new byte[180]; + while (comPort.BytesToRead > 0) { - lastrecv = DateTime.Now; - } - bps += read; - for (int a = 0; a < read; a++) - { - int seenmsg = -1; - // rtcm类解析RTCM数据 如果 rtcm.Read>0解析成功返回消息号码-1解析失败 - if ((seenmsg = rtcm.Read(buffer[a])) > 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++) { - bpsusefull += rtcm.length; - string msgshowname = rtcm_typename(seenmsg); - _commModuleManager.SetAllCoptersForWifi(_copterManager.Copters); - if (_rtcmsendtype == 0) + int seenmsg = -1; + // rtcm and not can + if ((seenmsg = rtcm.Read(buffer[a])) > 0) { - await _commModuleManager.InjectGpsDataAsync(rtcm.packet, (ushort)rtcm.length); - }else - if (_rtcmsendtype == 1) - { - Ensend = false; - //第1秒是gps,第2秒是GLONASS，第3秒是 Beidou - //1秒内可能发送多个同一种类型的数据，看数据大小和原始频率 - long curr_s = CurrentTimeMillis()/1000; - int sendtype = (int) (curr_s % 3); - switch (seenmsg) + // 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) { - case MSG_GPS: - if (sendtype==0) //第1秒是gps, - Ensend = true; - break; + Console.WriteLine(DateTime.Now.ToString("HH:mm:ss") + "--广播Rtcm长度: " + (ushort)rtcm.length + " 类型: " + msgshowname + " (" + seenmsg + ")"); + _commModuleManager.BroadcastGpsDataAsync(rtcm.packet, (ushort)rtcm.length); + } - case MSG_GLONASS: - 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; + //直接发送 + 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) - { - // Plane.Windows.Messages.Message.Show(DateTime.Now.ToString("HH:mm:ss")+"--Rtcm长度: " + (ushort)rtcm.length+" , 类型: "+ seenmsg); - Console.WriteLine(DateTime.Now.ToString("HH:mm:ss") + "--Rtcm长度: " + (ushort)rtcm.length + " 类型: " + msgshowname + " ("+ seenmsg+")"); - await _commModuleManager.InjectGpsDataAsync(rtcm.packet, (ushort)rtcm.length, _enrecom); - } - //else - // Console.WriteLine(DateTime.Now.ToString("HH:mm:ss") + "--放弃发送-----Rtcm长度: " + (ushort)rtcm.length + " 类型: " + msgshowname + " (" + seenmsg + ")"); + 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; + */ } - //累加消息数量，用于界面显示 - if (!msgseen.ContainsKey(msgshowname)) - msgseen[msgshowname] = 0; - msgseen[msgshowname] = (int)msgseen[msgshowname] + 1; - //检测基站定位状态和卫星定位状态，用于在界面上显示 - await ExtractBasePos(seenmsg); } + ////////////////////////////解析/发送RTCM结束 + await Task.Delay(10); } - await Task.Delay(10); } - catch(Exception ex) + catch (Exception ex) { Plane.Windows.Messages.Message.Show(ex.Message); } diff --git a/Plane.FormationCreator/Plane.FormationCreator.csproj b/Plane.FormationCreator/Plane.FormationCreator.csproj index 13bf095..05bdeae 100644 --- a/Plane.FormationCreator/Plane.FormationCreator.csproj +++ b/Plane.FormationCreator/Plane.FormationCreator.csproj @@ -211,10 +211,12 @@ + + diff --git a/Plane.FormationCreator/Util/nmea.cs b/Plane.FormationCreator/Util/nmea.cs new file mode 100644 index 0000000..f2f7eb9 --- /dev/null +++ b/Plane.FormationCreator/Util/nmea.cs @@ -0,0 +1,123 @@ +using System; + +using uint8_t = System.Byte; +using uint16_t = System.UInt16; +using int32_t = System.Int32; +using uint32_t = System.UInt32; +using int8_t = System.SByte; +using System.Runtime.InteropServices; + +namespace Plane.Util +{ + public class nmea : ICorrections + { + int step = 0; + + public byte[] buffer = new byte[1024 * 1]; + int payloadlen = 0; + int msglencount = 0; + + public int length + { + get + { + return 2 + 2 + 2 + 2 + payloadlen; // header2, class,subclass,length2,data,crc2 + } + } + + public byte[] packet + { + get + { + return buffer; + } + } + + public bool resetParser() + { + step = 0; + return true; + } + + public int Read(byte data) + { + switch (step) + { + default: + case 0: + if (data == '$') + { + step = 1; + msglencount = 0; + buffer[0] = data; + } + break; + case 1: + if (data == 'G') + { + buffer[1] = data; + step++; + } + else + step = 0; + break; + case 2: + if (msglencount > 1000) + { + step = 0; + + } + + buffer[msglencount + 2] = data; + msglencount++; + if(data == '\n') + { + var line = System.Text.ASCIIEncoding.ASCII.GetString(buffer, 0, msglencount + 2); + string[] items = line.Trim().Split(',', '*'); + if (items[items.Length-1] == GetChecksum(line)) + { + return 1; + } + step = 0; + } + break; + } + + return -1; + } + + // 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 + return Checksum.ToString("X2"); + 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"); + } + } +} \ No newline at end of file diff --git a/Plane.FormationCreator/Util/rtcm3.cs b/Plane.FormationCreator/Util/rtcm3.cs index 82cae04..9b91e50 100644 --- a/Plane.FormationCreator/Util/rtcm3.cs +++ b/Plane.FormationCreator/Util/rtcm3.cs @@ -5,6 +5,9 @@ using u16 = System.UInt16; using s32 = System.Int32; using u32 = System.UInt32; using gps_time_t = System.UInt64; +using System.Runtime.InteropServices; +using RTCM3 = Plane.Util.rtcm3; + namespace Plane.Util { @@ -17,7 +20,7 @@ namespace Plane.Util private const double SC2RAD = 3.1415926535898; /* semi-circle to radian (IS-GPS) */ private const double FREQ1 = 1.57542E9; /* L1/E1 frequency (Hz) */ private const double FREQ2 = 1.22760E9; /* L2 frequency (Hz) */ - private const double RANGE_MS = CLIGHT*0.001; /* range in 1 ms */ + private const double RANGE_MS = CLIGHT * 0.001; /* range in 1 ms */ private const double P2_5 = 0.03125; /* 2^-5 */ private const double P2_6 = 0.015625; /* 2^-6 */ private const double P2_10 = 0.0009765625; /* 2^-10 */ @@ -44,25 +47,32 @@ namespace Plane.Util private const double P2_50 = 8.881784197001252E-16; /* 2^-50 */ private const double P2_55 = 2.775557561562891E-17; /* 2^-55 */ private const double RE_WGS84 = 6378137.0; /* earth semimajor axis (WGS84) (m) */ - private const double FE_WGS84 = (1.0/298.257223563); /* earth flattening (WGS84) */ + private const double FE_WGS84 = (1.0 / 298.257223563); /* earth flattening (WGS84) */ private const double PI = Math.PI; /* pi */ - public const double D2R = (PI/180.0); /* deg to rad */ - public const double R2D = (180.0/PI); /* rad to deg */ + public const double D2R = (PI / 180.0); /* deg to rad */ + public const double R2D = (180.0 / PI); /* rad to deg */ + + public u32 sync; + + private static double lasttow = 0; + + public static int weekGuess = 0; + private int msglencount; private uint payloadlen; private rtcmpreamble pre; - private int step; + public int step; public event EventHandler ObsMessage; public event EventHandler BasePosMessage; - + public event EventHandler EphMessage; public int length { - get { return (int) (payloadlen + 2 + 1); } + get { return (int)(payloadlen + 2 + 1); } } - public byte[] packet { get; } = new byte[1024*4]; + public byte[] packet { get; } = new byte[1024 * 4]; public bool resetParser() { @@ -122,13 +132,15 @@ namespace Plane.Util payloadlen = payloadlen + 3; var crc = crc24.crc24q(packet, payloadlen, 0); - var crcpacket = getbitu(packet, payloadlen*8, 24); + var crcpacket = getbitu(packet, payloadlen * 8, 24); if (crc == crcpacket) { var head = new rtcmheader(); head.Read(packet); + sync = head.sync; + step = 0; if (head.messageno == 1002) @@ -194,6 +206,42 @@ namespace Plane.Util if (ObsMessage != null) ObsMessage(tp.obs, null); } + else if (head.messageno == 1094) + { + var tp = new type1094(); + + tp.Read(packet); + + if (ObsMessage != null) + ObsMessage(tp.obs, null); + } + else if (head.messageno == 1097) + { + var tp = new type1097(); + + tp.Read(packet); + + if (ObsMessage != null) + ObsMessage(tp.obs, null); + } + else if (head.messageno == 1114) + { + var tp = new type1114(); + + tp.Read(packet); + + if (ObsMessage != null) + ObsMessage(tp.obs, null); + } + else if (head.messageno == 1117) + { + var tp = new type1117(); + + tp.Read(packet); + + if (ObsMessage != null) + ObsMessage(tp.obs, null); + } else if (head.messageno == 1124) { var tp = new type1124(); @@ -230,16 +278,35 @@ namespace Plane.Util if (BasePosMessage != null) BasePosMessage(tp, null); } - /* else if (head.messageno == 1019) { var tp = new type1019(); tp.Read(packet); + var week = (int)tp.week; + var tow = 0.0; + + // assumes this is always a live stream + StaticUtils.GetFromTime(DateTime.UtcNow, ref week, ref tow); + + // both at start of a week + if (tp.toes < (60 * 60 * 24) && lasttow < (60 * 60 * 24)) + { + RTCM3.weekGuess = week; + } // eph at end of week and tow at start of week + else if (tp.toes > 500000 && lasttow < (60 * 60 * 24)) + { + RTCM3.weekGuess = week + 1; + } // everything else + else + { + RTCM3.weekGuess = week; + } + if (EphMessage != null) EphMessage(tp, null); - }*/ + } return head.messageno; } @@ -255,35 +322,40 @@ namespace Plane.Util uint bits = 0; uint i; for (i = pos; i < pos + len; i++) - bits = (uint) ((bits << 1) + ((buff[i/8] >> (int) (7 - i%8)) & 1u)); + bits = (uint)((bits << 1) + ((buff[i / 8] >> (int)(7 - i % 8)) & 1u)); return bits; } + private static void setbitu(byte[] buff, uint pos, uint len, double data) + { + setbitu(buff, pos, len, (uint)data); + } + private static void setbitu(byte[] buff, uint pos, uint len, uint data) { - var mask = 1u << (int) (len - 1); + var mask = 1u << (int)(len - 1); if (len <= 0 || 32 < len) return; for (var i = pos; i < pos + len; i++, mask >>= 1) { if ((data & mask) > 0) - buff[i/8] |= (byte) (1u << (int) (7 - i%8)); + buff[i / 8] |= (byte)(1u << (int)(7 - i % 8)); else - buff[i/8] &= (byte) (~(1u << (int) (7 - i%8))); + buff[i / 8] &= (byte)(~(1u << (int)(7 - i % 8))); } } private static double ROUND(double x) { - return (int) Math.Floor(x + 0.5); + return (int)Math.Floor(x + 0.5); } /* carrier-phase - pseudorange in cycle --------------------------------------*/ private static double cp_pr(double cp, double pr_cyc) { - var x = (cp - pr_cyc + 1500.0)%3000.0; + var x = (cp - pr_cyc + 1500.0) % 3000.0; if (x < 0) x += 3000; x -= 1500.0; @@ -292,48 +364,53 @@ namespace Plane.Util private static double getbits_38(byte[] buff, uint pos) { - return getbits(buff, pos, 32)*64.0 + getbitu(buff, pos + 32, 6); + return getbits(buff, pos, 32) * 64.0 + getbitu(buff, pos + 32, 6); } private static int getbits(byte[] buff, uint pos, uint len) { var bits = getbitu(buff, pos, len); - if (len <= 0 || 32 <= len || !((bits & (1u << (int) (len - 1))) != 0)) - return (int) bits; - return (int) (bits | (~0u << (int) len)); /* extend sign */ + if (len <= 0 || 32 <= len || !((bits & (1u << (int)(len - 1))) != 0)) + return (int)bits; + return (int)(bits | (~0u << (int)len)); /* extend sign */ } private static void set38bits(byte[] buff, uint pos, double value) { - var word_h = (int) Math.Floor(value/64.0); - var word_l = (uint) (value - word_h*64.0); + var word_h = (int)Math.Floor(value / 64.0); + var word_l = (uint)(value - word_h * 64.0); setbits(buff, pos, 32, word_h); setbitu(buff, pos + 32, 6, word_l); } + private static void setbits(byte[] buff, uint pos, uint len, double data) + { + setbits(buff, pos, len, (int)data); + } + private static void setbits(byte[] buff, uint pos, uint len, int data) { if (data < 0) - data |= 1 << (int) (len - 1); + data |= 1 << (int)(len - 1); else - data &= ~(1 << (int) (len - 1)); /* set sign bit */ - setbitu(buff, pos, len, (uint) data); + data &= ~(1 << (int)(len - 1)); /* set sign bit */ + setbitu(buff, pos, len, (uint)data); } public static void ecef2pos(double[] r, ref double[] pos) { - double e2 = FE_WGS84*(2.0 - FE_WGS84), r2 = dot(r, r, 2), z, zk, v = RE_WGS84, sinp; + double e2 = FE_WGS84 * (2.0 - FE_WGS84), r2 = dot(r, r, 2), z, zk, v = RE_WGS84, sinp; for (z = r[2], zk = 0.0; Math.Abs(z - zk) >= 1E-4;) { zk = z; - sinp = z/Math.Sqrt(r2 + z*z); - v = RE_WGS84/Math.Sqrt(1.0 - e2*sinp*sinp); - z = r[2] + v*e2*sinp; + sinp = z / Math.Sqrt(r2 + z * z); + v = RE_WGS84 / Math.Sqrt(1.0 - e2 * sinp * sinp); + z = r[2] + v * e2 * sinp; } - pos[0] = r2 > 1E-12 ? Math.Atan(z/Math.Sqrt(r2)) : (r[2] > 0.0 ? PI/2.0 : -PI/2.0); + pos[0] = r2 > 1E-12 ? Math.Atan(z / Math.Sqrt(r2)) : (r[2] > 0.0 ? PI / 2.0 : -PI / 2.0); pos[1] = r2 > 1E-12 ? Math.Atan2(r[1], r[0]) : 0.0; - pos[2] = Math.Sqrt(r2 + z*z) - v; + pos[2] = Math.Sqrt(r2 + z * z) - v; } public static void pos2ecef(double[] pos, ref double[] r) @@ -353,7 +430,7 @@ namespace Plane.Util { var c = 0.0; - while (--n >= 0) c += a[n]*b[n]; + while (--n >= 0) c += a[n] * b[n]; return c; } @@ -367,11 +444,11 @@ namespace Plane.Util { uint i = 0; - preamble = (byte) getbitu(buffer, i, 8); + preamble = (byte)getbitu(buffer, i, 8); i += 8; - resv1 = (byte) getbitu(buffer, i, 6); + resv1 = (byte)getbitu(buffer, i, 6); i += 6; - length = (ushort) getbitu(buffer, i, 10); + length = (ushort)getbitu(buffer, i, 10); i += 10; } @@ -404,9 +481,9 @@ namespace Plane.Util { uint i = 24; - messageno = (ushort) getbitu(buffer, i, 12); + messageno = (ushort)getbitu(buffer, i, 12); i += 12; /* message no */ - refstationid = (ushort) getbitu(buffer, i, 12); + refstationid = (ushort)getbitu(buffer, i, 12); i += 12; /* ref station id */ if (messageno < 1009 || messageno > 1012) { @@ -418,13 +495,13 @@ namespace Plane.Util epoch = getbitu(buffer, i, 27); i += 27; /* glonass epoch time */ } - sync = (byte) getbitu(buffer, i, 1); + sync = (byte)getbitu(buffer, i, 1); i += 1; /* synchronous gnss flag */ - nsat = (byte) getbitu(buffer, i, 5); + nsat = (byte)getbitu(buffer, i, 5); i += 5; /* no of satellites */ - smoothind = (byte) getbitu(buffer, i, 1); + smoothind = (byte)getbitu(buffer, i, 1); i += 1; /* smoothing indicator */ - smoothint = (byte) getbitu(buffer, i, 3); + smoothint = (byte)getbitu(buffer, i, 3); i += 3; /* smoothing interval */ } @@ -454,39 +531,39 @@ namespace Plane.Util { private static readonly uint[] crc24qtab = { - 0x000000, 0x864CFB, 0x8AD50D, 0x0C99F6, 0x93E6E1, 0x15AA1A, 0x1933EC, 0x9F7F17, - 0xA18139, 0x27CDC2, 0x2B5434, 0xAD18CF, 0x3267D8, 0xB42B23, 0xB8B2D5, 0x3EFE2E, - 0xC54E89, 0x430272, 0x4F9B84, 0xC9D77F, 0x56A868, 0xD0E493, 0xDC7D65, 0x5A319E, - 0x64CFB0, 0xE2834B, 0xEE1ABD, 0x685646, 0xF72951, 0x7165AA, 0x7DFC5C, 0xFBB0A7, - 0x0CD1E9, 0x8A9D12, 0x8604E4, 0x00481F, 0x9F3708, 0x197BF3, 0x15E205, 0x93AEFE, - 0xAD50D0, 0x2B1C2B, 0x2785DD, 0xA1C926, 0x3EB631, 0xB8FACA, 0xB4633C, 0x322FC7, - 0xC99F60, 0x4FD39B, 0x434A6D, 0xC50696, 0x5A7981, 0xDC357A, 0xD0AC8C, 0x56E077, - 0x681E59, 0xEE52A2, 0xE2CB54, 0x6487AF, 0xFBF8B8, 0x7DB443, 0x712DB5, 0xF7614E, - 0x19A3D2, 0x9FEF29, 0x9376DF, 0x153A24, 0x8A4533, 0x0C09C8, 0x00903E, 0x86DCC5, - 0xB822EB, 0x3E6E10, 0x32F7E6, 0xB4BB1D, 0x2BC40A, 0xAD88F1, 0xA11107, 0x275DFC, - 0xDCED5B, 0x5AA1A0, 0x563856, 0xD074AD, 0x4F0BBA, 0xC94741, 0xC5DEB7, 0x43924C, - 0x7D6C62, 0xFB2099, 0xF7B96F, 0x71F594, 0xEE8A83, 0x68C678, 0x645F8E, 0xE21375, - 0x15723B, 0x933EC0, 0x9FA736, 0x19EBCD, 0x8694DA, 0x00D821, 0x0C41D7, 0x8A0D2C, - 0xB4F302, 0x32BFF9, 0x3E260F, 0xB86AF4, 0x2715E3, 0xA15918, 0xADC0EE, 0x2B8C15, - 0xD03CB2, 0x567049, 0x5AE9BF, 0xDCA544, 0x43DA53, 0xC596A8, 0xC90F5E, 0x4F43A5, - 0x71BD8B, 0xF7F170, 0xFB6886, 0x7D247D, 0xE25B6A, 0x641791, 0x688E67, 0xEEC29C, - 0x3347A4, 0xB50B5F, 0xB992A9, 0x3FDE52, 0xA0A145, 0x26EDBE, 0x2A7448, 0xAC38B3, - 0x92C69D, 0x148A66, 0x181390, 0x9E5F6B, 0x01207C, 0x876C87, 0x8BF571, 0x0DB98A, - 0xF6092D, 0x7045D6, 0x7CDC20, 0xFA90DB, 0x65EFCC, 0xE3A337, 0xEF3AC1, 0x69763A, - 0x578814, 0xD1C4EF, 0xDD5D19, 0x5B11E2, 0xC46EF5, 0x42220E, 0x4EBBF8, 0xC8F703, - 0x3F964D, 0xB9DAB6, 0xB54340, 0x330FBB, 0xAC70AC, 0x2A3C57, 0x26A5A1, 0xA0E95A, - 0x9E1774, 0x185B8F, 0x14C279, 0x928E82, 0x0DF195, 0x8BBD6E, 0x872498, 0x016863, - 0xFAD8C4, 0x7C943F, 0x700DC9, 0xF64132, 0x693E25, 0xEF72DE, 0xE3EB28, 0x65A7D3, - 0x5B59FD, 0xDD1506, 0xD18CF0, 0x57C00B, 0xC8BF1C, 0x4EF3E7, 0x426A11, 0xC426EA, - 0x2AE476, 0xACA88D, 0xA0317B, 0x267D80, 0xB90297, 0x3F4E6C, 0x33D79A, 0xB59B61, - 0x8B654F, 0x0D29B4, 0x01B042, 0x87FCB9, 0x1883AE, 0x9ECF55, 0x9256A3, 0x141A58, - 0xEFAAFF, 0x69E604, 0x657FF2, 0xE33309, 0x7C4C1E, 0xFA00E5, 0xF69913, 0x70D5E8, - 0x4E2BC6, 0xC8673D, 0xC4FECB, 0x42B230, 0xDDCD27, 0x5B81DC, 0x57182A, 0xD154D1, - 0x26359F, 0xA07964, 0xACE092, 0x2AAC69, 0xB5D37E, 0x339F85, 0x3F0673, 0xB94A88, - 0x87B4A6, 0x01F85D, 0x0D61AB, 0x8B2D50, 0x145247, 0x921EBC, 0x9E874A, 0x18CBB1, - 0xE37B16, 0x6537ED, 0x69AE1B, 0xEFE2E0, 0x709DF7, 0xF6D10C, 0xFA48FA, 0x7C0401, - 0x42FA2F, 0xC4B6D4, 0xC82F22, 0x4E63D9, 0xD11CCE, 0x575035, 0x5BC9C3, 0xDD8538 - }; + 0x000000, 0x864CFB, 0x8AD50D, 0x0C99F6, 0x93E6E1, 0x15AA1A, 0x1933EC, 0x9F7F17, + 0xA18139, 0x27CDC2, 0x2B5434, 0xAD18CF, 0x3267D8, 0xB42B23, 0xB8B2D5, 0x3EFE2E, + 0xC54E89, 0x430272, 0x4F9B84, 0xC9D77F, 0x56A868, 0xD0E493, 0xDC7D65, 0x5A319E, + 0x64CFB0, 0xE2834B, 0xEE1ABD, 0x685646, 0xF72951, 0x7165AA, 0x7DFC5C, 0xFBB0A7, + 0x0CD1E9, 0x8A9D12, 0x8604E4, 0x00481F, 0x9F3708, 0x197BF3, 0x15E205, 0x93AEFE, + 0xAD50D0, 0x2B1C2B, 0x2785DD, 0xA1C926, 0x3EB631, 0xB8FACA, 0xB4633C, 0x322FC7, + 0xC99F60, 0x4FD39B, 0x434A6D, 0xC50696, 0x5A7981, 0xDC357A, 0xD0AC8C, 0x56E077, + 0x681E59, 0xEE52A2, 0xE2CB54, 0x6487AF, 0xFBF8B8, 0x7DB443, 0x712DB5, 0xF7614E, + 0x19A3D2, 0x9FEF29, 0x9376DF, 0x153A24, 0x8A4533, 0x0C09C8, 0x00903E, 0x86DCC5, + 0xB822EB, 0x3E6E10, 0x32F7E6, 0xB4BB1D, 0x2BC40A, 0xAD88F1, 0xA11107, 0x275DFC, + 0xDCED5B, 0x5AA1A0, 0x563856, 0xD074AD, 0x4F0BBA, 0xC94741, 0xC5DEB7, 0x43924C, + 0x7D6C62, 0xFB2099, 0xF7B96F, 0x71F594, 0xEE8A83, 0x68C678, 0x645F8E, 0xE21375, + 0x15723B, 0x933EC0, 0x9FA736, 0x19EBCD, 0x8694DA, 0x00D821, 0x0C41D7, 0x8A0D2C, + 0xB4F302, 0x32BFF9, 0x3E260F, 0xB86AF4, 0x2715E3, 0xA15918, 0xADC0EE, 0x2B8C15, + 0xD03CB2, 0x567049, 0x5AE9BF, 0xDCA544, 0x43DA53, 0xC596A8, 0xC90F5E, 0x4F43A5, + 0x71BD8B, 0xF7F170, 0xFB6886, 0x7D247D, 0xE25B6A, 0x641791, 0x688E67, 0xEEC29C, + 0x3347A4, 0xB50B5F, 0xB992A9, 0x3FDE52, 0xA0A145, 0x26EDBE, 0x2A7448, 0xAC38B3, + 0x92C69D, 0x148A66, 0x181390, 0x9E5F6B, 0x01207C, 0x876C87, 0x8BF571, 0x0DB98A, + 0xF6092D, 0x7045D6, 0x7CDC20, 0xFA90DB, 0x65EFCC, 0xE3A337, 0xEF3AC1, 0x69763A, + 0x578814, 0xD1C4EF, 0xDD5D19, 0x5B11E2, 0xC46EF5, 0x42220E, 0x4EBBF8, 0xC8F703, + 0x3F964D, 0xB9DAB6, 0xB54340, 0x330FBB, 0xAC70AC, 0x2A3C57, 0x26A5A1, 0xA0E95A, + 0x9E1774, 0x185B8F, 0x14C279, 0x928E82, 0x0DF195, 0x8BBD6E, 0x872498, 0x016863, + 0xFAD8C4, 0x7C943F, 0x700DC9, 0xF64132, 0x693E25, 0xEF72DE, 0xE3EB28, 0x65A7D3, + 0x5B59FD, 0xDD1506, 0xD18CF0, 0x57C00B, 0xC8BF1C, 0x4EF3E7, 0x426A11, 0xC426EA, + 0x2AE476, 0xACA88D, 0xA0317B, 0x267D80, 0xB90297, 0x3F4E6C, 0x33D79A, 0xB59B61, + 0x8B654F, 0x0D29B4, 0x01B042, 0x87FCB9, 0x1883AE, 0x9ECF55, 0x9256A3, 0x141A58, + 0xEFAAFF, 0x69E604, 0x657FF2, 0xE33309, 0x7C4C1E, 0xFA00E5, 0xF69913, 0x70D5E8, + 0x4E2BC6, 0xC8673D, 0xC4FECB, 0x42B230, 0xDDCD27, 0x5B81DC, 0x57182A, 0xD154D1, + 0x26359F, 0xA07964, 0xACE092, 0x2AAC69, 0xB5D37E, 0x339F85, 0x3F0673, 0xB94A88, + 0x87B4A6, 0x01F85D, 0x0D61AB, 0x8B2D50, 0x145247, 0x921EBC, 0x9E874A, 0x18CBB1, + 0xE37B16, 0x6537ED, 0x69AE1B, 0xEFE2E0, 0x709DF7, 0xF6D10C, 0xFA48FA, 0x7C0401, + 0x42FA2F, 0xC4B6D4, 0xC82F22, 0x4E63D9, 0xD11CCE, 0x575035, 0x5BC9C3, 0xDD8538 + }; /** Calculate Qualcomm 24-bit Cyclical Redundancy Check (CRC-24Q). * @@ -526,7 +603,7 @@ namespace Plane.Util var staid = getbitu(buffer, i, 12); i += 12; - var tow = getbitu(buffer, i, 30)*0.001; + var tow = getbitu(buffer, i, 30) * 0.001; i += 30; var sync = getbitu(buffer, i, 1); i += 1; @@ -534,18 +611,12 @@ namespace Plane.Util i = 24 + 64; - var week = 0; - double seconds = 0; - - // asumes current week - StaticUtils.GetFromTime(DateTime.Now, ref week, ref seconds); - - // if tow is larger than the calced curretn time, go back one week - if (tow > seconds) - week--; + var week = RTCM3.weekGuess; var gpstime = StaticUtils.GetFromGps(week, tow); + lasttow = tow; + //Console.WriteLine("> {0,4} {1,2} {2,2} {3,2} {4,2} {5,10} {6,2} {7,2}", gpstime.Year, gpstime.Month,gpstime.Day, gpstime.Hour, gpstime.Minute, gpstime.Second + gpstime.Millisecond/1000.0, 0, nsat); for (var a = 0; a < nsat; a++) @@ -555,33 +626,33 @@ namespace Plane.Util ob.tow = tow; ob.week = week; - ob.raw.prn = (byte) getbitu(buffer, i, 6); + ob.raw.prn = (byte)getbitu(buffer, i, 6); i += 6; - ob.raw.code1 = (byte) getbitu(buffer, i, 1); + ob.raw.code1 = (byte)getbitu(buffer, i, 1); i += 1; ob.raw.pr1 = getbitu(buffer, i, 24); i += 24; ob.raw.ppr1 = getbits(buffer, i, 20); i += 20; - ob.raw.lock1 = (byte) getbitu(buffer, i, 7); + ob.raw.lock1 = (byte)getbitu(buffer, i, 7); i += 7; - ob.raw.amb = (byte) getbitu(buffer, i, 8); + ob.raw.amb = (byte)getbitu(buffer, i, 8); i += 8; - ob.raw.cnr1 = (byte) getbitu(buffer, i, 8); + ob.raw.cnr1 = (byte)getbitu(buffer, i, 8); i += 8; - var pr1 = ob.raw.pr1*0.02 + ob.raw.amb*PRUNIT_GPS; + var pr1 = ob.raw.pr1 * 0.02 + ob.raw.amb * PRUNIT_GPS; - var lam1 = CLIGHT/FREQ1; + var lam1 = CLIGHT / FREQ1; - var cp1 = ob.raw.ppr1*0.0005/lam1; + var cp1 = ob.raw.ppr1 * 0.0005 / lam1; - if ((uint) ob.raw.ppr1 != 0xFFF80000) + if ((uint)ob.raw.ppr1 != 0xFFF80000) { ob.prn = ob.raw.prn; - ob.cp = pr1/lam1 + cp1; + ob.cp = pr1 / lam1 + cp1; ob.pr = pr1; - ob.snr = (byte) (ob.raw.cnr1*0.25); // *4.0+0.5 + ob.snr = (byte)(ob.raw.cnr1 * 0.25); // *4.0+0.5 obs.Add(ob); @@ -589,7 +660,7 @@ namespace Plane.Util } } - obs.Sort(delegate(ob a, ob b) { return a.prn.CompareTo(b.prn); }); + obs.Sort(delegate (ob a, ob b) { return a.prn.CompareTo(b.prn); }); nbits = i; } @@ -600,29 +671,29 @@ namespace Plane.Util foreach (var ob in obs) { - var lam1 = CLIGHT/FREQ1; + var lam1 = CLIGHT / FREQ1; - var amb = (int) Math.Floor(ob.pr/PRUNIT_GPS); - var pr1 = ROUND((ob.pr - amb*PRUNIT_GPS)/0.02); - var pr1c = pr1*0.02 + amb*PRUNIT_GPS; + var amb = (int)Math.Floor(ob.pr / PRUNIT_GPS); + var pr1 = ROUND((ob.pr - amb * PRUNIT_GPS) / 0.02); + var pr1c = pr1 * 0.02 + amb * PRUNIT_GPS; - var ppr = cp_pr(ob.cp, pr1c/lam1); + var ppr = cp_pr(ob.cp, pr1c / lam1); - var ppr1 = ROUND(ppr*lam1/0.0005); + var ppr1 = ROUND(ppr * lam1 / 0.0005); setbitu(buffer, i, 6, ob.prn); i += 6; setbitu(buffer, i, 1, 0); i += 1; - setbitu(buffer, i, 24, (uint) pr1); + setbitu(buffer, i, 24, (uint)pr1); i += 24; - setbits(buffer, i, 20, (int) ppr1); + setbits(buffer, i, 20, (int)ppr1); i += 20; setbitu(buffer, i, 7, ob.raw.lock1); i += 7; - setbitu(buffer, i, 8, (byte) amb); + setbitu(buffer, i, 8, (byte)amb); i += 8; - setbitu(buffer, i, 8, (byte) (ob.snr*4)); + setbitu(buffer, i, 8, (byte)(ob.snr * 4)); i += 8; } @@ -637,22 +708,44 @@ namespace Plane.Util public static DateTime GetFromGps(int weeknumber, double seconds) { var datum = new DateTime(1980, 1, 6, 0, 0, 0, DateTimeKind.Utc); - var week = datum.AddDays(weeknumber*7); + var week = datum.AddDays(weeknumber * 7); var time = week.AddSeconds(seconds); return time; } + public static int LeapSecondsGPS(int year, int month) + { + return LeapSecondsTAI(year, month) - 19; + } + + public static int LeapSecondsTAI(int year, int month) + { + //http://maia.usno.navy.mil/ser7/tai-utc.dat + + var yyyymm = year * 100 + month; + if (yyyymm >= 201701) return 37; + if (yyyymm >= 201507) return 36; + if (yyyymm >= 201207) return 35; + if (yyyymm >= 200901) return 34; + if (yyyymm >= 200601) return 33; + if (yyyymm >= 199901) return 32; + if (yyyymm >= 199707) return 31; + if (yyyymm >= 199601) return 30; + + return 0; + } + public static void GetFromTime(DateTime time, ref int week, ref double seconds) { var datum = new DateTime(1980, 1, 6, 0, 0, 0, DateTimeKind.Utc); var dif = time - datum; - var weeks = (int) (dif.TotalDays/7); + var weeks = (int)(dif.TotalDays / 7); week = weeks; - dif = time - datum.AddDays(weeks*7); + dif = time - datum.AddDays(weeks * 7); seconds = dif.TotalSeconds; } @@ -672,7 +765,7 @@ namespace Plane.Util var staid = getbitu(buffer, i, 12); i += 12; - var tow = getbitu(buffer, i, 30)*0.001; + var tow = getbitu(buffer, i, 30) * 0.001; i += 30; var sync = getbitu(buffer, i, 1); i += 1; @@ -681,18 +774,12 @@ namespace Plane.Util i = 24 + 64; - var week = 0; - double seconds = 0; - - // asumes current week - StaticUtils.GetFromTime(DateTime.Now, ref week, ref seconds); - - // if tow is larger than the calced curretn time, go back one week - if (tow > seconds) - week--; + var week = RTCM3.weekGuess; var gpstime = StaticUtils.GetFromGps(week, tow); + lasttow = tow; + //Console.WriteLine("> {0} {1} {2} {3,2} {4} {5} {6} {7}", gpstime.Year, gpstime.Month, gpstime.Day,gpstime.Hour, gpstime.Minute, gpstime.Second + gpstime.Millisecond/1000.0, 0, nsat); for (var a = 0; a < nsat; a++) @@ -702,47 +789,47 @@ namespace Plane.Util ob.tow = tow; ob.week = week; - ob.raw.prn = (byte) getbitu(buffer, i, 6); + ob.raw.prn = (byte)getbitu(buffer, i, 6); i += 6; - ob.raw.code1 = (byte) getbitu(buffer, i, 1); + ob.raw.code1 = (byte)getbitu(buffer, i, 1); i += 1; ob.raw.pr1 = getbitu(buffer, i, 24); i += 24; ob.raw.ppr1 = getbits(buffer, i, 20); i += 20; - ob.raw.lock1 = (byte) getbitu(buffer, i, 7); + ob.raw.lock1 = (byte)getbitu(buffer, i, 7); i += 7; - ob.raw.amb = (byte) getbitu(buffer, i, 8); + ob.raw.amb = (byte)getbitu(buffer, i, 8); i += 8; - ob.raw.cnr1 = (byte) getbitu(buffer, i, 8); + ob.raw.cnr1 = (byte)getbitu(buffer, i, 8); i += 8; - ob.raw.code2 = (byte) getbitu(buffer, i, 2); + ob.raw.code2 = (byte)getbitu(buffer, i, 2); i += 2; ob.raw.pr21 = getbits(buffer, i, 14); i += 14; ob.raw.ppr2 = getbits(buffer, i, 20); i += 20; - ob.raw.lock2 = (byte) getbitu(buffer, i, 7); + ob.raw.lock2 = (byte)getbitu(buffer, i, 7); i += 7; - ob.raw.cnr2 = (byte) getbitu(buffer, i, 8); + ob.raw.cnr2 = (byte)getbitu(buffer, i, 8); i += 8; - var pr1 = ob.raw.pr1*0.02 + ob.raw.amb*PRUNIT_GPS; + var pr1 = ob.raw.pr1 * 0.02 + ob.raw.amb * PRUNIT_GPS; - var lam1 = CLIGHT/FREQ1; - var lam2 = CLIGHT/FREQ2; + var lam1 = CLIGHT / FREQ1; + var lam2 = CLIGHT / FREQ2; - var cp1 = ob.raw.ppr1*0.0005/lam1; + var cp1 = ob.raw.ppr1 * 0.0005 / lam1; - if ((uint) ob.raw.ppr1 != 0xFFF80000) + if ((uint)ob.raw.ppr1 != 0xFFF80000) { ob.prn = ob.raw.prn; - ob.cp = pr1/lam1 + cp1; + ob.cp = pr1 / lam1 + cp1; ob.pr = pr1; - ob.snr = (byte) (ob.raw.cnr1*0.25); // *4.0+0.5 + ob.snr = (byte)(ob.raw.cnr1 * 0.25); // *4.0+0.5 - ob.pr2 = pr1 + ob.raw.pr21*0.02; - ob.cp2 = pr1/lam2 + ob.raw.ppr2*0.0005/lam2; + ob.pr2 = pr1 + ob.raw.pr21 * 0.02; + ob.cp2 = pr1 / lam2 + ob.raw.ppr2 * 0.0005 / lam2; obs.Add(ob); @@ -751,7 +838,7 @@ namespace Plane.Util } } - obs.Sort(delegate(ob a, ob b) { return a.prn.CompareTo(b.prn); }); + obs.Sort(delegate (ob a, ob b) { return a.prn.CompareTo(b.prn); }); nbits = i; } @@ -762,28 +849,28 @@ namespace Plane.Util foreach (var ob in obs) { - var lam1 = CLIGHT/FREQ1; + var lam1 = CLIGHT / FREQ1; - var amb = (int) Math.Floor(ob.pr/PRUNIT_GPS); - var pr1 = ROUND((ob.pr - amb*PRUNIT_GPS)/0.02); - var pr1c = pr1*0.02 + amb*PRUNIT_GPS; + var amb = (int)Math.Floor(ob.pr / PRUNIT_GPS); + var pr1 = ROUND((ob.pr - amb * PRUNIT_GPS) / 0.02); + var pr1c = pr1 * 0.02 + amb * PRUNIT_GPS; - var ppr = cp_pr(ob.cp, pr1c/lam1); - var ppr1 = ROUND(ppr*lam1/0.0005); + var ppr = cp_pr(ob.cp, pr1c / lam1); + var ppr1 = ROUND(ppr * lam1 / 0.0005); setbitu(buffer, i, 6, ob.prn); i += 6; setbitu(buffer, i, 1, 0); i += 1; - setbitu(buffer, i, 24, (uint) pr1); + setbitu(buffer, i, 24, (uint)pr1); i += 24; - setbits(buffer, i, 20, (int) ppr1); + setbits(buffer, i, 20, (int)ppr1); i += 20; setbitu(buffer, i, 7, ob.raw.lock1); i += 7; - setbitu(buffer, i, 8, (byte) amb); + setbitu(buffer, i, 8, (byte)amb); i += 8; - setbitu(buffer, i, 8, (byte) (ob.snr*4)); + setbitu(buffer, i, 8, (byte)(ob.snr * 4)); i += 8; // l2 - all 0's setbitu(buffer, i, 2, ob.raw.code2); @@ -829,25 +916,15 @@ namespace Plane.Util var smoothint = getbitu(buffer, i, 3); i += 3; - // WIP - int WIP; + var week = RTCM3.weekGuess; - var week = 0; - double seconds = 0; + var gpstime = StaticUtils.GetFromGps(week, tow - 10800 - (60 * 60 * 24)); - // asumes current week - StaticUtils.GetFromTime(DateTime.Now, ref week, ref seconds); + gpstime = gpstime.AddSeconds(StaticUtils.LeapSecondsGPS(gpstime.Year, gpstime.Month)); - // if tow is larger than the calced curretn time, go back one week - if (tow > seconds) - week--; + lasttow = tow; - // 3hrs - tow += 10800; - - var gpstime = StaticUtils.GetFromGps(week, tow); - - Console.WriteLine("> {0} {1} {2} {3,2} {4} {5} {6} {7}", gpstime.Year, gpstime.Month, gpstime.Day,gpstime.Hour, gpstime.Minute, gpstime.Second + gpstime.Millisecond/1000.0, 0, nsat); + Console.WriteLine("> {0} {1} {2} {3,2} {4} {5} {6} {7}", gpstime.Year, gpstime.Month, gpstime.Day, gpstime.Hour, gpstime.Minute, gpstime.Second + gpstime.Millisecond / 1000.0, 0, nsat); for (var a = 0; a < nsat; a++) { @@ -902,8 +979,8 @@ namespace Plane.Util obs.Add(ob); - Console.WriteLine("R{0,2} {1,13} {2,15}0{3,15} {4,15}0{5,15}", ob.prn, ob.pr.ToString("0.000"), ob.cp.ToString("0.000"), ob.snr.ToString("0.000"), - ob.pr2.ToString("0.000"), ob.cp2.ToString("0.000")); + Console.WriteLine("R{0,2} {1,13} {2,15}0{3,15} {4,15}0{5,15}", ob.prn, ob.pr.ToString("0.000"), ob.cp.ToString("0.000"), ob.snr.ToString("0.000"), + ob.pr2.ToString("0.000"), ob.cp2.ToString("0.000")); } } @@ -918,14 +995,14 @@ namespace Plane.Util foreach (var ob in obs) { - var lam1 = CLIGHT/FREQ1; + var lam1 = CLIGHT / FREQ1; - var amb = (int) Math.Floor(ob.pr/PRUNIT_GPS); - var pr1 = ROUND((ob.pr - amb*PRUNIT_GPS)/0.02); - var pr1c = pr1*0.02 + amb*PRUNIT_GPS; + var amb = (int)Math.Floor(ob.pr / PRUNIT_GPS); + var pr1 = ROUND((ob.pr - amb * PRUNIT_GPS) / 0.02); + var pr1c = pr1 * 0.02 + amb * PRUNIT_GPS; - var ppr = cp_pr(ob.cp, pr1c/lam1); - var ppr1 = ROUND(ppr*lam1/0.0005); + var ppr = cp_pr(ob.cp, pr1c / lam1); + var ppr1 = ROUND(ppr * lam1 / 0.0005); } @@ -940,6 +1017,7 @@ namespace Plane.Util public double cp2; public double pr; public double pr2; + public byte prn; public rawrtcm raw = new rawrtcm(); public byte snr; @@ -970,7 +1048,7 @@ namespace Plane.Util public uint nbits; public List obs = new List(); - public void Read(byte[] buffer) + public virtual void Read(byte[] buffer) { uint i = 24; @@ -1028,20 +1106,13 @@ namespace Plane.Util // end of header i=202 - var week = 0; - double seconds = 0; - - // asumes current week - StaticUtils.GetFromTime(DateTime.Now, ref week, ref seconds); - - // if tow is larger than the calced curretn time, go back one week - if (tow > seconds) - week--; + var week = RTCM3.weekGuess; var gpstime = StaticUtils.GetFromGps(week, tow); - //Console.WriteLine("> {0,4} {1,2} {2,2} {3,2} {4,2} {5,10} {6,2} {7,2}", gpstime.Year, gpstime.Month,gpstime.Day, gpstime.Hour, gpstime.Minute, gpstime.Second + gpstime.Millisecond/1000.0, 0, nsat); + lasttow = tow; + //Console.WriteLine("> {0,4} {1,2} {2,2} {3,2} {4,2} {5,10} {6,2} {7,2}", gpstime.Year, gpstime.Month,gpstime.Day, gpstime.Hour, gpstime.Minute, gpstime.Second + gpstime.Millisecond/1000.0, 0, nsat); var r = new double[64]; var rr = new double[64]; @@ -1104,11 +1175,15 @@ namespace Plane.Util for (j = 0; j < ncell; j++) { /* cnr */ - cnr[j] = getbitu(buffer, i, 6)*1;// * 0.0625; + cnr[j] = getbitu(buffer, i, 6) * 1;// * 0.0625; i += 6; } var lam1 = CLIGHT / FREQ1; + var lam2 = CLIGHT / FREQ2; + + var sig = 0; + var cellmaskused = 0; for (j = 0; j < nsat; j++) { @@ -1119,14 +1194,35 @@ namespace Plane.Util ob.prn = (byte)sats[j]; - ob.pr = r[j] + pr[j]; - ob.cp = (r[j] + cp[j]) / lam1; - ob.snr = (byte)(cnr[j]); - - if (nsig > 1) + for (int k = 0; k < nsig; k++) { - ob.pr2 = r[j] + pr[j + sats.Count * 1]; - ob.cp2 = (r[j] + cp[j + sats.Count * 1]); // / lam2; + if (cellmask[sig] == 1) + { + if (sigs[k] >= 1 && sigs[k] <= 13 && sigs[k] != 10) + { + ob.pr = r[j] + pr[cellmaskused]; + ob.cp = (r[j] + cp[cellmaskused]); + ob.snr = (byte)(cnr[cellmaskused]); + } + else if (sigs[k] >= 14 && sigs[k] <= 23 || sigs[k] == 10) + { + ob.pr2 = r[j] + pr[cellmaskused]; + ob.cp2 = (r[j] + cp[cellmaskused]); + } + else if (sigs[k] >= 24 && sigs[k] <= 26) + { + //ob.pr5 = r[j] + pr[sig]; + //ob.cp5 = (r[j] + cp[sig]); + } + else + { + + } + + cellmaskused++; + } + + sig++; } obs.Add(ob); @@ -1150,7 +1246,7 @@ namespace Plane.Util public uint nbits; public List obs = new List(); - public void Read(byte[] buffer) + public virtual void Read(byte[] buffer) { uint i = 24; @@ -1159,7 +1255,7 @@ namespace Plane.Util var staid = getbitu(buffer, i, 12); i += 12; - var tow = getbitu(buffer, i, 30)*0.001; + var tow = getbitu(buffer, i, 30) * 0.001; i += 30; var sync = getbitu(buffer, i, 1); i += 1; @@ -1199,27 +1295,21 @@ namespace Plane.Util if (mask > 0) sigs[nsig++] = j; } - for (j = 0; j < nsat*nsig; j++) + for (j = 0; j < nsat * nsig; j++) { - cellmask[j] = (byte) getbitu(buffer, i, 1); + cellmask[j] = (byte)getbitu(buffer, i, 1); i += 1; if (cellmask[j] > 0) ncell++; } // end of header i=202 - var week = 0; - double seconds = 0; - - // asumes current week - StaticUtils.GetFromTime(DateTime.Now, ref week, ref seconds); - - // if tow is larger than the calced curretn time, go back one week - if (tow > seconds) - week--; + var week = RTCM3.weekGuess; var gpstime = StaticUtils.GetFromGps(week, tow); + lasttow = tow; + //Console.WriteLine("> {0,4} {1,2} {2,2} {3,2} {4,2} {5,10} {6,2} {7,2}", gpstime.Year, gpstime.Month,gpstime.Day, gpstime.Hour, gpstime.Minute, gpstime.Second + gpstime.Millisecond/1000.0, 0, nsat); @@ -1246,7 +1336,7 @@ namespace Plane.Util /* range */ var rng = getbitu(buffer, i, 8); i += 8; - if (rng != 255) r[j] = rng*RANGE_MS; + if (rng != 255) r[j] = rng * RANGE_MS; } for (j = 0; j < nsat; j++) { @@ -1258,14 +1348,14 @@ namespace Plane.Util { var rng_m = getbitu(buffer, i, 10); i += 10; - if (r[j] != 0.0) r[j] += rng_m*P2_10*RANGE_MS; + if (r[j] != 0.0) r[j] += rng_m * P2_10 * RANGE_MS; } for (j = 0; j < nsat; j++) { /* phaserangerate */ var rate = getbits(buffer, i, 14); i += 14; - if (rate != -8192) rr[j] = rate*1.0; + if (rate != -8192) rr[j] = rate * 1.0; } /* decode signal data */ for (j = 0; j < ncell; j++) @@ -1273,14 +1363,14 @@ namespace Plane.Util /* pseudorange */ var prv = getbits(buffer, i, 20); i += 20; - if (prv != -524288) pr[j] = prv*P2_29*RANGE_MS; + if (prv != -524288) pr[j] = prv * P2_29 * RANGE_MS; } for (j = 0; j < ncell; j++) { /* phaserange */ var cpv = getbits(buffer, i, 24); i += 24; - if (cpv != -8388608) cp[j] = cpv*P2_31*RANGE_MS; + if (cpv != -8388608) cp[j] = cpv * P2_31 * RANGE_MS; } for (j = 0; j < ncell; j++) { @@ -1297,7 +1387,7 @@ namespace Plane.Util for (j = 0; j < ncell; j++) { /* cnr */ - cnr[j] = getbitu(buffer, i, 10)*0.0625; + cnr[j] = getbitu(buffer, i, 10) * 0.0625; i += 10; } for (j = 0; j < ncell; j++) @@ -1305,10 +1395,12 @@ namespace Plane.Util /* phaserangerate */ var rrv = getbits(buffer, i, 15); i += 15; - if (rrv != -16384) rrf[j] = rrv*0.0001; + if (rrv != -16384) rrf[j] = rrv * 0.0001; } - var lam1 = CLIGHT/FREQ1; + var lam1 = CLIGHT / FREQ1; + var sig = 0; + var cellmaskused = 0; for (j = 0; j < nsat; j++) { @@ -1317,22 +1409,43 @@ namespace Plane.Util ob.tow = tow; ob.week = week; - ob.prn = (byte) sats[j]; + ob.prn = (byte)sats[j]; - ob.pr = r[j] + pr[j]; - ob.cp = (r[j] + cp[j])/lam1; - ob.snr = (byte) (cnr[j]); - - if (nsig > 1) + for (int k = 0; k < nsig; k++) { - ob.pr2 = r[j] + pr[j + sats.Count*1]; - ob.cp2 = (r[j] + cp[j + sats.Count*1]); // / lam2; + if (cellmask[sig] == 1) + { + if (sigs[k] >= 1 && sigs[k] <= 13) + { + ob.pr = r[j] + pr[cellmaskused]; + ob.cp = (r[j] + cp[cellmaskused]); + ob.snr = (byte)(cnr[cellmaskused]); + } + else if (sigs[k] >= 14 && sigs[k] <= 23) + { + ob.pr2 = r[j] + pr[cellmaskused]; + ob.cp2 = (r[j] + cp[cellmaskused]); + } + else if (sigs[k] >= 24 && sigs[k] <= 26) + { + //ob.pr5 = r[j] + pr[sig]; + //ob.cp5 = (r[j] + cp[sig]); + } + else + { + + } + + cellmaskused++; + } + + sig++; } obs.Add(ob); } - obs.Sort(delegate(ob a1, ob b) { return a1.prn.CompareTo(b.prn); }); + obs.Sort(delegate (ob a1, ob b) { return a1.prn.CompareTo(b.prn); }); nbits = i; } @@ -1344,766 +1457,110 @@ namespace Plane.Util } } - public class type1084 + + public class type1084 : type1074 { - public uint nbits; - public List obs = new List(); - - public void Read(byte[] buffer) + public override void Read(byte[] buffer) { - uint i = 24; + base.Read(buffer); - var type = getbitu(buffer, i, 12); - i += 12; - - var staid = getbitu(buffer, i, 12); - i += 12; - // wrong - var tow = getbitu(buffer, i, 30) * 0.001; - i += 30; - var sync = getbitu(buffer, i, 1); - i += 1; - var iod = getbitu(buffer, i, 3); - i += 3; - - var time_s = getbitu(buffer, i, 7); - i += 7; - var clk_str = getbitu(buffer, i, 2); - i += 2; - var clk_ext = getbitu(buffer, i, 2); - i += 2; - var smooth = getbitu(buffer, i, 1); - i += 1; - var tint_s = getbitu(buffer, i, 3); - i += 3; - - var nsat = 0; - var nsig = 0; - var ncell = 0; - var j = 0; - - var sats = new Dictionary(); - var sigs = new Dictionary(); - var cellmask = new byte[64]; - - for (j = 1; j <= 64; j++) + foreach (var ob in obs) { - var mask = getbitu(buffer, i, 1); - i += 1; - if (mask > 0) sats[nsat++] = j; - } - for (j = 1; j <= 32; j++) - { - var mask = getbitu(buffer, i, 1); - i += 1; - if (mask > 0) sigs[nsig++] = j; - } - - for (j = 0; j < nsat * nsig; j++) - { - cellmask[j] = (byte)getbitu(buffer, i, 1); - i += 1; - if (cellmask[j] > 0) ncell++; - } - - // end of header i=202 - - var week = 0; - double seconds = 0; - - // asumes current week - StaticUtils.GetFromTime(DateTime.Now, ref week, ref seconds); - - // if tow is larger than the calced curretn time, go back one week - if (tow > seconds) - week--; - - var gpstime = StaticUtils.GetFromGps(week, tow); - - //Console.WriteLine("> {0,4} {1,2} {2,2} {3,2} {4,2} {5,10} {6,2} {7,2}", gpstime.Year, gpstime.Month,gpstime.Day, gpstime.Hour, gpstime.Minute, gpstime.Second + gpstime.Millisecond/1000.0, 0, nsat); - - - var r = new double[64]; - var rr = new double[64]; - var pr = new double[64]; - var cp = new double[64]; - var rrf = new double[64]; - var cnr = new double[64]; - var ex = new uint[64]; - var half = new uint[64]; - var @lock = new uint[64]; - - for (j = 0; j < nsat; j++) - { - r[j] = rr[j] = 0.0; - ex[j] = 15; - } - for (j = 0; j < ncell; j++) pr[j] = cp[j] = rrf[j] = -1E16; - - /* decode satellite data */ - for (j = 0; j < nsat; j++) - { - /* range */ - var rng = getbitu(buffer, i, 8); - i += 8; - if (rng != 255) r[j] = rng * RANGE_MS; - } - for (j = 0; j < nsat; j++) - { - var rng_m = getbitu(buffer, i, 10); - i += 10; - if (r[j] != 0.0) r[j] += rng_m * P2_10 * RANGE_MS; - } - /* decode signal data */ - for (j = 0; j < ncell; j++) - { - /* pseudorange */ - var prv = getbits(buffer, i, 15); - i += 15; - if (prv != -16384) pr[j] = prv * P2_24 * RANGE_MS; - } - for (j = 0; j < ncell; j++) - { - /* phaserange */ - var cpv = getbits(buffer, i, 22); - i += 22; - if (cpv != -2097152) cp[j] = cpv * P2_29 * RANGE_MS; - } - for (j = 0; j < ncell; j++) - { - /* lock time */ - @lock[j] = getbitu(buffer, i, 4); - i += 4; - } - for (j = 0; j < ncell; j++) - { - /* half-cycle amiguity */ - half[j] = getbitu(buffer, i, 1); - i += 1; - } - for (j = 0; j < ncell; j++) - { - /* cnr */ - cnr[j] = getbitu(buffer, i, 6) * 1;// * 0.0625; - i += 6; - } - - var lam1 = CLIGHT / FREQ1; - - for (j = 0; j < nsat; j++) - { - var ob = new ob(); ob.sys = 'R'; - ob.tow = tow; - ob.week = week; - - ob.prn = (byte)sats[j]; - - ob.pr = r[j] + pr[j]; - ob.cp = (r[j] + cp[j]) / lam1; - ob.snr = (byte)(cnr[j]); - - if (nsig > 1) - { - ob.pr2 = r[j] + pr[j + sats.Count * 1]; - ob.cp2 = (r[j] + cp[j + sats.Count * 1]); // / lam2; - } - - obs.Add(ob); } - - obs.Sort(delegate (ob a1, ob b) { return a1.prn.CompareTo(b.prn); }); - - nbits = i; - } - - public - uint Write(byte[] buffer) - { - return 0; } } - - public class type1087 + public class type1087 : type1077 { - public uint nbits; - public List obs = new List(); - - public void Read(byte[] buffer) + public override void Read(byte[] buffer) { - uint i = 24; + base.Read(buffer); - var type = getbitu(buffer, i, 12); - i += 12; - - var staid = getbitu(buffer, i, 12); - i += 12; - // wrong - var tow = getbitu(buffer, i, 30) * 0.001; - i += 30; - var sync = getbitu(buffer, i, 1); - i += 1; - var iod = getbitu(buffer, i, 3); - i += 3; - - var time_s = getbitu(buffer, i, 7); - i += 7; - var clk_str = getbitu(buffer, i, 2); - i += 2; - var clk_ext = getbitu(buffer, i, 2); - i += 2; - var smooth = getbitu(buffer, i, 1); - i += 1; - var tint_s = getbitu(buffer, i, 3); - i += 3; - - var nsat = 0; - var nsig = 0; - var ncell = 0; - var j = 0; - - var sats = new Dictionary(); - var sigs = new Dictionary(); - var cellmask = new byte[64]; - - for (j = 1; j <= 64; j++) + foreach (var ob in obs) { - var mask = getbitu(buffer, i, 1); - i += 1; - if (mask > 0) sats[nsat++] = j; - } - for (j = 1; j <= 32; j++) - { - var mask = getbitu(buffer, i, 1); - i += 1; - if (mask > 0) sigs[nsig++] = j; - } - - for (j = 0; j < nsat * nsig; j++) - { - cellmask[j] = (byte)getbitu(buffer, i, 1); - i += 1; - if (cellmask[j] > 0) ncell++; - } - - // end of header i=202 - - var week = 0; - double seconds = 0; - - // asumes current week - StaticUtils.GetFromTime(DateTime.Now, ref week, ref seconds); - - // if tow is larger than the calced curretn time, go back one week - if (tow > seconds) - week--; - - var gpstime = StaticUtils.GetFromGps(week, tow); - - //Console.WriteLine("> {0,4} {1,2} {2,2} {3,2} {4,2} {5,10} {6,2} {7,2}", gpstime.Year, gpstime.Month,gpstime.Day, gpstime.Hour, gpstime.Minute, gpstime.Second + gpstime.Millisecond/1000.0, 0, nsat); - - - var r = new double[64]; - var rr = new double[64]; - var pr = new double[64]; - var cp = new double[64]; - var rrf = new double[64]; - var cnr = new double[64]; - var ex = new uint[64]; - var half = new uint[64]; - var @lock = new uint[64]; - - for (j = 0; j < nsat; j++) - { - r[j] = rr[j] = 0.0; - ex[j] = 15; - } - for (j = 0; j < ncell; j++) pr[j] = cp[j] = rrf[j] = -1E16; - - /* decode satellite data */ - for (j = 0; j < nsat; j++) - { - /* range */ - var rng = getbitu(buffer, i, 8); - i += 8; - if (rng != 255) r[j] = rng * RANGE_MS; - } - for (j = 0; j < nsat; j++) - { - /* extended info */ - ex[j] = getbitu(buffer, i, 4); - i += 4; - } - for (j = 0; j < nsat; j++) - { - var rng_m = getbitu(buffer, i, 10); - i += 10; - if (r[j] != 0.0) r[j] += rng_m * P2_10 * RANGE_MS; - } - for (j = 0; j < nsat; j++) - { - /* phaserangerate */ - var rate = getbits(buffer, i, 14); - i += 14; - if (rate != -8192) rr[j] = rate * 1.0; - } - /* decode signal data */ - for (j = 0; j < ncell; j++) - { - /* pseudorange */ - var prv = getbits(buffer, i, 20); - i += 20; - if (prv != -524288) pr[j] = prv * P2_29 * RANGE_MS; - } - for (j = 0; j < ncell; j++) - { - /* phaserange */ - var cpv = getbits(buffer, i, 24); - i += 24; - if (cpv != -8388608) cp[j] = cpv * P2_31 * RANGE_MS; - } - for (j = 0; j < ncell; j++) - { - /* lock time */ - @lock[j] = getbitu(buffer, i, 10); - i += 10; - } - for (j = 0; j < ncell; j++) - { - /* half-cycle amiguity */ - half[j] = getbitu(buffer, i, 1); - i += 1; - } - for (j = 0; j < ncell; j++) - { - /* cnr */ - cnr[j] = getbitu(buffer, i, 10) * 0.0625; - i += 10; - } - for (j = 0; j < ncell; j++) - { - /* phaserangerate */ - var rrv = getbits(buffer, i, 15); - i += 15; - if (rrv != -16384) rrf[j] = rrv * 0.0001; - } - - var lam1 = CLIGHT / FREQ1; - - for (j = 0; j < nsat; j++) - { - var ob = new ob(); ob.sys = 'R'; - ob.tow = tow; - ob.week = week; - - ob.prn = (byte)sats[j]; - - ob.pr = r[j] + pr[j]; - ob.cp = (r[j] + cp[j]) / lam1; - ob.snr = (byte)(cnr[j]); - - if (nsig > 1) - { - ob.pr2 = r[j] + pr[j + sats.Count * 1]; - ob.cp2 = (r[j] + cp[j + sats.Count * 1]); // / lam2; - } - - obs.Add(ob); } - - obs.Sort(delegate (ob a1, ob b) { return a1.prn.CompareTo(b.prn); }); - - nbits = i; - } - - public - uint Write(byte[] buffer) - { - return 0; } } - public class type1124 + public class type1094 : type1084 { - public uint nbits; - public List obs = new List(); - - public void Read(byte[] buffer) + public override void Read(byte[] buffer) { - uint i = 24; + base.Read(buffer); - var type = getbitu(buffer, i, 12); - i += 12; - - var staid = getbitu(buffer, i, 12); - i += 12; - var tow = getbitu(buffer, i, 30) * 0.001 + 14; - i += 30; - var sync = getbitu(buffer, i, 1); - i += 1; - var iod = getbitu(buffer, i, 3); - i += 3; - - var time_s = getbitu(buffer, i, 7); - i += 7; - var clk_str = getbitu(buffer, i, 2); - i += 2; - var clk_ext = getbitu(buffer, i, 2); - i += 2; - var smooth = getbitu(buffer, i, 1); - i += 1; - var tint_s = getbitu(buffer, i, 3); - i += 3; - - var nsat = 0; - var nsig = 0; - var ncell = 0; - var j = 0; - - var sats = new Dictionary(); - var sigs = new Dictionary(); - var cellmask = new byte[64]; - - for (j = 1; j <= 64; j++) + foreach (var ob in obs) { - var mask = getbitu(buffer, i, 1); - i += 1; - if (mask > 0) sats[nsat++] = j; + ob.sys = 'E'; } - for (j = 1; j <= 32; j++) - { - var mask = getbitu(buffer, i, 1); - i += 1; - if (mask > 0) sigs[nsig++] = j; - } - - for (j = 0; j < nsat * nsig; j++) - { - cellmask[j] = (byte)getbitu(buffer, i, 1); - i += 1; - if (cellmask[j] > 0) ncell++; - } - - // end of header i=202 - - var week = 0; - double seconds = 0; - - // asumes current week - StaticUtils.GetFromTime(DateTime.Now, ref week, ref seconds); - - // if tow is larger than the calced curretn time, go back one week - if (tow > seconds) - week--; - - var gpstime = StaticUtils.GetFromGps(week, tow); - - //Console.WriteLine("> {0,4} {1,2} {2,2} {3,2} {4,2} {5,10} {6,2} {7,2}", gpstime.Year, gpstime.Month,gpstime.Day, gpstime.Hour, gpstime.Minute, gpstime.Second + gpstime.Millisecond/1000.0, 0, nsat); - - - var r = new double[64]; - var rr = new double[64]; - var pr = new double[64]; - var cp = new double[64]; - var rrf = new double[64]; - var cnr = new double[64]; - var ex = new uint[64]; - var half = new uint[64]; - var @lock = new uint[64]; - - for (j = 0; j < nsat; j++) - { - r[j] = rr[j] = 0.0; - ex[j] = 15; - } - for (j = 0; j < ncell; j++) pr[j] = cp[j] = rrf[j] = -1E16; - - /* decode satellite data */ - for (j = 0; j < nsat; j++) - { - /* range */ - var rng = getbitu(buffer, i, 8); - i += 8; - if (rng != 255) r[j] = rng * RANGE_MS; - } - for (j = 0; j < nsat; j++) - { - var rng_m = getbitu(buffer, i, 10); - i += 10; - if (r[j] != 0.0) r[j] += rng_m * P2_10 * RANGE_MS; - } - /* decode signal data */ - for (j = 0; j < ncell; j++) - { - /* pseudorange */ - var prv = getbits(buffer, i, 15); - i += 15; - if (prv != -16384) pr[j] = prv * P2_24 * RANGE_MS; - } - for (j = 0; j < ncell; j++) - { - /* phaserange */ - var cpv = getbits(buffer, i, 22); - i += 22; - if (cpv != -2097152) cp[j] = cpv * P2_29 * RANGE_MS; - } - for (j = 0; j < ncell; j++) - { - /* lock time */ - @lock[j] = getbitu(buffer, i, 4); - i += 4; - } - for (j = 0; j < ncell; j++) - { - /* half-cycle amiguity */ - half[j] = getbitu(buffer, i, 1); - i += 1; - } - for (j = 0; j < ncell; j++) - { - /* cnr */ - cnr[j] = getbitu(buffer, i, 6) * 1;// * 0.0625; - i += 6; - } - - var lam1 = CLIGHT / FREQ1; - - for (j = 0; j < nsat; j++) - { - var ob = new ob(); - ob.sys = 'C'; - ob.tow = tow; - ob.week = week; - - ob.prn = (byte)sats[j]; - - ob.pr = r[j] + pr[j]; - ob.cp = (r[j] + cp[j]) / lam1; - ob.snr = (byte)(cnr[j]); - - if (nsig > 1) - { - ob.pr2 = r[j] + pr[j + sats.Count * 1]; - ob.cp2 = (r[j] + cp[j + sats.Count * 1]); // / lam2; - } - - obs.Add(ob); - } - - obs.Sort(delegate (ob a1, ob b) { return a1.prn.CompareTo(b.prn); }); - - nbits = i; - } - - public - uint Write(byte[] buffer) - { - return 0; } } - - public class type1127 + public class type1097 : type1087 { - public uint nbits; - public List obs = new List(); - - public void Read(byte[] buffer) + public override void Read(byte[] buffer) { - uint i = 24; + base.Read(buffer); - var type = getbitu(buffer, i, 12); - i += 12; - - var staid = getbitu(buffer, i, 12); - i += 12; - var tow = getbitu(buffer, i, 30) * 0.001 + 14; - i += 30; - var sync = getbitu(buffer, i, 1); - i += 1; - var iod = getbitu(buffer, i, 3); - i += 3; - - var time_s = getbitu(buffer, i, 7); - i += 7; - var clk_str = getbitu(buffer, i, 2); - i += 2; - var clk_ext = getbitu(buffer, i, 2); - i += 2; - var smooth = getbitu(buffer, i, 1); - i += 1; - var tint_s = getbitu(buffer, i, 3); - i += 3; - - var nsat = 0; - var nsig = 0; - var ncell = 0; - var j = 0; - - var sats = new Dictionary(); - var sigs = new Dictionary(); - var cellmask = new byte[64]; - - for (j = 1; j <= 64; j++) + foreach (var ob in obs) { - var mask = getbitu(buffer, i, 1); - i += 1; - if (mask > 0) sats[nsat++] = j; + ob.sys = 'E'; } - for (j = 1; j <= 32; j++) - { - var mask = getbitu(buffer, i, 1); - i += 1; - if (mask > 0) sigs[nsig++] = j; - } - - for (j = 0; j < nsat * nsig; j++) - { - cellmask[j] = (byte)getbitu(buffer, i, 1); - i += 1; - if (cellmask[j] > 0) ncell++; - } - - // end of header i=202 - - var week = 0; - double seconds = 0; - - // asumes current week - StaticUtils.GetFromTime(DateTime.Now, ref week, ref seconds); - - // if tow is larger than the calced curretn time, go back one week - if (tow > seconds) - week--; - - var gpstime = StaticUtils.GetFromGps(week, tow); - - //Console.WriteLine("> {0,4} {1,2} {2,2} {3,2} {4,2} {5,10} {6,2} {7,2}", gpstime.Year, gpstime.Month,gpstime.Day, gpstime.Hour, gpstime.Minute, gpstime.Second + gpstime.Millisecond/1000.0, 0, nsat); - - - var r = new double[64]; - var rr = new double[64]; - var pr = new double[64]; - var cp = new double[64]; - var rrf = new double[64]; - var cnr = new double[64]; - var ex = new uint[64]; - var half = new uint[64]; - var @lock = new uint[64]; - - for (j = 0; j < nsat; j++) - { - r[j] = rr[j] = 0.0; - ex[j] = 15; - } - for (j = 0; j < ncell; j++) pr[j] = cp[j] = rrf[j] = -1E16; - - /* decode satellite data */ - for (j = 0; j < nsat; j++) - { - /* range */ - var rng = getbitu(buffer, i, 8); - i += 8; - if (rng != 255) r[j] = rng * RANGE_MS; - } - for (j = 0; j < nsat; j++) - { - /* extended info */ - ex[j] = getbitu(buffer, i, 4); - i += 4; - } - for (j = 0; j < nsat; j++) - { - var rng_m = getbitu(buffer, i, 10); - i += 10; - if (r[j] != 0.0) r[j] += rng_m * P2_10 * RANGE_MS; - } - for (j = 0; j < nsat; j++) - { - /* phaserangerate */ - var rate = getbits(buffer, i, 14); - i += 14; - if (rate != -8192) rr[j] = rate * 1.0; - } - /* decode signal data */ - for (j = 0; j < ncell; j++) - { - /* pseudorange */ - var prv = getbits(buffer, i, 20); - i += 20; - if (prv != -524288) pr[j] = prv * P2_29 * RANGE_MS; - } - for (j = 0; j < ncell; j++) - { - /* phaserange */ - var cpv = getbits(buffer, i, 24); - i += 24; - if (cpv != -8388608) cp[j] = cpv * P2_31 * RANGE_MS; - } - for (j = 0; j < ncell; j++) - { - /* lock time */ - @lock[j] = getbitu(buffer, i, 10); - i += 10; - } - for (j = 0; j < ncell; j++) - { - /* half-cycle amiguity */ - half[j] = getbitu(buffer, i, 1); - i += 1; - } - for (j = 0; j < ncell; j++) - { - /* cnr */ - cnr[j] = getbitu(buffer, i, 10) * 0.0625; - i += 10; - } - for (j = 0; j < ncell; j++) - { - /* phaserangerate */ - var rrv = getbits(buffer, i, 15); - i += 15; - if (rrv != -16384) rrf[j] = rrv * 0.0001; - } - - var lam1 = CLIGHT / FREQ1; - - for (j = 0; j < nsat; j++) - { - var ob = new ob(); - ob.sys = 'C'; - ob.tow = tow; - ob.week = week; - - ob.prn = (byte)sats[j]; - - ob.pr = r[j] + pr[j]; - ob.cp = (r[j] + cp[j]) / lam1; - ob.snr = (byte)(cnr[j]); - - if (nsig > 1) - { - ob.pr2 = r[j] + pr[j + sats.Count * 1]; - ob.cp2 = (r[j] + cp[j + sats.Count * 1]); // / lam2; - } - - obs.Add(ob); - } - - obs.Sort(delegate (ob a1, ob b) { return a1.prn.CompareTo(b.prn); }); - - nbits = i; - } - - public - uint Write(byte[] buffer) - { - return 0; } } + public class type1114 : type1084 + { + public override void Read(byte[] buffer) + { + base.Read(buffer); + + foreach (var ob in obs) + { + ob.sys = 'Q'; + } + } + } + + public class type1117 : type1087 + { + public override void Read(byte[] buffer) + { + base.Read(buffer); + + foreach (var ob in obs) + { + ob.sys = 'Q'; + } + } + } + + public class type1124 : type1084 + { + public override void Read(byte[] buffer) + { + base.Read(buffer); + + foreach (var ob in obs) + { + ob.sys = 'B'; + } + } + } + + public class type1127 : type1087 + { + public override void Read(byte[] buffer) + { + base.Read(buffer); + + foreach (var ob in obs) + { + ob.sys = 'B'; + } + } + } public class type1005 { @@ -2126,16 +1583,16 @@ namespace Plane.Util { return new[] { - rr0*0.0001, - rr1*0.0001, - rr2*0.0001 - }; + rr0*0.0001, + rr1*0.0001, + rr2*0.0001 + }; } set { - rr0 = value[0]/0.0001; - rr1 = value[1]/0.0001; - rr2 = value[2]/0.0001; + rr0 = value[0] / 0.0001; + rr1 = value[1] / 0.0001; + rr2 = value[2] / 0.0001; } } @@ -2143,9 +1600,9 @@ namespace Plane.Util { uint i = 24 + 12; - staid = (ushort) getbitu(buffer, i, 12); + staid = (ushort)getbitu(buffer, i, 12); i += 12; - itrf = (byte) getbitu(buffer, i, 6); + itrf = (byte)getbitu(buffer, i, 6); i += 6 + 4; rr0 = getbits_38(buffer, i); i += 38 + 2; @@ -2173,17 +1630,17 @@ namespace Plane.Util i += 1; /* galileo indicator */ setbitu(buffer, i, 1, 0); i += 1; /* ref station indicator */ - set38bits(buffer, i, ecefposition[0]/0.0001); + set38bits(buffer, i, ecefposition[0] / 0.0001); i += 38; /* antenna ref point ecef-x */ setbitu(buffer, i, 1, 1); i += 1; /* oscillator indicator */ setbitu(buffer, i, 1, 0); i += 1; /* reserved */ - set38bits(buffer, i, ecefposition[1]/0.0001); + set38bits(buffer, i, ecefposition[1] / 0.0001); i += 38; /* antenna ref point ecef-y */ setbitu(buffer, i, 2, 0); i += 2; /* quarter cycle indicator */ - set38bits(buffer, i, ecefposition[2]/0.0001); + set38bits(buffer, i, ecefposition[2] / 0.0001); i += 38; /* antenna ref point ecef-z */ return i; @@ -2212,16 +1669,16 @@ namespace Plane.Util { return new[] { - rr0*0.0001, - rr1*0.0001, - rr2*0.0001 - }; + rr0*0.0001, + rr1*0.0001, + rr2*0.0001 + }; } set { - rr0 = value[0]/0.0001; - rr1 = value[1]/0.0001; - rr2 = value[2]/0.0001; + rr0 = value[0] / 0.0001; + rr1 = value[1] / 0.0001; + rr2 = value[2] / 0.0001; } } @@ -2229,9 +1686,9 @@ namespace Plane.Util { uint i = 24 + 12; - staid = (ushort) getbitu(buffer, i, 12); + staid = (ushort)getbitu(buffer, i, 12); i += 12; - itrf = (byte) getbitu(buffer, i, 6); + itrf = (byte)getbitu(buffer, i, 6); i += 6 + 4; rr0 = getbits_38(buffer, i); i += 38 + 2; @@ -2239,7 +1696,7 @@ namespace Plane.Util i += 38 + 2; rr2 = getbits_38(buffer, i); i += 38; - anth = (ushort) getbitu(buffer, i, 16); + anth = (ushort)getbitu(buffer, i, 16); i += 16; } @@ -2261,24 +1718,407 @@ namespace Plane.Util i += 1; /* galileo indicator */ setbitu(buffer, i, 1, 0); i += 1; /* ref station indicator */ - set38bits(buffer, i, ecefposition[0]/0.0001); + set38bits(buffer, i, ecefposition[0] / 0.0001); i += 38; /* antenna ref point ecef-x */ setbitu(buffer, i, 1, 1); i += 1; /* oscillator indicator */ setbitu(buffer, i, 1, 0); i += 1; /* reserved */ - set38bits(buffer, i, ecefposition[1]/0.0001); + set38bits(buffer, i, ecefposition[1] / 0.0001); i += 38; /* antenna ref point ecef-y */ setbitu(buffer, i, 2, 0); i += 2; /* quarter cycle indicator */ - set38bits(buffer, i, ecefposition[2]/0.0001); + set38bits(buffer, i, ecefposition[2] / 0.0001); i += 38; /* antenna ref point ecef-z */ setbitu(buffer, i, 16, anth); i += 16; /* antenna height */ return i; } + } + public class type1019 + { + public double A; + public double af0; + public double af1; + public double af2; + public double cic; + public double cis; + public double code; + public double crc; + public double crs; + public double cuc; + public double cus; + public double deln; + public double e; + public bool fit; + public uint flag; + public double i0; + public double idot; + public uint iodc; + public uint iode; + public double M0; + public double omg; + public double OMG0; + public double OMGd; + public double prn; + public double sqrtA; + public double sva; + public uint svh; + public double tgd; + public double toc; + public double toes; + public double week; + + + public void Read(byte[] buffer) + { + uint i = 24 + 12; + + prn = getbitu(buffer, i, 6); + i += 6; + week = getbitu(buffer, i, 10); + i += 10; + sva = getbitu(buffer, i, 4); + i += 4; + code = getbitu(buffer, i, 2); + i += 2; + idot = getbits(buffer, i, 14) * P2_43 * SC2RAD; + i += 14; + iode = getbitu(buffer, i, 8); + i += 8; + toc = getbitu(buffer, i, 16) * 16.0; + i += 16; + af2 = getbits(buffer, i, 8) * P2_55; + i += 8; + af1 = getbits(buffer, i, 16) * P2_43; + i += 16; + af0 = getbits(buffer, i, 22) * P2_31; + i += 22; + iodc = getbitu(buffer, i, 10); + i += 10; + crs = getbits(buffer, i, 16) * P2_5; + i += 16; + deln = getbits(buffer, i, 16) * P2_43 * SC2RAD; + i += 16; + M0 = getbits(buffer, i, 32) * P2_31 * SC2RAD; + i += 32; + cuc = getbits(buffer, i, 16) * P2_29; + i += 16; + e = getbitu(buffer, i, 32) * P2_33; + i += 32; + cus = getbits(buffer, i, 16) * P2_29; + i += 16; + sqrtA = getbitu(buffer, i, 32) * P2_19; + i += 32; + toes = getbitu(buffer, i, 16) * 16.0; + i += 16; + cic = getbits(buffer, i, 16) * P2_29; + i += 16; + OMG0 = getbits(buffer, i, 32) * P2_31 * SC2RAD; + i += 32; + cis = getbits(buffer, i, 16) * P2_29; + i += 16; + i0 = getbits(buffer, i, 32) * P2_31 * SC2RAD; + i += 32; + crc = getbits(buffer, i, 16) * P2_5; + i += 16; + omg = getbits(buffer, i, 32) * P2_31 * SC2RAD; + i += 32; + OMGd = getbits(buffer, i, 24) * P2_43 * SC2RAD; + i += 24; + tgd = getbits(buffer, i, 8) * P2_31; + i += 8; + svh = getbitu(buffer, i, 6); + i += 6; + flag = getbitu(buffer, i, 1); + i += 1; + fit = getbitu(buffer, i, 1) > 0 ? true : false; /* 0:4hr,1:>4hr */ + + A = sqrtA * sqrtA; + } + + public uint Write(byte[] buffer) + { + uint i = 24; + + var toe = (uint)(toes / 16.0); + + setbitu(buffer, i, 12, 1019); + i += 12; + setbitu(buffer, i, 6, prn); + i += 6; + setbitu(buffer, i, 10, week % 1024); + i += 10; + setbitu(buffer, i, 4, sva); + i += 4; + setbitu(buffer, i, 2, code); + i += 2; + setbits(buffer, i, 14, idot / P2_43 / SC2RAD); + i += 14; + setbitu(buffer, i, 8, iode); + i += 8; + setbitu(buffer, i, 16, toc / 16.0); + i += 16; + setbits(buffer, i, 8, af2 / P2_55); + i += 8; + setbits(buffer, i, 16, af1 / P2_43); + i += 16; + setbits(buffer, i, 22, af0 / P2_31); + i += 22; + setbitu(buffer, i, 10, iodc); + i += 10; + setbits(buffer, i, 16, crs / P2_5); + i += 16; + setbits(buffer, i, 16, deln / P2_43 / SC2RAD); + i += 16; + setbits(buffer, i, 32, M0 / P2_31 / SC2RAD); + i += 32; + setbits(buffer, i, 16, cuc / P2_29); + i += 16; + setbitu(buffer, i, 32, e / P2_33); + i += 32; + setbits(buffer, i, 16, cus / P2_29); + i += 16; + setbitu(buffer, i, 32, sqrtA / P2_19); + i += 32; + setbitu(buffer, i, 16, toe); + i += 16; + setbits(buffer, i, 16, cic / P2_29); + i += 16; + setbits(buffer, i, 32, OMG0 / P2_31 / SC2RAD); + i += 32; + setbits(buffer, i, 16, cis / P2_29); + i += 16; + setbits(buffer, i, 32, i0 / P2_31 / SC2RAD); + i += 32; + setbits(buffer, i, 16, crc / P2_5); + i += 16; + setbits(buffer, i, 32, omg / P2_31 / SC2RAD); + i += 32; + setbits(buffer, i, 24, OMGd / P2_43 / SC2RAD); + i += 24; + setbits(buffer, i, 8, tgd / P2_31); + i += 8; + setbitu(buffer, i, 6, svh); + i += 6; + setbitu(buffer, i, 1, flag); + i += 1; + setbitu(buffer, i, 1, fit ? 1 : 0); + i += 1; + + return i; + } + + [StructLayout(LayoutKind.Sequential, Pack = 1)] + public struct gps_time_t + { + public bool Equals(gps_time_t other) + { + return tow.Equals(other.tow) && wn == other.wn; + } + + public override bool Equals(object obj) + { + //VS2022 转过来 语法 不正确 + //return obj is gps_time_t other && Equals(other); + bool ret =false; + if (obj is gps_time_t) + ret = Equals((gps_time_t)obj); + return ret; + } + + public override int GetHashCode() + { + unchecked + { + return (tow.GetHashCode() * 397) ^ wn.GetHashCode(); + } + } + + public double tow; /**< Seconds since the GPS start of week. */ + public u16 wn; /**< GPS week number. */ + + public gps_time_t(double tow, u16 wn) + { + this.tow = tow; + this.wn = wn; + } + + public override string ToString() + { + return String.Format("{0}:{1}", wn, tow); + } + + public static bool operator ==(gps_time_t ob1, gps_time_t ob2) + { + return ob1.wn == ob2.wn && ob1.tow == ob2.tow; + } + + public static bool operator !=(gps_time_t ob1, gps_time_t ob2) + { + return !(ob1 == ob2); + } + } + + private double gpsdifftime(gps_time_t end, gps_time_t beginning) + { + return (end.wn - beginning.wn) * 7 * 24 * 3600 + + end.tow - beginning.tow; + } + + public double[] pos = new double[3]; + public double[] vel = new double[3]; + public double clock_err; + public double clock_rate_err; + public gps_time_t tot; + + + public int calc_sat_pos(double[] pos, double[] vel, + ref double clock_err, ref double clock_rate_err, + gps_time_t tot) + { + if (tot == this.tot) + { + Array.Copy(this.pos, pos, 3); + Array.Copy(this.vel, vel, 3); + clock_err = this.clock_err; + clock_rate_err = this.clock_rate_err; + + return 0; + } + + double tempd1 = 0, tempd2, tempd3; + double tdiff; + double a; // semi major axis + double ma, ma_dot; // mean anomoly and first derivative (mean motion) + double ea, ea_dot, ea_old; // eccentric anomoly, first deriv, iteration var + double einstein; // relativistic correction + double al, al_dot; // argument of lattitude and first derivative + double cal, cal_dot; // corrected argument of lattitude and first deriv + double r, r_dot; // radius and first derivative + double inc, inc_dot; // inclination and first derivative + double x, x_dot, y, y_dot; // position in orbital plan and first derivatives + double om, om_dot; // omega and first derivatives + + const double NAV_OMEGAE_DOT = 7.2921151467e-005; + const double NAV_GM = 3.986005e14; + + var toc = new gps_time_t(this.toc, (u16)(week)); + var toe = new gps_time_t(toes, (u16)(week)); + + // Satellite clock terms + // Seconds from clock data reference time (toc) + tdiff = gpsdifftime(tot, toc); + + if (tdiff > 4 * 3600) + tdiff = gpsdifftime(tot, toe); + + clock_err = this.af0 + tdiff * (this.af1 + tdiff * this.af2) - this.tgd; + clock_rate_err = this.af1 + 2.0 * tdiff * this.af2; + + // Seconds from the time from ephemeris reference epoch (toe) + tdiff = gpsdifftime(tot, toe); + + // If tdiff is too large our ephemeris isn't valid, maybe we want to wait until we get a + // new one? At least let's warn the user. + // TODO: this doesn't exclude ephemerides older than a week so could be made better. + if (Math.Abs(tdiff) > 4 * 3600) + { + Console.Write(" WARNING: using ephemeris older (or newer!) than 4 hours. {0} {1} hrs \r", prn, + tdiff / 3600); + return -1; + } + + // Calculate position per IS-GPS-200D p 97 Table 20-IV + a = sqrtA * sqrtA; // [m] Semi-major axis + ma_dot = Math.Sqrt(NAV_GM / (a * a * a)) + deln; // [rad/sec] Corrected mean motion + ma = M0 + ma_dot * tdiff; // [rad] Corrected mean anomaly + + // Iteratively solve for the Eccentric Anomaly (from Keith Alter and David Johnston) + ea = ma; // Starting value for E + double ecc = e; + u32 count = 0; + + /* TODO: Implement convergence test uSing integer difference of doubles, + * http://www.cygnus-software.com/papers/comparingfloats/comparingfloats.htm */ + do + { + ea_old = ea; + tempd1 = 1.0 - ecc * Math.Cos(ea_old); + ea = ea + (ma - ea_old + ecc * Math.Sin(ea_old)) / tempd1; + count++; + if (count > 5) + break; + } while (Math.Abs(ea - ea_old) > 1.0E-14); + ea_dot = ma_dot / tempd1; + + // Relativistic correction term + einstein = -4.442807633E-10 * ecc * sqrtA * Math.Sin(ea); + + // Begin calc for True Anomaly and Argument of Latitude + tempd2 = Math.Sqrt(1.0 - ecc * ecc); + al = Math.Atan2(tempd2 * Math.Sin(ea), Math.Cos(ea) - ecc) + this.omg; + // [rad] Argument of Latitude = True Anomaly + Argument of Perigee + al_dot = tempd2 * ea_dot / tempd1; + + // Calculate corrected argument of latitude based on position + cal = al + this.cus * Math.Sin(2.0 * al) + this.cuc * Math.Cos(2.0 * al); + cal_dot = + al_dot * (1.0 + + 2.0 * (this.cus * Math.Cos(2.0 * al) - + this.cuc * Math.Sin(2.0 * al))); + + // Calculate corrected radius based on argument of latitude + r = + a * tempd1 + this.crc * Math.Cos(2.0 * al) + + this.crs * Math.Sin(2.0 * al); + r_dot = + a * ecc * Math.Sin(ea) * ea_dot + + 2.0 * al_dot * (this.crs * Math.Cos(2.0 * al) - + this.crc * Math.Sin(2.0 * al)); + + // Calculate inclination based on argument of latitude + inc = + i0 + idot * tdiff + + this.cic * Math.Cos(2.0 * al) + this.cis * Math.Sin(2.0 * al); + inc_dot = + idot + 2.0 * al_dot * (this.cis * Math.Cos(2.0 * al) - + this.cic * Math.Sin(2.0 * al)); + + // Calculate position and velocity in orbital plane + x = r * Math.Cos(cal); + y = r * Math.Sin(cal); + x_dot = r_dot * Math.Cos(cal) - y * cal_dot; + y_dot = r_dot * Math.Sin(cal) + x * cal_dot; + + // Corrected longitude of ascenting node + om_dot = OMGd - NAV_OMEGAE_DOT; + om = OMG0 + tdiff * om_dot - NAV_OMEGAE_DOT * toe.tow; + + // Compute the satellite's position in Earth-Centered Earth-Fixed coordiates + pos[0] = x * Math.Cos(om) - y * Math.Cos(inc) * Math.Sin(om); + pos[1] = x * Math.Sin(om) + y * Math.Cos(inc) * Math.Cos(om); + pos[2] = y * Math.Sin(inc); + + tempd3 = y_dot * Math.Cos(inc) - y * Math.Sin(inc) * inc_dot; + + // Compute the satellite's velocity in Earth-Centered Earth-Fixed coordiates + vel[0] = -om_dot * pos[1] + x_dot * Math.Cos(om) - tempd3 * Math.Sin(om); + vel[1] = om_dot * pos[0] + x_dot * Math.Sin(om) + tempd3 * Math.Cos(om); + vel[2] = y * Math.Cos(inc) * inc_dot + y_dot * Math.Sin(inc); + + clock_err += einstein; + + this.pos = pos; + this.vel = vel; + this.clock_err = clock_err; + this.clock_rate_err = clock_rate_err; + this.tot = tot; + + return 0; + } } } } \ No newline at end of file diff --git a/Plane.FormationCreator/Util/ubx_m8p.cs b/Plane.FormationCreator/Util/ubx_m8p.cs new file mode 100644 index 0000000..732571f --- /dev/null +++ b/Plane.FormationCreator/Util/ubx_m8p.cs @@ -0,0 +1,573 @@ +using System; + +using uint8_t = System.Byte; +using uint16_t = System.UInt16; +using int32_t = System.Int32; +using uint32_t = System.UInt32; +using int8_t = System.SByte; + +using System.Runtime.InteropServices; +using System.Threading; + +namespace Plane.Util +{ + public class Ubx : ICorrections + { + int step = 0; + + public byte[] buffer = new byte[1024 * 8]; + int payloadlen = 0; + int msglencount = 0; + + public byte @class + { + get { return buffer[2]; } + } + + public byte subclass + { + get { return buffer[3]; } + } + + public int length + { + get + { + return 2 + 2 + 2 + 2 + payloadlen; // header2, class,subclass,length2,data,crc2 + } + } + + public byte[] packet + { + get { return buffer; } + } + + public bool resetParser() + { + step = 0; + return true; + } + + public int Read(byte data) + { + switch (step) + { + default: + case 0: + if (data == 0xb5) + { + step = 1; + buffer[0] = data; + } + + break; + case 1: + if (data == 0x62) + { + buffer[1] = data; + step++; + } + else + step = 0; + + break; + case 2: + buffer[2] = data; + step++; + break; + case 3: + buffer[3] = data; + step++; + break; + case 4: + buffer[4] = data; + payloadlen = data; + step++; + break; + case 5: + buffer[5] = data; + step++; + payloadlen += (data << 8); + msglencount = 0; + // reset on oversize packet + if (payloadlen > buffer.Length) + step = 0; + break; + case 6: + if (msglencount < (payloadlen)) + { + buffer[msglencount + 6] = data; + msglencount++; + + if (msglencount == payloadlen) + step++; + } + + break; + case 7: + buffer[msglencount + 6] = data; + step++; + break; + case 8: + buffer[msglencount + 6 + 1] = data; + + var crc = ubx_checksum(buffer, payloadlen + 6); + + var crcpacket = new byte[] { buffer[msglencount + 6], data }; + + if (crc[0] == crcpacket[0] && crc[1] == crcpacket[1]) + { + step = 0; + return (@class << 8) + subclass; + } + + step = 0; + break; + } + + return -1; + } + + private static byte[] ubx_checksum(byte[] packet, int size, int offset = 2) + { + uint a = 0x00; + uint b = 0x00; + var i = offset; + while (i < size) + { + a += packet[i++]; + b += a; + } + + var ans = new byte[2]; + + ans[0] = (byte)(a & 0xFF); + ans[1] = (byte)(b & 0xFF); + + return ans; + } + + public static byte[] generate(byte cl, byte subclass, byte[] payload) + { + var data = new byte[2 + 2 + 2 + 2 + payload.Length]; + data[0] = 0xb5; + data[1] = 0x62; + data[2] = cl; + data[3] = subclass; + data[4] = (byte)(payload.Length & 0xff); + data[5] = (byte)((payload.Length >> 8) & 0xff); + + Array.ConstrainedCopy(payload, 0, data, 6, payload.Length); + + var checksum = ubx_checksum(data, data.Length - 2); + + data[data.Length - 2] = checksum[0]; + data[data.Length - 1] = checksum[1]; + + return data; + } + + [StructLayout(LayoutKind.Sequential, Pack = 1)] + public struct ubx_mon_ver + { + [MarshalAs(UnmanagedType.ByValArray, SizeConst = 30)] + public byte[] swVersion; + + [MarshalAs(UnmanagedType.ByValArray, SizeConst = 10)] + public Byte[] hwVersion; + + [MarshalAs(UnmanagedType.ByValArray, SizeConst = 30)] + public byte[] extension; + } + + [StructLayout(LayoutKind.Sequential, Pack = 1)] + public struct ubx_mon_hw + { + public int pinSel; + public int pinBank; + public int pinDir; + public int pinVal; + public ushort noisePerMS; + public ushort agcCnt; + public byte aStatus; + public byte aPower; + public byte flags; + public byte reserved1; + public int usedMask; + + [MarshalAs(UnmanagedType.ByValArray, SizeConst = 17)] + public byte[] VP; + + public byte jamInd; + public ushort reserved3; + public int pinIrq; + public int pullH; + public int pullL; + } + + [StructLayout(LayoutKind.Sequential, Pack = 1)] + public struct ubx_nav_pvt + { + public uint32_t itow; + public uint16_t year; + public uint8_t month, day, hour, min, sec; + public uint8_t valid; + public uint32_t t_acc; + public int32_t nano; + public uint8_t fix_type; + public uint8_t flags; + public uint8_t flags2; + public uint8_t num_sv; + public int32_t lon, lat; + public int32_t height, h_msl; + public uint32_t h_acc, v_acc; + public int32_t velN, velE, velD, gspeed; + public int32_t head_mot; + public uint32_t s_acc; + public uint32_t head_acc; + public uint16_t p_dop; + + [MarshalAs(UnmanagedType.ByValArray, SizeConst = 6)] + public uint8_t[] reserved1; + + public uint32_t headVeh; + + [MarshalAs(UnmanagedType.ByValArray, SizeConst = 4)] + public uint8_t[] reserved2; + } + + [StructLayout(LayoutKind.Sequential, Pack = 1)] + public struct ubx_nav_svin + { + public uint8_t version; + + [MarshalAs(UnmanagedType.ByValArray, SizeConst = 3)] + public uint8_t[] reserved1; + + public uint32_t iTOW; + public uint32_t dur; + public int32_t meanX; + public int32_t meanY; + public int32_t meanZ; + public int8_t meanXHP; + public int8_t meanYHP; + public int8_t meanZHP; + public uint8_t reserved2; + public uint32_t meanAcc; + public uint32_t obs; + public uint8_t valid; + public uint8_t active; + + [MarshalAs(UnmanagedType.ByValArray, SizeConst = 2)] + public uint8_t[] reserved3; + + public double[] getECEF() + { + var X = meanX / 100.0 + meanXHP * 0.0001; + var Y = meanY / 100.0 + meanYHP * 0.0001; + var Z = meanZ / 100.0 + meanZHP * 0.0001; + + return new double[] { X, Y, Z }; + } + } + + [StructLayout(LayoutKind.Sequential, Pack = 1)] + public struct ubx_nav_velned + { + public uint iTOW; + public int velN; + public int velE; + public int velD; + public uint speed; + public uint gSpeed; + public int heading; + public uint sAcc; + public uint cAcc; + } + + [StructLayout(LayoutKind.Sequential, Pack = 1)] + public struct ubx_cfg_nav5_getset_s + { + public uint16_t mask; + public uint8_t dynModel; + public uint8_t fixMode; + public int32_t fixedAlt; + public uint32_t fixedAltVar; + public int8_t minElev; + public uint8_t drLimit; + public uint16_t pDop; + public uint16_t tDop; + public uint16_t pAcc; + public uint16_t tAcc; + public uint8_t staticHoldThresh; + public uint8_t dgnssTimeout; + public uint8_t cnoThreshNumSVs; + public uint8_t cnoThresh; + + [MarshalAs(UnmanagedType.ByValArray, SizeConst = 2)] + public uint8_t[] reserved1; + + public uint16_t staticHoldMaxDist; + public uint8_t utcStandard; + + [MarshalAs(UnmanagedType.ByValArray, SizeConst = 5)] + public uint8_t[] reserved2; + } + + [StructLayout(LayoutKind.Sequential, Pack = 1, Size = 40)] + public struct ubx_cfg_tmode3 + { + public ubx_cfg_tmode3(double lat, double lng, double alt, double acc = 0.001) + { + version = 0; + reserved1 = 0; + if (Math.Abs(lat) > 90) + { + flags = 2; // fixed mode ecef + ecefXorLat = (int)(lat * 100); + ecefYorLon = (int)(lng * 100); + ecefZorAlt = (int)(alt * 100); + ecefXOrLatHP = (sbyte)((lat * 100 - ecefXorLat) * 100.0); + ecefYOrLonHP = (sbyte)((lng * 100 - ecefYorLon) * 100.0); + ecefZOrAltHP = (sbyte)((alt * 100 - ecefZorAlt) * 100.0); + } + else + { + flags = 256 + 2; // lla + fixed mode + ecefXorLat = (int)(lat * 1e7); + ecefYorLon = (int)(lng * 1e7); + ecefZorAlt = (int)(alt * 100.0); + ecefXOrLatHP = (sbyte)((lat * 1e7 - ecefXorLat) * 100.0); + ecefYOrLonHP = (sbyte)((lng * 1e7 - ecefYorLon) * 100.0); + ecefZOrAltHP = (sbyte)((alt * 100.0 - ecefZorAlt) * 100.0); + } + + reserved2 = 0; + fixedPosAcc = (uint)(acc * 1000.0); + svinMinDur = 60; + svinAccLimit = 2000; + reserved3 = new byte[8]; + } + + public ubx_cfg_tmode3(uint DurationS, double AccLimit) + { + version = 0; + reserved1 = 0; + flags = 1; // surveyin mode + ecefXorLat = 0; + ecefYorLon = 0; + ecefZorAlt = 0; + ecefXOrLatHP = (sbyte)0; + ecefYOrLonHP = (sbyte)0; + ecefZOrAltHP = (sbyte)0; + reserved2 = 0; + fixedPosAcc = 0; + svinMinDur = DurationS; + svinAccLimit = (uint)(AccLimit * 10000); + reserved3 = new byte[8]; + } + + public static ubx_cfg_tmode3 Disable + { + get + { + return new ubx_cfg_tmode3() + { + flags = 0, // disable + reserved3 = new byte[8] + }; + } + } + + public static implicit operator byte[] (ubx_cfg_tmode3 input) + { + return null;// MavlinkUtil.StructureToByteArray(input); + } + + public enum modeflags + { + Disabled = 0, + SurveyIn = 1, + FixedECEF = 2, + LLA = 256, + FixedLLA = 258 + } + + public byte version; + public byte reserved1; + public ushort flags; + public int ecefXorLat; // 1e7 + public int ecefYorLon; + public int ecefZorAlt; + public sbyte ecefXOrLatHP; + public sbyte ecefYOrLonHP; + public sbyte ecefZOrAltHP; + public byte reserved2; + public uint fixedPosAcc; + public uint svinMinDur; + public uint svinAccLimit; + + [MarshalAs(UnmanagedType.ByValArray, SizeConst = 8)] + public byte[] reserved3; + + + + public void SetupM8P(ICommsSerial port, bool m8p_130plus = false, bool movingbase = false) + { + port.BaseStream.Flush(); + + var bauds = new[] { port.BaudRate, 9600, 38400, 57600, 115200, 230400, 460800 }; + + // change the baudrate + foreach (var baud in bauds) + { + port.BaudRate = baud; + + System.Threading.Thread.Sleep(50); + // U = bit 01010101 - often used for autobaud + port.Write("UU"); + // port config - 460800 - uart1 + var packet = generate(0x6, 0x00, new byte[] + { + 0x01, 0x00, 0x00, 0x00, 0xD0, 0x08, 0x00, 0x00, 0x00, 0x08, + 0x07, 0x00, 0x23, 0x00, 0x23, 0x00, 0x00, 0x00, 0x00, 0x00 + }); + port.Write(packet, 0, packet.Length); + port.BaseStream.Flush(); + System.Threading.Thread.Sleep(100); + } + + { + // port config - usb + var packet = generate(0x6, 0x00, new byte[] + { + 0x03, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x23, 0x00, 0x23, 0x00, 0x00, 0x00, 0x00, 0x00 + }); + port.Write(packet, 0, packet.Length); + System.Threading.Thread.Sleep(300); + + port.BaseStream.Flush(); + + port.BaudRate = 460800; + } + + { + // set rate to 1hz + var packet = generate(0x6, 0x8, new byte[] { 0xE8, 0x03, 0x01, 0x00, 0x01, 0x00 }); + port.Write(packet, 0, packet.Length); + System.Threading.Thread.Sleep(200); + + // set navmode to stationary + { + packet = generate(0x6, 0x24, + new byte[] + { + 0xFF, 0xFF, 0x02, 0x03, 0x00, 0x00, 0x00, 0x00, 0x10, 0x27, 0x00, 0x00, 0x0F, 0x00, 0xFA, + 0x00, + 0xFA, 0x00, 0x64, 0x00, 0x2C, 0x01, 0x00, 0x00, 0x00, 0x23, 0x10, 0x27, 0x00, 0x00, 0x00, + 0x00, + 0x00, 0x00, 0x00, 0x00 + }); + port.Write(packet, 0, packet.Length); + System.Threading.Thread.Sleep(200); + } + } + + // turn off all nmea + for (int a = 0; a <= 0xf; a++) + { + if (a == 0xb || a == 0xc || a == 0xe) + continue; + turnon_off(port, 0xf0, (byte)a, 0); + } + + // mon-ver + poll_msg(port, 0xa, 0x4); + + // surveyin msg - for feedback + turnon_off(port, 0x01, 0x3b, 1); + + // pvt msg - for feedback + turnon_off(port, 0x01, 0x07, 1); + + // 1005 - 5s + turnon_off(port, 0xf5, 0x05, 5); + + byte rate1 = 1; + byte rate2 = 0; + + if (m8p_130plus) + { + rate1 = 0; + rate2 = 1; + } + + // 1074 - 1s + turnon_off(port, 0xf5, 0x4a, rate2); + // 1077 - 1s + turnon_off(port, 0xf5, 0x4d, rate1); + + // 1084 - 1s + turnon_off(port, 0xf5, 0x54, rate2); + // 1087 - 1s + turnon_off(port, 0xf5, 0x57, rate1); + + // 1094 - 1s + turnon_off(port, 0xf5, 0x5e, rate2); + // 1097 - 1s + turnon_off(port, 0xf5, 0x61, rate1); + + // 1124 - 1s + turnon_off(port, 0xf5, 0x7c, rate2); + // 1127 - 1s + turnon_off(port, 0xf5, 0x7f, rate1); + + // 4072 + turnon_off(port, 0xf5, 0xFE, 0); + + // 1230 - 5s + turnon_off(port, 0xf5, 0xE6, 5); + + // NAV-VELNED - 1s + turnon_off(port, 0x01, 0x12, 1); + + // rxm-raw/rawx - 1s + turnon_off(port, 0x02, 0x15, 1); + turnon_off(port, 0x02, 0x10, 1); + + // rxm-sfrb/sfrb - 2s + turnon_off(port, 0x02, 0x13, 2); + turnon_off(port, 0x02, 0x11, 2); + + // mon-hw - 2s + turnon_off(port, 0x0a, 0x09, 2); + + System.Threading.Thread.Sleep(100); + } + + + public void turnon_off(ICommsSerial port, byte clas, byte subclass, byte every_xsamples) + { + byte[] datastruct1 = { clas, subclass, 0, every_xsamples, 0, every_xsamples, 0, 0 }; + + var packet = generate(0x6, 0x1, datastruct1); + + port.Write(packet, 0, packet.Length); + + System.Threading.Thread.Sleep(10); + } + + public void poll_msg(ICommsSerial port, byte clas, byte subclass) + { + byte[] datastruct1 = { }; + + var packet = generate(clas, subclass, datastruct1); + + port.Write(packet, 0, packet.Length); + + System.Threading.Thread.Sleep(10); + } + } + } +} \ No newline at end of file diff --git a/Plane.FormationCreator/ViewModels/RtcmInfoViewModel.cs b/Plane.FormationCreator/ViewModels/RtcmInfoViewModel.cs index cb27aa7..6ddfe7d 100644 --- a/Plane.FormationCreator/ViewModels/RtcmInfoViewModel.cs +++ b/Plane.FormationCreator/ViewModels/RtcmInfoViewModel.cs @@ -40,6 +40,17 @@ namespace Plane.FormationCreator.ViewModels set { Set(nameof(SerialPortResend), ref _serialPortResend, value); } } + + private bool _smalldatamk; + public bool SmalldataMK + { + get { return _smalldatamk; } + set + { + Set(nameof(SmalldataMK), ref _smalldatamk, value); + _rtcmManager.SmallRtcmData= value; + } + } private bool _resendtocommk; public bool ResendToComMK @@ -83,6 +94,7 @@ namespace Plane.FormationCreator.ViewModels public void RefreshPorts() { string lastSelectValue = SerialPortsSelectdValue; + string lastSelectValue_re = SerialPortResend; serialPorts.Clear(); serialPorts.Add("魔方基站"); serialPorts.Add("千寻"); @@ -93,6 +105,17 @@ namespace Plane.FormationCreator.ViewModels } if (serialPorts.Contains(lastSelectValue)) SerialPortsSelectdValue = lastSelectValue; + + serialRePorts.Clear(); + + foreach (var item in commports) + { + serialRePorts.Add(item); + } + if (serialRePorts.Contains(lastSelectValue_re)) + SerialPortResend = lastSelectValue_re; + + } private ICommand _ConnectRtcmCommand; diff --git a/Plane.FormationCreator/Views/RtcmInfoView.xaml b/Plane.FormationCreator/Views/RtcmInfoView.xaml index 9dfcc88..1ef0670 100644 --- a/Plane.FormationCreator/Views/RtcmInfoView.xaml +++ b/Plane.FormationCreator/Views/RtcmInfoView.xaml @@ -5,24 +5,43 @@ xmlns:d="http://schemas.microsoft.com/expression/blend/2008" xmlns:local="clr-namespace:Plane.FormationCreator.Views" xmlns:c="http://metro.mahapps.com/winfx/xaml/controls" - mc:Ignorable="d" - d:DesignHeight="300" d:DesignWidth="800" - Height="350" Width="1500" WindowStartupLocation="CenterScreen" Background="#FF2D2D2D" BorderBrush="#FF039AF0" Title="RTK控制" ResizeMode="NoResize"> + mc:Ignorable="d" d:DesignWidth="800" + Height="426" Width="1500" WindowStartupLocation="CenterScreen" Background="#FF2D2D2D" BorderBrush="#FF039AF0" Title="RTK控制" ResizeMode="NoResize"> - + + + -