From eb7f38448beca1cc6ca3334aad1805b51e9ada43 Mon Sep 17 00:00:00 2001 From: ndvorkina Date: Fri, 1 Sep 2023 15:37:49 +0300 Subject: [PATCH 1/3] MSM5 added 1042, 1046 ephemeris added with tests. --- src/Asv.Gnss.Test/Asv.Gnss.Test.csproj | 5 + src/Asv.Gnss.Test/RTCMv3Msm5Test.cs | 225 +++++++++++++ src/Asv.Gnss.Test/RTCMv3TestExt.cs | 83 ++++- src/Asv.Gnss.Test/TestData.Designer.cs | 3 +- .../Messages/Ephemeris/RtcmV3Message1042.cs | 264 +++++++++++++++ .../Messages/Ephemeris/RtcmV3Message1046.cs | 26 ++ .../RTCM/V3/Messages/MSM/MSM5/RtcmV3Msm5.cs | 308 ++++++++++++++++++ .../V3/Messages/MSM/MSM5/RtcmV3Msm5Msg1075.cs | 10 + .../V3/Messages/MSM/MSM5/RtcmV3Msm5Msg1085.cs | 8 + .../V3/Messages/MSM/MSM5/RtcmV3Msm5Msg1095.cs | 8 + .../V3/Messages/MSM/MSM5/RtcmV3Msm5Msg1125.cs | 8 + .../Messages/Proprietary/RtcmV3Msg4094.cs.cs | 26 ++ .../StationaryRTK/RtcmV3Message1008.cs | 2 +- .../StationaryRTK/RtcmV3Message1033.cs | 2 +- .../Parsers/RTCM/V3/RtcmV3FactoryExtension.cs | 26 +- 15 files changed, 991 insertions(+), 13 deletions(-) create mode 100644 src/Asv.Gnss.Test/RTCMv3Msm5Test.cs create mode 100644 src/Asv.Gnss/Parsers/RTCM/V3/Messages/Ephemeris/RtcmV3Message1042.cs create mode 100644 src/Asv.Gnss/Parsers/RTCM/V3/Messages/Ephemeris/RtcmV3Message1046.cs create mode 100644 src/Asv.Gnss/Parsers/RTCM/V3/Messages/MSM/MSM5/RtcmV3Msm5.cs create mode 100644 src/Asv.Gnss/Parsers/RTCM/V3/Messages/MSM/MSM5/RtcmV3Msm5Msg1075.cs create mode 100644 src/Asv.Gnss/Parsers/RTCM/V3/Messages/MSM/MSM5/RtcmV3Msm5Msg1085.cs create mode 100644 src/Asv.Gnss/Parsers/RTCM/V3/Messages/MSM/MSM5/RtcmV3Msm5Msg1095.cs create mode 100644 src/Asv.Gnss/Parsers/RTCM/V3/Messages/MSM/MSM5/RtcmV3Msm5Msg1125.cs create mode 100644 src/Asv.Gnss/Parsers/RTCM/V3/Messages/Proprietary/RtcmV3Msg4094.cs.cs diff --git a/src/Asv.Gnss.Test/Asv.Gnss.Test.csproj b/src/Asv.Gnss.Test/Asv.Gnss.Test.csproj index a51b00c..47efa0e 100644 --- a/src/Asv.Gnss.Test/Asv.Gnss.Test.csproj +++ b/src/Asv.Gnss.Test/Asv.Gnss.Test.csproj @@ -37,4 +37,9 @@ + + + Never + + \ No newline at end of file diff --git a/src/Asv.Gnss.Test/RTCMv3Msm5Test.cs b/src/Asv.Gnss.Test/RTCMv3Msm5Test.cs new file mode 100644 index 0000000..ba7227e --- /dev/null +++ b/src/Asv.Gnss.Test/RTCMv3Msm5Test.cs @@ -0,0 +1,225 @@ +using System; +using System.Linq; +using System.Reactive.Linq; +using Xunit; + +namespace Asv.Gnss.Test +{ + public class RTCMv3Msm5Test + { + [Fact] + public void TestMsg1075() + { + var array = new byte[] + { + 0xd3,0x00,0x22,0x43,0x30,0x0d,0x2f,0xaf,0xb3,0xe2,0x00,0x20,0x00,0x00,0x04,0x00,0x00,0x00,0x00,0x00, + 0x20,0x00,0x00,0x00,0x55,0x02,0x1e,0xf9,0xf0,0x68,0xc8,0x3a,0x68,0x34,0x78,0x52,0x48,0xb3,0x8e,0xf7 + }; + var parser = new RtcmV3Parser().RegisterExtendedMessages(); + RtcmV3MessageBase msg = null; + parser.OnMessage.Cast().Subscribe(_ => msg = _); + foreach (var p in array) + { + parser.Read(p); + } + Assert.NotNull(msg); + Assert.Equal("1075", msg.MessageStringId); + var msg1075 = msg as RtcmV3Msm5Msg1075; + Assert.Equal((uint)200011000, msg1075.EpochTimeTow); + Assert.Equal(1, msg1075.MultipleMessageBit); + Assert.Equal(0, msg1075.Iods); + Assert.Equal(0, msg1075.Reserved); + Assert.Equal((uint)1, msg1075.ClockSteeringIndicator); + Assert.Equal((uint)0, msg1075.ExternalClockIndicator); + Assert.Equal((uint)0, msg1075.SmoothingIndicator); + Assert.Equal((uint)0, msg1075.SmoothingInterval); + + Assert.Single(msg1075.Satellites); + Assert.Single(msg1075.Satellites[0].Signals); + + var firstSat = msg1075.Satellites[0]; + Assert.Equal(21, firstSat.SatellitePrn); + + Assert.Equal("L1C", firstSat.Signals[0].RinexCode); + Assert.Equal(1, firstSat.Signals[0].ObservationCode); + + var freq = RtcmV3Helper.Code2Freq(NavigationSystemEnum.SYS_GPS, 1); + + Assert.Equal((84 + 0.529296875 + 0.00019985437393188477) * RtcmV3Helper.RANGE_MS, firstSat.Signals[0].PseudoRange); + Assert.Equal(10, firstSat.Signals[0].LockTime); + Assert.Equal(0, firstSat.Signals[0].HalfCycle); + Assert.Equal(30 + 0.5, firstSat.Signals[0].Cnr); + Assert.True(((-388 + 0.26330000000000003) * freq / RtcmV3Helper.CLIGHT - firstSat.Signals[0].PhaseRangeRate) < double.Epsilon); + Assert.True(((84 + 0.0002228040248155594) * freq / RtcmV3Helper.CLIGHT - firstSat.Signals[0].CarrierPhase) < double.Epsilon); + + } + + [Fact] + public void TestMsg1085() + { + var array = new byte[] + { + 0xd3,0x01,0x7c,0x43,0xd0,0x0d,0x49,0x0e,0xe8,0xa2,0x00,0x20,0x70,0xb8,0x1c,0x00,0x00,0x00,0x00,0x00, + 0x30,0xc2,0x00,0x00,0x7b,0xde,0xf6,0x3d,0xef,0x7b,0xe8,0xa8,0x69,0xea,0x09,0x28,0x08,0xe9,0x89,0x2a, + 0x30,0x79,0xa0,0xed,0x53,0x67,0x53,0x1d,0xbd,0x5b,0xeb,0xe0,0x52,0x90,0x76,0x9e,0x84,0xd4,0x12,0xd7, + 0xd9,0x7e,0x6e,0x86,0xbe,0x14,0x88,0x07,0x7e,0xcf,0x83,0x21,0xf3,0xd7,0x99,0x20,0x95,0x41,0x15,0x02, + 0xa3,0x06,0x0f,0xd7,0x3b,0xae,0x37,0x61,0x6e,0xc2,0xc4,0x69,0x88,0xc1,0x12,0x01,0x23,0xf1,0xc4,0xef, + 0x8a,0x3f,0x2f,0x8e,0x5f,0x46,0x8e,0x4d,0x00,0x96,0xd4,0x2d,0xb6,0x5c,0x14,0xb7,0xa0,0x09,0x80,0x12, + 0xc0,0x49,0xc0,0x90,0x86,0xe4,0x0d,0xf4,0x21,0x78,0x3e,0x17,0x1d,0x0e,0x33,0x9c,0xa2,0xb9,0x3a,0xe4, + 0x03,0xc9,0x39,0x96,0xe7,0x35,0xd6,0x34,0x70,0x06,0x3b,0x00,0x24,0xb3,0x00,0x40,0x3c,0x00,0xb0,0x9f, + 0xa9,0x6e,0xbe,0xa0,0x7b,0xfa,0xb2,0x97,0xe9,0xf2,0x90,0x8a,0x3d,0x82,0x16,0xae,0x08,0x9b,0x7c,0x22, + 0x02,0x4f,0x8e,0xd3,0x3d,0xac,0xe7,0xf7,0x45,0x47,0xd8,0xfd,0xa1,0x2b,0x79,0x03,0x33,0xf7,0x0b,0xb8, + 0x9c,0x2e,0x4f,0xb0,0xbd,0x00,0x02,0xf5,0xac,0xff,0xb5,0xb8,0x04,0x14,0x00,0x14,0xbe,0x40,0x59,0xb6, + 0x03,0xbf,0x5c,0x0f,0xf8,0xb0,0x40,0xad,0x01,0x07,0xc4,0xfa,0x19,0xab,0xe7,0xfe,0x1f,0xa3,0x61,0x3e, + 0x9c,0xac,0xf1,0xc1,0x4f,0xca,0x5f,0xdf,0x2c,0x41,0xbc,0xa5,0x40,0xf2,0x7a,0xd3,0xff,0xff,0xff,0xff, + 0x77,0x77,0xff,0xff,0xff,0xff,0xff,0xff,0xff,0xff,0xff,0xff,0xff,0x2f,0x2e,0xc0,0x00,0x00,0x00,0x00, + 0x61,0x85,0x54,0x69,0xa6,0x17,0xc5,0x1c,0x91,0xd1,0x44,0xf2,0xe1,0x86,0x9a,0x69,0x9e,0x59,0x5d,0x6c, + 0x71,0xd5,0x4c,0xd3,0x59,0x64,0x71,0xc5,0x0c,0x9f,0xb1,0x7f,0x62,0xfe,0xcb,0xfd,0xb5,0xcc,0xf7,0x99, + 0xef,0x31,0x2e,0x63,0xbc,0xa6,0x79,0x4c,0xf2,0x12,0xe4,0x8f,0xef,0xaf,0xdf,0x5f,0xc5,0x8f,0x8c,0x3e, + 0x09,0x7c,0x12,0x8a,0x1a,0x14,0x34,0x27,0x5c,0x4e,0x27,0x37,0xce,0x6f,0x9c,0xe5,0x39,0xcf,0x6c,0xff, + 0xd9,0xff,0xb4,0xef,0x6a,0x30,0xef,0xc1,0xdf,0x83,0xb9,0x07,0x7f,0x92,0xf7,0x25,0xee,0x48,0x98,0x99, + 0xc9,0x27,0xc0,0xa2,0xae,0x98 + }; + var parser = new RtcmV3Parser().RegisterExtendedMessages(); + RtcmV3MessageBase msg = null; + parser.OnMessage.Cast().Subscribe(_ => msg = _); + foreach (var p in array) + { + parser.Read(p); + } + Assert.NotNull(msg); + Assert.Equal("1085", msg.MessageStringId); + var msg1085 = msg as RtcmV3Msm5Msg1085; + + Assert.Equal(1, msg1085.MultipleMessageBit); + Assert.Equal(0, msg1085.Iods); + Assert.Equal(0, msg1085.Reserved); + Assert.Equal((uint)1, msg1085.ClockSteeringIndicator); + Assert.Equal((uint)0, msg1085.ExternalClockIndicator); + Assert.Equal((uint)0, msg1085.SmoothingIndicator); + Assert.Equal((uint)0, msg1085.SmoothingInterval); + + Assert.Equal(10, msg1085.Satellites.Length); + Assert.Equal(5, msg1085.Satellites.Select(s => s.Signals).Max(ss => ss.Length)); + Assert.Equal(39, msg1085.Satellites.Select(s => s.Signals).Sum(ss => ss.Length)); + + } + + [Fact] + public void TestMsg1095() + { + var array = new byte[] + { + 0xd3,0x01,0xa6,0x44,0x70,0x0d,0x2f,0xaf,0xb3,0xe2,0x00,0x20,0x35,0x88,0x00,0xc0,0x40,0x00,0x00,0x00, + 0x04,0x10,0x88,0x80,0x7f,0xff,0xff,0xff,0xff,0xfd,0x49,0x45,0x75,0x79,0x45,0x5d,0x65,0x39,0x70,0x00, + 0x00,0x00,0x00,0x3a,0xc7,0x6c,0x87,0x6a,0x79,0x0f,0x18,0xda,0x6c,0xaf,0xef,0x8f,0x81,0x54,0x09,0x6f, + 0xdc,0x8f,0xc8,0xbe,0x7d,0x07,0xa8,0x01,0xd0,0x10,0xce,0x4e,0x1c,0x9f,0x38,0x8c,0x71,0xb8,0xe7,0x68, + 0x20,0x70,0x3a,0x80,0x7c,0x01,0x05,0x02,0x7f,0xce,0x41,0x9d,0xff,0x3f,0xce,0x82,0x4d,0x15,0xde,0xa1, + 0xfd,0x34,0x7a,0x80,0xf5,0x51,0xec,0x77,0x06,0x3e,0x0e,0x0c,0x04,0x58,0x0f,0xf0,0x57,0x6e,0x29,0xdb, + 0x45,0xb7,0x8b,0x70,0x96,0xe8,0xcd,0xf7,0x7b,0xf2,0x37,0xb0,0xef,0x98,0xe0,0x45,0xa9,0x8f,0x4f,0xb6, + 0xa2,0x3d,0x47,0x7a,0xac,0x0b,0x96,0x97,0x6e,0x2e,0x1e,0x5c,0xcc,0xbd,0x90,0x7a,0xcd,0x61,0xe4,0x85, + 0x07,0x9c,0x52,0x1e,0xa4,0x18,0x7a,0x0b,0xa0,0x2c,0x0e,0x00,0x7c,0x0e,0x01,0xf2,0x58,0x07,0x98,0x00, + 0x1e,0x7a,0xf9,0x40,0x41,0xe4,0x3c,0x8f,0x8f,0xe5,0x9e,0x42,0x60,0xf8,0xf8,0x71,0xfb,0x9b,0x6f,0xe7, + 0x91,0x5f,0x9c,0xee,0x7e,0x6f,0xf3,0xf9,0xcd,0x7f,0x80,0xd7,0x5d,0xfd,0x01,0xf7,0xee,0xdd,0xdf,0xad, + 0xb7,0x7e,0xf1,0x3f,0x17,0xf9,0xfb,0xd0,0x37,0xef,0x4b,0x07,0xbc,0x2b,0xbe,0xf4,0x95,0x7b,0xbd,0x91, + 0xee,0x54,0x2f,0xb9,0x03,0xfe,0xe2,0x27,0x7b,0x8b,0x7b,0xeb,0xac,0xbf,0xaa,0x58,0x7e,0xa9,0x59,0x7a, + 0xb0,0xbf,0xea,0x9c,0x08,0x61,0x77,0x81,0x7a,0x09,0x85,0xea,0x56,0x17,0x9e,0xd8,0x5e,0xfb,0x9f,0xbf, + 0xff,0xff,0xff,0xff,0xff,0xff,0xff,0xff,0xff,0xff,0xff,0xff,0xff,0xff,0xff,0xff,0xff,0xff,0xff,0xff, + 0xfe,0x00,0x00,0x00,0x00,0x00,0x0c,0x6e,0xd7,0x8d,0x73,0xc7,0x7e,0xb6,0xb2,0x8b,0xf1,0xba,0x18,0x23, + 0x96,0x6c,0xf1,0xdf,0xad,0xe8,0xa6,0xdb,0xec,0xc2,0xfc,0x74,0xc7,0x4d,0x39,0xef,0x8a,0x6a,0xc3,0x2b, + 0xc4,0x59,0x08,0xc8,0x11,0xb2,0x23,0x90,0x46,0xe9,0x32,0x62,0x60,0x44,0xc1,0x89,0x88,0x12,0xf9,0x0d, + 0x5c,0x19,0x40,0x34,0x28,0x68,0xa0,0xd1,0x9e,0xbe,0xfd,0xbb,0xfb,0xb1,0xf7,0x35,0xef,0x50,0x53,0x48, + 0xa5,0xc1,0x4a,0xa2,0x9b,0x05,0x35,0x0e,0xaf,0x1c,0xac,0x38,0xa4,0x72,0xf8,0xe6,0x20,0x3a,0x00,0x5f, + 0xc0,0xc4,0x01,0x8b,0x02,0xe3,0xc6,0xef,0x8b,0x67,0x17,0xde,0x32,0x3c,0x60,0xf8,0xee,0xf2,0x12,0xe4, + 0x09,0xc8,0xeb,0x91,0x70,0xb2,0xc1,0xdb + }; + var parser = new RtcmV3Parser().RegisterExtendedMessages(); + RtcmV3MessageBase msg = null; + parser.OnError.Subscribe(_ => + { + Console.WriteLine("ERR:" + _.Message); + }); + parser.OnMessage.Cast().Subscribe(_ => msg = _); + foreach (var p in array) + { + parser.Read(p); + } + Assert.NotNull(msg); + Assert.Equal("1095", msg.MessageStringId); + var msg1095 = msg as RtcmV3Msm5Msg1095; + + Assert.Equal(1, msg1095.MultipleMessageBit); + Assert.Equal(0, msg1095.Iods); + Assert.Equal(0, msg1095.Reserved); + Assert.Equal((uint)1, msg1095.ClockSteeringIndicator); + Assert.Equal((uint)0, msg1095.ExternalClockIndicator); + Assert.Equal((uint)0, msg1095.SmoothingIndicator); + Assert.Equal((uint)0, msg1095.SmoothingInterval); + + Assert.Equal(9, msg1095.Satellites.Length); + Assert.Equal(5, msg1095.Satellites.Select(s => s.Signals).Max(ss => ss.Length)); + Assert.Equal(45, msg1095.Satellites.Select(s => s.Signals).Sum(ss => ss.Length)); + + } + + [Fact] + public void TestMsg1125() + { + var array = new byte[] + { + 0xd3,0x01,0xc0,0x46,0x50,0x0d,0x2f,0xae,0xd9,0x22,0x00,0x20,0x01,0x20,0x19,0x00,0x04,0x92,0x00,0x00, + 0x20,0x82,0x08,0x90,0xf0,0xe1,0xbf,0x7e,0xfd,0xfb,0xf7,0xef,0x7d,0x7b,0x4e,0x4e,0x4f,0x48,0x7d,0x52, + 0x51,0x00,0x00,0x00,0x00,0x07,0x06,0xac,0x42,0xa6,0x25,0x3b,0x81,0xaa,0xbc,0x36,0x88,0x05,0x00,0x00, + 0xff,0x39,0x05,0xec,0x1c,0xcf,0xf3,0x00,0x84,0xfb,0xf7,0xdd,0xaf,0x61,0x3e,0x4a,0xbc,0xae,0x84,0x44, + 0x06,0x08,0x0d,0x0c,0x09,0x3f,0xf7,0x80,0x09,0xa0,0x29,0x00,0x1b,0x00,0x8a,0xf8,0xf9,0xe9,0x67,0xd8, + 0xf7,0xb7,0x4f,0x60,0xbe,0xfc,0x36,0x1c,0x69,0xe4,0xd4,0xe9,0xab,0x1f,0x52,0xbe,0xba,0xaf,0x6e,0x5e, + 0xdb,0x7d,0xd1,0x7b,0xf1,0xf7,0x13,0xec,0x04,0xf4,0xa1,0xda,0xe3,0xb8,0x67,0x88,0x4e,0xd2,0x1e,0x0b, + 0x20,0xb6,0x3f,0xd0,0x82,0x39,0x09,0xc2,0x03,0x03,0xfa,0x8d,0xba,0x9c,0x6d,0x38,0x66,0x72,0x38,0xe0, + 0x49,0xb5,0xef,0xc6,0x4a,0xff,0x34,0xa9,0xfc,0xf6,0xec,0x07,0xbd,0xe0,0x19,0xef,0xc0,0xb8,0x98,0x00, + 0x23,0xcb,0xfd,0xa1,0x6f,0xfb,0x22,0x7f,0xef,0x8b,0xff,0xb7,0x94,0x00,0x9a,0xcf,0xe8,0xf3,0x7e,0xf1, + 0xd0,0xfc,0x5c,0xcb,0xf3,0x28,0xbf,0xc4,0x1e,0x3f,0x5d,0x8b,0xf5,0xbd,0x67,0xd1,0xc2,0x4f,0x4d,0xc3, + 0x7d,0x4b,0xf5,0xf4,0xe8,0x47,0xd7,0x6b,0x6f,0xeb,0x37,0xbf,0x84,0xef,0xfe,0x20,0x3b,0xf8,0x2c,0xaf, + 0xe0,0x76,0x3f,0x77,0x8c,0x0f,0x15,0x9c,0x39,0xe2,0x00,0xe9,0x01,0x03,0xb8,0xf4,0x0e,0x79,0x40,0x3d, + 0x1f,0xb0,0xa4,0x6f,0x82,0x67,0xb5,0x0a,0x0a,0x34,0x28,0x78,0x40,0x9d,0xe4,0xc2,0x5b,0xc8,0x0f,0x2d, + 0x80,0x3b,0x9c,0x40,0xf1,0x4e,0x83,0xe7,0xbc,0x0e,0xe5,0x04,0x3a,0x07,0xaf,0xff,0xff,0xff,0xff,0xff, + 0xff,0xff,0xff,0xff,0xff,0xff,0xff,0xff,0xff,0xff,0xff,0xff,0xff,0xff,0xff,0xff,0xff,0xff,0xf0,0x00, + 0x00,0x00,0x00,0x00,0x0b,0x6e,0xc6,0xdb,0xb2,0xb6,0xed,0xf4,0xd2,0xdc,0x70,0xe3,0x6d,0x71,0xc3,0x2e, + 0x35,0xdb,0x0c,0xf3,0xf3,0x9e,0x72,0xbe,0xfd,0xb3,0xce,0xeb,0xef,0xcf,0x0c,0xad,0xba,0xdd,0xb3,0xce, + 0xe1,0x68,0xa2,0xd8,0x45,0xfe,0x93,0x06,0x26,0xb0,0x4c,0xd3,0xf5,0xd7,0xec,0x5f,0xe6,0x3f,0xcc,0x3f, + 0x92,0xff,0x02,0x07,0x1a,0x0d,0x30,0x1a,0x38,0x32,0xe0,0x66,0x00,0xb5,0x49,0x63,0x92,0x27,0x25,0xb2, + 0x4b,0x2c,0x95,0x79,0x33,0x2e,0x01,0xdc,0x0b,0xf8,0x06,0x70,0x15,0xe0,0x61,0xc0,0x3f,0xa8,0x8f,0x49, + 0x2e,0x9e,0x3d,0x25,0xfa,0x58,0x74,0x3a,0x1c,0xa4,0x36,0xb0,0x6d,0xf8,0xdb,0x01,0xca,0x63,0x88,0xf8, + 0x26,0x71,0x8b,0xe1,0x6d,0xc1,0xd3,0x84,0x27,0x05,0x60,0xd6,0x12,0x66 + }; + + var parser = new RtcmV3Parser().RegisterExtendedMessages(); + RtcmV3MessageBase msg = null; + parser.OnError.Subscribe(_ => + { + Console.WriteLine("ERR:" + _.Message); + }); + parser.OnMessage.Cast().Subscribe(_ => msg = _); + foreach (var p in array) + { + parser.Read(p); + } + Assert.NotNull(msg); + Assert.Equal("1125", msg.MessageStringId); + var msg1125 = msg as RtcmV3Msm5Msg1125; + + Assert.Equal(1, msg1125.MultipleMessageBit); + Assert.Equal(0, msg1125.Iods); + Assert.Equal(0, msg1125.Reserved); + Assert.Equal((uint)1, msg1125.ClockSteeringIndicator); + Assert.Equal((uint)0, msg1125.ExternalClockIndicator); + Assert.Equal((uint)0, msg1125.SmoothingIndicator); + Assert.Equal((uint)0, msg1125.SmoothingInterval); + + Assert.Equal(9, msg1125.Satellites.Length); + Assert.Equal(6, msg1125.Satellites.Select(s => s.Signals).Max(ss => ss.Length)); + Assert.Equal(48, msg1125.Satellites.Select(s => s.Signals).Sum(ss => ss.Length)); + + } + + } +} \ No newline at end of file diff --git a/src/Asv.Gnss.Test/RTCMv3TestExt.cs b/src/Asv.Gnss.Test/RTCMv3TestExt.cs index e420723..51c127a 100644 --- a/src/Asv.Gnss.Test/RTCMv3TestExt.cs +++ b/src/Asv.Gnss.Test/RTCMv3TestExt.cs @@ -169,7 +169,88 @@ public void TestMsg1031() Assert.InRange(msg1031.SIc, 0.0, 511 * 0.5); Assert.InRange(msg1031.SId, 0.0, 10.23 * 0.01); } + [Fact] + public void TestMsg1042() + { + var array = new byte[] + { + 0xd3,0x00,0x40,0x41,0x28,0xc7,0x1c,0x1e,0xc5,0x05,0x2e,0x58,0x00,0x3f,0xde,0x66,0x4f,0xb3,0x6b,0x0f, + 0xdb,0xe6,0x5d,0xc6,0x6e,0x64,0x03,0x7b,0xf1,0x9e,0x80,0x36,0xcf,0x68,0x81,0xbb,0xf4,0xa2,0xaa,0xeb, + 0xc9,0x72,0xcf,0xff,0x18,0xe0,0xf0,0xec,0x04,0x01,0x16,0x27,0x11,0xb3,0x1e,0x14,0x75,0x45,0xe8,0xf3, + 0x5c,0x3f,0xeb,0xf4,0xbe,0xdf,0xb4,0x8c,0x66,0xfd + }; + // File.WriteAllBytes("rtcm1125.log", array); + var parser = new RtcmV3Parser().RegisterDefaultMessages().RegisterExtendedMessages(); + RtcmV3MessageBase msg = null; + parser.OnError.Subscribe(_ => + { + Console.WriteLine("ERR:" + _.Message); + }); + parser.OnMessage.Cast().Subscribe(_ => msg = _); + foreach (var p in array) + { + parser.Read(p); + } + Assert.NotNull(msg); + Assert.Equal("1042", msg.MessageStringId); + var msg1042 = msg as RtcmV3Message1042; + var utc = new DateTime(2023, 6, 14); + var week = msg1042.GetWeek(utc); + Assert.Equal((uint)910, msg1042.WeekRaw); + Assert.Equal(0, msg1042.Urai); + Assert.True((-7.16227077646181e-11 - msg1042.IdotRaw) < double.Epsilon); + Assert.Equal((uint)1, msg1042.Aode); + Assert.Equal(309600D, msg1042.TocRaw); + Assert.Equal(0.0, msg1042.A2); + Assert.True((-7.640110766260477e-12 - msg1042.A1) < double.Epsilon); + Assert.True(msg1042.A0 - 0.0006080692401155829 < double.Epsilon); + Assert.Equal((uint)1, msg1042.Adoc); + Assert.Equal(-72.203125, msg1042.Crs); + Assert.True(msg1042.DeltaN - 1.3645831131725572e-09 < double.Epsilon); + Assert.True(0.43121358612552285 - msg1042.M0 < double.Epsilon); + Assert.True(-3.428664058446884e-06 - msg1042.Cuc < double.Epsilon); + Assert.True(msg1042.e - 0.0008363371016457677 < double.Epsilon); + Assert.True(1.653563231229782e-06 - msg1042.Cus < double.Epsilon); + Assert.True(5282.6676597595215 - msg1042.Apow1_2 < double.Epsilon); + Assert.Equal(309600D, msg1042.ToeRaw); + Assert.True(-2.7008354663848877e-08 - msg1042.Cic < double.Epsilon); + Assert.True(0.4393380885012448 - msg1042.OmegaBig0 < double.Epsilon); + Assert.True(1.2945383787155151e-07 - msg1042.Cis < double.Epsilon); + Assert.True(0.3052276512607932 - msg1042.I0 < double.Epsilon); + Assert.Equal(327.328125, msg1042.Crc); + Assert.True(0.1846863552927971 - msg1042.Omega < double.Epsilon); + Assert.True(-2.3335360310738906e-09 - msg1042.OmegaBig < double.Epsilon); + Assert.Equal(-1.9000000000000001, msg1042.TGd1); + Assert.Equal(-1.9000000000000001, msg1042.TGd2); + Assert.Equal(0, msg1042.SvHealth); + } + + [Fact] + public void TestMsg1046() + { + var array = new byte[] + { + 0xd3,0x00,0x3f,0x41,0x61,0x53,0x68,0x07,0x6b,0x05,0x5d,0x44,0x60,0x00,0x01,0xf1,0xff,0xc3,0x20,0x18, + 0x02,0x14,0x99,0x79,0xd3,0x13,0x72,0xdc,0x01,0x48,0x00,0x89,0xdb,0x80,0x27,0xe2,0xa8,0x13,0xb6,0x05, + 0x44,0x60,0x00,0xda,0x55,0x76,0xa5,0xcf,0xfe,0x72,0x70,0xd8,0x10,0x71,0xe3,0x9d,0x17,0xe7,0x50,0x6f, + 0xfc,0x0f,0xf0,0x34,0x0f,0x00,0x95,0x78,0x82 + }; + // File.WriteAllBytes("rtcm1125.log", array); + var parser = new RtcmV3Parser().RegisterExtendedMessages(); + RtcmV3MessageBase msg = null; + parser.OnError.Subscribe(_ => + { + Console.WriteLine("ERR:" + _.Message); + }); + parser.OnMessage.Cast().Subscribe(_ => msg = _); + foreach (var p in array) + { + parser.Read(p); + } + Assert.NotNull(msg); + Assert.Equal("1046", msg.MessageStringId); + var msg1042 = msg as RtcmV3Message1042; + } - } } \ No newline at end of file diff --git a/src/Asv.Gnss.Test/TestData.Designer.cs b/src/Asv.Gnss.Test/TestData.Designer.cs index 7fb538b..c407b48 100644 --- a/src/Asv.Gnss.Test/TestData.Designer.cs +++ b/src/Asv.Gnss.Test/TestData.Designer.cs @@ -1,6 +1,7 @@ //------------------------------------------------------------------------------ // // This code was generated by a tool. +// Runtime Version:4.0.30319.42000 // // Changes to this file may cause incorrect behavior and will be lost if // the code is regenerated. @@ -18,7 +19,7 @@ namespace Asv.Gnss.Test { // class via a tool like ResGen or Visual Studio. // To add or remove a member, edit your .ResX file then rerun ResGen // with the /str option, or rebuild your VS project. - [global::System.CodeDom.Compiler.GeneratedCodeAttribute("System.Resources.Tools.StronglyTypedResourceBuilder", "16.0.0.0")] + [global::System.CodeDom.Compiler.GeneratedCodeAttribute("System.Resources.Tools.StronglyTypedResourceBuilder", "17.0.0.0")] [global::System.Diagnostics.DebuggerNonUserCodeAttribute()] [global::System.Runtime.CompilerServices.CompilerGeneratedAttribute()] internal class TestData { diff --git a/src/Asv.Gnss/Parsers/RTCM/V3/Messages/Ephemeris/RtcmV3Message1042.cs b/src/Asv.Gnss/Parsers/RTCM/V3/Messages/Ephemeris/RtcmV3Message1042.cs new file mode 100644 index 0000000..ed011fd --- /dev/null +++ b/src/Asv.Gnss/Parsers/RTCM/V3/Messages/Ephemeris/RtcmV3Message1042.cs @@ -0,0 +1,264 @@ +using Asv.IO; +using System; + +namespace Asv.Gnss +{ + /// + /// Proprietary Message Type. + /// Trimble Navigation Ltd. + /// + public class RtcmV3Message1042 : RtcmV3MessageBase + { + /// + /// 2^-66 + /// + public const double P2_66 = 1.35525271560688E-20; + public static DateTime BdsStart => new(2006, 1, 1, 0, 0, 0, DateTimeKind.Utc); + /// + /// Rtcm Message Id + /// + public const int RtcmMessageId = 1042; + /// + public override ushort MessageId => RtcmMessageId; + + /// + public override string Name => "BDS ephemeris information"; + + + /// + protected override void DeserializeContent(ReadOnlySpan buffer, ref int bitIndex, int messageLength) + { + var sys = NavigationSystemEnum.SYS_CMP; + + var prn = SpanBitHelper.GetBitU(buffer, ref bitIndex, 6); + var sat = RtcmV3Helper.satno(sys, (int)prn); + + if (sat == 0) + { + throw new Exception($"Rtcm3 1042 satellite number error: prn={prn}"); + } + + SatelliteNumber = sat; + SatellitePrn = prn; + WeekRaw = SpanBitHelper.GetBitU(buffer, ref bitIndex, 13); + Urai = (byte)SpanBitHelper.GetBitU(buffer, ref bitIndex, 4); + IdotRaw = SpanBitHelper.GetBitS(buffer, ref bitIndex, 14) * RtcmV3Helper.P2_43; + Aode = SpanBitHelper.GetBitU(buffer, ref bitIndex, 5); + TocRaw = SpanBitHelper.GetBitU(buffer, ref bitIndex, 17) * 8; + A2 = SpanBitHelper.GetBitU(buffer, ref bitIndex, 11) * P2_66; + A1 = SpanBitHelper.GetBitU(buffer, ref bitIndex, 22) * RtcmV3Helper.P2_50; + A0 = SpanBitHelper.GetBitU(buffer, ref bitIndex, 24) * RtcmV3Helper.P2_33; + Adoc = SpanBitHelper.GetBitU(buffer, ref bitIndex, 5); + Crs = SpanBitHelper.GetBitS(buffer, ref bitIndex, 18) * RtcmV3Helper.P2_6; + DeltaN = SpanBitHelper.GetBitS(buffer, ref bitIndex, 16) * RtcmV3Helper.P2_43; + M0 = SpanBitHelper.GetBitS(buffer, ref bitIndex, 32) * RtcmV3Helper.P2_31; + Cuc = SpanBitHelper.GetBitS(buffer, ref bitIndex, 18) * RtcmV3Helper.P2_31; + e = SpanBitHelper.GetBitS(buffer, ref bitIndex, 32) * RtcmV3Helper.P2_33; + Cus = SpanBitHelper.GetBitS(buffer, ref bitIndex, 18) * RtcmV3Helper.P2_31; + Apow1_2 = SpanBitHelper.GetBitU(buffer, ref bitIndex, 32) * RtcmV3Helper.P2_19; + ToeRaw = SpanBitHelper.GetBitU(buffer, ref bitIndex, 17) * 8; + Cic = SpanBitHelper.GetBitS(buffer, ref bitIndex, 18) * RtcmV3Helper.P2_31; + OmegaBig0 = SpanBitHelper.GetBitS(buffer, ref bitIndex, 32) * RtcmV3Helper.P2_31; + Cis = SpanBitHelper.GetBitS(buffer, ref bitIndex, 18) * RtcmV3Helper.P2_31; + I0 = SpanBitHelper.GetBitS(buffer, ref bitIndex, 32) * RtcmV3Helper.P2_31; + Crc = SpanBitHelper.GetBitS(buffer, ref bitIndex, 18) * RtcmV3Helper.P2_6; + Omega = SpanBitHelper.GetBitS(buffer, ref bitIndex, 32) * RtcmV3Helper.P2_31; + OmegaBig = SpanBitHelper.GetBitS(buffer, ref bitIndex, 24) * RtcmV3Helper.P2_43; + TGd1 = SpanBitHelper.GetBitS(buffer, ref bitIndex, 10) * 0.1; + TGd2 = SpanBitHelper.GetBitS(buffer, ref bitIndex, 10) * 0.1; + SvHealth = (byte)SpanBitHelper.GetBitU(buffer, ref bitIndex, 1); + } + + /// + /// satellite number in all the constellations. + /// + public int SatelliteNumber { get; set; } + /// + /// A BDS Satellite ID number from 1 to 37 refers to the PRN code of the BDS satellite. + /// Satelilite ID's heigher then 37 are reserved. Range 1-63 + /// + public uint SatellitePrn { get; set; } + /// + /// A BDS Satellite RINEX Code. + /// + public string SatelliteCode => RtcmV3Helper.Sat2Code(SatelliteNumber, (int)SatellitePrn); + /// + /// BDS Week number. Range 0 - 8191. Resolution - 1 week. + /// Roll-over every 8192 weeks starting fromm 00:00:00UTC on Jan 1. 2006 of BDT. + /// + public uint WeekRaw { get; set; } + /// + /// BDS URAI Range 1 - 15. + /// User Range Accuracy Index. + /// + public byte Urai { get; set; } + /// + /// BDS IDOT Range +-9.31*10e-10. Resolution ^-43 + /// Rate of inclination angle. Unit: pi/s + /// + public double IdotRaw { get; private set; } + /// + /// BDS IDOT Range 0-31. + /// Age of Data Ephemeris. + /// + public uint Aode { get; private set; } + /// + /// The reference time of clock parameters. Unit: s. + /// Range 0 - 604.792. Resolution: 8s. + /// + public uint TocRaw { get; private set; } + /// + /// Clock correcton parameter Unit:s. + /// Range: +-1.38E-17 s/s^2. Resolution: 2^-66 s/s^2 + /// + public double A2 { get; private set; } + /// + /// Clock correcton parameter Unit:s. + /// Range: +-1.86E-9 s/s^2. Resolution: 2^-50 s/s^2 + /// + public double A1 { get; private set; } + /// + /// Clock correcton parameter Unit:s. + /// Range: +-9.77E-4 s/s^2. Resolution: 2^-33 s/s^2 + /// + public double A0 { get; private set; } + /// + /// Age of Data Clock. + /// Range: 0-31. + /// + public uint Adoc { get; private set; } + /// + /// Amplitude of sine harmonic correction term to the orbit radius. Unit: m. + /// Range: +-2048. Resolution: 2^-6 m. + /// + public double Crs { get; private set; } + /// + /// Mean moution difference from computed value. Unit: pi/s. + /// Range: +-3.73E-9 pi/s. Resolution 2^-43 pi/s + /// + public double DeltaN { get; private set; } + /// + /// Mean anomaly at reference time. Unit: pi. + /// Range: +-pi. Resolution 2^-31 pi + /// + public double M0 { get; private set; } + /// + /// Amplitude of cusine harmonic correction term to the argument of latitude. Unit: rad. + /// Range: +-6.10E-5. Resolution: 2^-31 rad. + /// + public double Cuc { get; private set; } + /// + /// Eccentricity. + /// Range: 0 - 0.5. Resolution: 2^-33. + /// + public double e { get; private set; } + /// + /// Amplitude of sine harmonic correction term to the argument of latitude. Unit: rad. + /// Range: +-6.10E-5. Resolution: 2^-31 rad. + /// + public double Cus { get; private set; } + /// + /// Square root of semi-major axis. Unit: m^1/2. + /// Range: 0 - 8192 m^1/2. Resolution: 2^-19 m^1/2. + /// + public double Apow1_2 { get; private set; } + /// + /// Ephemeris reference time. Unit: s. + /// Range 0 - 604.792. Resolution: 8s. + /// + public uint ToeRaw { get; private set; } + /// + /// Amplitude of cusine harmonic correction term to the argument of angle of inclination. Unit: rad. + /// Range: +-6.10E-5. Resolution: 2^-31 rad. + /// + public double Cic { get; private set; } + /// + /// Longitude of ascending node of orbital of phase computed according to reference time. Unit: pi. + /// Range: +-pi. Resolution 2^-31 pi + /// + public double OmegaBig0 { get; private set; } + /// + /// Amplitude of sine harmonic correction term to the argument of angle of inclination. Unit: rad. + /// Range: +-6.10E-5. Resolution: 2^-31 rad. + /// + public double Cis { get; private set; } + /// + /// Inclination angle at reference time. Unit: pi. + /// Range: +-pi. Resolution 2^-31 pi + /// + public double I0 { get; private set; } + /// + /// Amplitude of cosine harmonic correction term to the orbit radius. Unit: m. + /// Range: +-2048. Resolution: 2^-6 m. + /// + public double Crc { get; private set; } + /// + /// Argument of perigree. Unit: pi. + /// Range: +-pi. Resolution 2^-31 pi + /// + public double Omega { get; private set; } + /// + /// Rate of right ascension. Unit: pi/s. + /// Range: +-9.54E-7 pi/s. Resolution: 2^-43 pi/s. + /// + public double OmegaBig { get; private set; } + /// + /// Equipment Group Delay Differential. Unit: ns. + /// Range: +-51.2 ns. Resolution: 0.1 ns. + /// + public double TGd1 { get; private set; } + /// + /// Equipment Group Delay Differential. Unit: ns. + /// Range: +-51.2 ns. Resolution: 0.1 ns. + /// + public double TGd2 { get; private set; } + /// + /// Autonomous satelite health flag. + /// Range: 0 - 1. Resolution: 1. + /// + public byte SvHealth { get; private set; } + + /// + /// BDS Week number. + /// + public int GetWeek(DateTime utc) + { + return GetBdsWeek(utc, (int)WeekRaw); + } + public DateTime GetToc() + { + return RtcmV3Helper.GetFromBeiDou((int)WeekRaw, TocRaw); + } + public double Idot => Idot * RtcmV3Helper.SC2RAD; + public DateTime GetToc(DateTime utc) + { + return RtcmV3Helper.GetFromBeiDou(GetWeek(utc), TocRaw); + } + public DateTime GetToe() + { + return RtcmV3Helper.GetFromBeiDou((int)WeekRaw, ToeRaw); + } + public static int GetBdsWeek(DateTime utc, int week) + { + int w = 0; + var s = 0.0; + var gpsTime = RtcmV3Helper.Utc2Gps(utc); + var bdsTime = RtcmV3Helper.Gps2BeiDou(gpsTime); + GetFromTime(BdsStart, bdsTime, ref w, ref s); + return week + (w - week + 1) / 8192 * 8192; + } + + public static void GetFromTime(DateTime datum, DateTime time, ref int week, ref double seconds) + { + var dif = time - datum; + + var weeks = (int)(dif.TotalDays / 7); + + week = weeks; + + dif = time - datum.AddDays(weeks * 7); + + seconds = dif.TotalSeconds; + } + } +} diff --git a/src/Asv.Gnss/Parsers/RTCM/V3/Messages/Ephemeris/RtcmV3Message1046.cs b/src/Asv.Gnss/Parsers/RTCM/V3/Messages/Ephemeris/RtcmV3Message1046.cs new file mode 100644 index 0000000..8663691 --- /dev/null +++ b/src/Asv.Gnss/Parsers/RTCM/V3/Messages/Ephemeris/RtcmV3Message1046.cs @@ -0,0 +1,26 @@ +using System; + +namespace Asv.Gnss +{ + /// + /// Proprietary Message Type. + /// Trimble Navigation Ltd. + /// + public class RtcmV3Message1046 : RtcmV3MessageBase + { + /// + /// Rtcm Message Id + /// + public const int RtcmMessageId = 1046; + /// + public override ushort MessageId => RtcmMessageId; + + /// + public override string Name => "Galileo I/NAV ephemeris information"; + + /// + protected override void DeserializeContent(ReadOnlySpan buffer, ref int bitIndex, int messageLength) + { + } + } +} diff --git a/src/Asv.Gnss/Parsers/RTCM/V3/Messages/MSM/MSM5/RtcmV3Msm5.cs b/src/Asv.Gnss/Parsers/RTCM/V3/Messages/MSM/MSM5/RtcmV3Msm5.cs new file mode 100644 index 0000000..6b558a9 --- /dev/null +++ b/src/Asv.Gnss/Parsers/RTCM/V3/Messages/MSM/MSM5/RtcmV3Msm5.cs @@ -0,0 +1,308 @@ +using Asv.IO; +using System.Collections.Generic; +using System.Linq; +using System; + +namespace Asv.Gnss +{ + public abstract class RtcmV3Msm5 : RtcmV3Msm7 + { + protected void DeserializeContentBase(ReadOnlySpan buffer, ref int bitIndex, int messageLength) + { + var utc = DateTime.UtcNow; + ReferenceStationId = SpanBitHelper.GetBitU(buffer, ref bitIndex, 12); + + var sys = NavigationSystem = RtcmV3Helper.GetNavigationSystem(MessageId); + + switch (sys) + { + case NavigationSystemEnum.SYS_GLO: + { + var dow = SpanBitHelper.GetBitU(buffer, ref bitIndex, 3); + var tod = SpanBitHelper.GetBitU(buffer, ref bitIndex, 27); + EpochTimeTow = dow * 86400000 + tod; + EpochTime = RtcmV3Helper.AdjustDailyRoverGlonassTime(utc, tod * 0.001); + break; + } + case NavigationSystemEnum.SYS_CMP: + { + EpochTimeTow = SpanBitHelper.GetBitU(buffer, ref bitIndex, 30); + var tow = EpochTimeTow * 0.001; + tow += 14.0; /* BDT -> GPS Time */ + EpochTime = RtcmV3Helper.AdjustWeekly(utc, tow); + break; + } + default: + { + EpochTimeTow = SpanBitHelper.GetBitU(buffer, ref bitIndex, 30); + var tow = EpochTimeTow * 0.001; + EpochTime = RtcmV3Helper.AdjustWeekly(utc, tow); + break; + } + } + + MultipleMessageBit = (byte)SpanBitHelper.GetBitU(buffer, ref bitIndex, 1); + ObservableDataIsComplete = MultipleMessageBit == 0 ? true : false; + + Iods = (byte)SpanBitHelper.GetBitU(buffer, ref bitIndex, 3); + + SessionTime = (byte)SpanBitHelper.GetBitU(buffer, ref bitIndex, 7); + + ClockSteeringIndicator = SpanBitHelper.GetBitU(buffer, ref bitIndex, 2); + ExternalClockIndicator = SpanBitHelper.GetBitU(buffer, ref bitIndex, 2); + + SmoothingIndicator = SpanBitHelper.GetBitU(buffer, ref bitIndex, 1); + SmoothingInterval = SpanBitHelper.GetBitU(buffer, ref bitIndex, 3); + + + var satellites = new List(); + var signals = new List(); + + for (byte i = 1; i <= 64; i++) + { + var mask = SpanBitHelper.GetBitU(buffer, ref bitIndex, 1); + if (mask > 0) satellites.Add(i); + } + + for (byte i = 1; i <= 32; i++) + { + var mask = SpanBitHelper.GetBitU(buffer, ref bitIndex, 1); + if (mask > 0) signals.Add(i); + } + + SatelliteIds = satellites.ToArray(); + SignalIds = signals.ToArray(); + var cellMaskCount = SatelliteIds.Length * SignalIds.Length; + + if (cellMaskCount > 64) + { + throw new Exception($"RtcmV3 {MessageId} number of Satellite and Signals error: Satellite={SatelliteIds.Length} Signals={SignalIds.Length}"); + } + + // CellMask = new byte[cellMaskCount]; + CellMask = new byte[SatelliteIds.Length][]; + for (var i = 0; i < SatelliteIds.Length; i++) + { + CellMask[i] = new byte[SignalIds.Length]; + for (var j = 0; j < SignalIds.Length; j++) + { + CellMask[i][j] = (byte)SpanBitHelper.GetBitU(buffer, ref bitIndex, 1); + } + } + } + protected override void DeserializeContent(ReadOnlySpan buffer, ref int bitIndex, int messageLength) + { + // base.DeserializeContent(buffer, ref bitIndex, messageLength); + DeserializeContentBase(buffer, ref bitIndex, messageLength); + var nCell = CellMask.SelectMany(_ => _).Count(_ => _ > 0); + + // Satellite data Nsat*(8 + 10) bit + // Satellite rough ranges + var roughRanges = new double[SatelliteIds.Length]; + var roughPhaseRangeRates = new double[SatelliteIds.Length]; + var extSatInfo = new byte[SatelliteIds.Length]; + + // Signal data + // Pseudoranges 15*Ncell + var pseudorange = new double[nCell]; + // PhaseRange data 22*Ncell + var phaseRange = new double[nCell]; + // signal CNRs 6*Ncell + var cnr = new double[nCell]; + // fine PhaseRangeRates data 15*nCell + var phaseRangeRates = new double[nCell]; + + //PhaseRange LockTime Indicator 4*Ncell + var @lock = new ushort[nCell]; + //Half-cycle ambiguityindicator 1*Ncell + var halfCycle = new byte[nCell]; + + for (var i = 0; i < SatelliteIds.Length; i++) + { + roughRanges[i] = roughPhaseRangeRates[i] = 0.0; + extSatInfo[i] = 15; + + } + for (var i = 0; i < nCell; i++) pseudorange[i] = phaseRange[i] = phaseRangeRates[i] = -1E16; + + /* decode satellite data, rough ranges */ + for (var i = 0; i < SatelliteIds.Length; i++) + { + /* Satellite rough ranges */ + var rng = SpanBitHelper.GetBitU(buffer, ref bitIndex, 8); + if (rng != 255) roughRanges[i] = rng * RtcmV3Helper.RANGE_MS; + } + + for (var j = 0; j < SatelliteIds.Length; j++) + { /* extended info */ + extSatInfo[j] = (byte)SpanBitHelper.GetBitU(buffer, ref bitIndex, 4); + } + + for (var i = 0; i < SatelliteIds.Length; i++) + { + var rngM = SpanBitHelper.GetBitU(buffer, ref bitIndex, 10); + if (roughRanges[i] != 0.0) roughRanges[i] += rngM * RtcmV3Helper.P2_10 * RtcmV3Helper.RANGE_MS; + } + + for (var i = 0; i < SatelliteIds.Length; i++) + { /* phaserangerate */ + var rate = SpanBitHelper.GetBitS(buffer, ref bitIndex, 14); + if (rate != -8192) roughPhaseRangeRates[i] = rate * 1.0; + } + + + /* decode signal data */ + for (var i = 0; i < nCell; i++) + { + /* pseudorange */ + var prv = SpanBitHelper.GetBitS(buffer, ref bitIndex, 15); + if (prv != -16384) pseudorange[i] = prv * RtcmV3Helper.P2_24 * RtcmV3Helper.RANGE_MS; + } + + for (var i = 0; i < nCell; i++) + { + /* phase range */ + var cpv = SpanBitHelper.GetBitS(buffer, ref bitIndex, 22); + if (cpv != -2097152) phaseRange[i] = cpv * RtcmV3Helper.P2_29 * RtcmV3Helper.RANGE_MS; + } + + for (var i = 0; i < nCell; i++) + { + /* lock time */ + @lock[i] = (byte)SpanBitHelper.GetBitU(buffer, ref bitIndex, 4); + } + + for (var i = 0; i < nCell; i++) + { + /* half-cycle ambiguity */ + halfCycle[i] = (byte)SpanBitHelper.GetBitU(buffer, ref bitIndex, 1); + } + + for (var i = 0; i < nCell; i++) + { + /* cnr */ + /* GNSS signal CNR + 1–63 dBHz */ + cnr[i] = SpanBitHelper.GetBitU(buffer, ref bitIndex, 6) * 1.0; + } + + for (var j = 0; j < nCell; j++) + { /* phaserangerate */ + var rrv = SpanBitHelper.GetBitS(buffer, ref bitIndex, 15); + if (rrv != -16384) phaseRangeRates[j] = rrv * 0.0001; + } + + CreateMsmObservable(roughRanges, roughPhaseRangeRates, pseudorange, phaseRange, phaseRangeRates, extSatInfo, @lock, halfCycle, cnr); + } + private void CreateMsmObservable(double[] roughRanges, double[] roughPhaseRangeRates, double[] pseudorange, + double[] phaseRange, double[] phaseRangeRates, byte[] extSatInfo, ushort[] @lock, byte[] halfCycle, + double[] cnr) + { + var sig = new SignalRaw[SignalIds.Length]; + var sys = RtcmV3Helper.GetNavigationSystem(MessageId); + Satellites = new Satellite[0]; + if (SatelliteIds.Length == 0) return; + Satellites = new Satellite[SatelliteIds.Length]; + + /* id to signal */ + for (var i = 0; i < SignalIds.Length; i++) + { + sig[i] = new SignalRaw(); + sig[i].RinexCode = RtcmV3Helper.GetRinexCodeFromMsm(sys, SignalIds[i] - 1); + /* signal to rinex obs type */ + sig[i].ObservationCode = RtcmV3Helper.Obs2Code(sig[i].RinexCode); + sig[i].ObservationIndex = RtcmV3Helper.Code2Idx(sys, sig[i].ObservationCode); + } + + + var k = 0; + for (var i = 0; i < SatelliteIds.Length; i++) + { + var prn = SatelliteIds[i]; + + if (sys == NavigationSystemEnum.SYS_QZS) prn += RtcmV3Helper.MINPRNQZS - 1; + else if (sys == NavigationSystemEnum.SYS_SBS) prn += RtcmV3Helper.MINPRNSBS - 1; + + + var sat = RtcmV3Helper.satno(sys, prn); + + Satellites[i] = new Satellite { SatellitePrn = prn, SatelliteCode = RtcmV3Helper.Sat2Code(sat, prn) }; + + var fcn = 0; + if (sys == NavigationSystemEnum.SYS_GLO) + { + #region SYS_GLO + + // ToDo Нужны дополнительные данные по GLONASS Ephemeris, либо использовать сообщение MSM5, там есть ex[] + // fcn = -8; /* no glonass fcn info */ + if (extSatInfo[i] <= 13) + { + fcn = extSatInfo[i] - 7; + } + // if (!rtcm->nav.glo_fcn[prn - 1]) + // { + // rtcm->nav.glo_fcn[prn - 1] = fcn + 8; /* fcn+8 */ + // } + // } + // else if (rtcm->nav.geph[prn - 1].sat == sat) + // { + // fcn = rtcm->nav.geph[prn - 1].frq; + // } + // else if (rtcm->nav.glo_fcn[prn - 1] > 0) + // { + // fcn = rtcm->nav.glo_fcn[prn - 1] - 8; + // } + + #endregion + + } + + var index = 0; + Satellites[i].Signals = new Signal[CellMask[i].Count(_ => _ != 0)]; + + for (var j = 0; j < SignalIds.Length; j++) + { + if (CellMask[i][j] == 0) continue; + + Satellites[i].Signals[index] = new Signal(); + if (sat != 0 && sig[j].ObservationIndex >= 0) + { + + var freq = fcn < -7.0 ? 0.0 : RtcmV3Helper.Code2Freq(sys, sig[j].ObservationCode, fcn); + if (Math.Abs(freq - 0.0) < 0.01) { } + /* pseudorange (m) */ + if (roughRanges[i] != 0.0 && pseudorange[k] > -1E12) + { + Satellites[i].Signals[index].PseudoRange = roughRanges[i] + pseudorange[k]; + } + + /* carrier-phase (cycle) */ + if (roughRanges[i] != 0.0 && phaseRange[k] > -1E12) + { + Satellites[i].Signals[index].CarrierPhase = (roughRanges[i] + phaseRange[k]) * freq / RtcmV3Helper.CLIGHT; + } + + /* doppler (hz) */ + if (roughPhaseRangeRates[i] != 0.0 && phaseRangeRates[k] > -1E12) + { + Satellites[i].Signals[index].PhaseRangeRate = -(roughPhaseRangeRates[i] + phaseRangeRates[k]) * freq / RtcmV3Helper.CLIGHT; + } + + Satellites[i].Signals[index].MinLockTime = RtcmV3Helper.GetMinLockTimeEx(@lock[k]); + Satellites[i].Signals[index].LockTime = @lock[k]; + Satellites[i].Signals[index].HalfCycle = halfCycle[k]; + // rtcm->obs.data[index].LLI[idx[k]] = + // LossOfLock(rtcm, sat, idx[k],lock[j]) +(halfCycle[j] ? 3 : 0); + // rtcm->obs.data[index].SNR[idx[k]] = (uint16_t)(cnr[j] / SNR_UNIT + 0.5); + Satellites[i].Signals[index].Cnr = cnr[k] + 0.5; + Satellites[i].Signals[index].ObservationCode = sig[j].ObservationCode; + Satellites[i].Signals[index].RinexCode = $"L{sig[j].RinexCode}"; + } + + k++; + index++; + } + } + } + } +} diff --git a/src/Asv.Gnss/Parsers/RTCM/V3/Messages/MSM/MSM5/RtcmV3Msm5Msg1075.cs b/src/Asv.Gnss/Parsers/RTCM/V3/Messages/MSM/MSM5/RtcmV3Msm5Msg1075.cs new file mode 100644 index 0000000..2d7edea --- /dev/null +++ b/src/Asv.Gnss/Parsers/RTCM/V3/Messages/MSM/MSM5/RtcmV3Msm5Msg1075.cs @@ -0,0 +1,10 @@ +namespace Asv.Gnss +{ + public class RtcmV3Msm5Msg1075 : RtcmV3Msm5 + { + /// + public override ushort MessageId => 1075; + /// + public override string Name => "GPS MSM5"; + } +} diff --git a/src/Asv.Gnss/Parsers/RTCM/V3/Messages/MSM/MSM5/RtcmV3Msm5Msg1085.cs b/src/Asv.Gnss/Parsers/RTCM/V3/Messages/MSM/MSM5/RtcmV3Msm5Msg1085.cs new file mode 100644 index 0000000..bbc9653 --- /dev/null +++ b/src/Asv.Gnss/Parsers/RTCM/V3/Messages/MSM/MSM5/RtcmV3Msm5Msg1085.cs @@ -0,0 +1,8 @@ +namespace Asv.Gnss +{ + public class RtcmV3Msm5Msg1085 : RtcmV3Msm5 + { + public override ushort MessageId => 1085; + public override string Name => "GLONASS MSM5"; + } +} diff --git a/src/Asv.Gnss/Parsers/RTCM/V3/Messages/MSM/MSM5/RtcmV3Msm5Msg1095.cs b/src/Asv.Gnss/Parsers/RTCM/V3/Messages/MSM/MSM5/RtcmV3Msm5Msg1095.cs new file mode 100644 index 0000000..34c190a --- /dev/null +++ b/src/Asv.Gnss/Parsers/RTCM/V3/Messages/MSM/MSM5/RtcmV3Msm5Msg1095.cs @@ -0,0 +1,8 @@ +namespace Asv.Gnss +{ + public class RtcmV3Msm5Msg1095 : RtcmV3Msm5 + { + public override ushort MessageId => 1095; + public override string Name => "Galileo MSM5"; + } +} diff --git a/src/Asv.Gnss/Parsers/RTCM/V3/Messages/MSM/MSM5/RtcmV3Msm5Msg1125.cs b/src/Asv.Gnss/Parsers/RTCM/V3/Messages/MSM/MSM5/RtcmV3Msm5Msg1125.cs new file mode 100644 index 0000000..747911d --- /dev/null +++ b/src/Asv.Gnss/Parsers/RTCM/V3/Messages/MSM/MSM5/RtcmV3Msm5Msg1125.cs @@ -0,0 +1,8 @@ +namespace Asv.Gnss +{ + public class RtcmV3Msm5Msg1125 : RtcmV3Msm5 + { + public override ushort MessageId => 1125; + public override string Name => "BeiDou MSM5"; + } +} diff --git a/src/Asv.Gnss/Parsers/RTCM/V3/Messages/Proprietary/RtcmV3Msg4094.cs.cs b/src/Asv.Gnss/Parsers/RTCM/V3/Messages/Proprietary/RtcmV3Msg4094.cs.cs new file mode 100644 index 0000000..9677036 --- /dev/null +++ b/src/Asv.Gnss/Parsers/RTCM/V3/Messages/Proprietary/RtcmV3Msg4094.cs.cs @@ -0,0 +1,26 @@ +using System; + +namespace Asv.Gnss +{ + /// + /// Proprietary Message Type. + /// Trimble Navigation Ltd. + /// + public class RtcmV3Msg4094 : RtcmV3MessageBase + { + /// + /// Rtcm Message Id + /// + public const int RtcmMessageId = 4094; + /// + public override ushort MessageId => RtcmMessageId; + + /// + public override string Name => "Trimble Navigation Ltd."; + + /// + protected override void DeserializeContent(ReadOnlySpan buffer, ref int bitIndex, int messageLength) + { + } + } +} diff --git a/src/Asv.Gnss/Parsers/RTCM/V3/Messages/StationaryRTK/RtcmV3Message1008.cs b/src/Asv.Gnss/Parsers/RTCM/V3/Messages/StationaryRTK/RtcmV3Message1008.cs index 45958b6..9cf29ec 100644 --- a/src/Asv.Gnss/Parsers/RTCM/V3/Messages/StationaryRTK/RtcmV3Message1008.cs +++ b/src/Asv.Gnss/Parsers/RTCM/V3/Messages/StationaryRTK/RtcmV3Message1008.cs @@ -3,7 +3,7 @@ namespace Asv.Gnss { - internal class RtcmV3Message1008 : RtcmV3Message1007and1008 + public class RtcmV3Message1008 : RtcmV3Message1007and1008 { public const int RtcmMessageId = 1008; public override ushort MessageId => RtcmMessageId; diff --git a/src/Asv.Gnss/Parsers/RTCM/V3/Messages/StationaryRTK/RtcmV3Message1033.cs b/src/Asv.Gnss/Parsers/RTCM/V3/Messages/StationaryRTK/RtcmV3Message1033.cs index 190c228..90ae576 100644 --- a/src/Asv.Gnss/Parsers/RTCM/V3/Messages/StationaryRTK/RtcmV3Message1033.cs +++ b/src/Asv.Gnss/Parsers/RTCM/V3/Messages/StationaryRTK/RtcmV3Message1033.cs @@ -3,7 +3,7 @@ namespace Asv.Gnss { - internal class RtcmV3Message1033 : RtcmV3Message1008 + public class RtcmV3Message1033 : RtcmV3Message1008 { public const int RtcmMessageRecAntId = 1033; public override ushort MessageId => RtcmMessageRecAntId; diff --git a/src/Asv.Gnss/Parsers/RTCM/V3/RtcmV3FactoryExtension.cs b/src/Asv.Gnss/Parsers/RTCM/V3/RtcmV3FactoryExtension.cs index 55f27d4..bf64bd7 100644 --- a/src/Asv.Gnss/Parsers/RTCM/V3/RtcmV3FactoryExtension.cs +++ b/src/Asv.Gnss/Parsers/RTCM/V3/RtcmV3FactoryExtension.cs @@ -3,14 +3,18 @@ namespace Asv.Gnss { + /// + /// Add messages that are not in the default set. + /// public static class RtcmV3FactoryExtension { + /// + /// Messages that are not in the default set. + /// public static IEnumerable> ExtendedMessages { get { - - yield return () => new RtcmV3Message1030(); yield return () => new RtcmV3Message1031(); yield return () => new RtcmV3Message1032(); @@ -22,11 +26,12 @@ public static IEnumerable> ExtendedMessages yield return () => new RtcmV3Msm3Msg1093(); yield return () => new RtcmV3Msm3Msg1123(); - //yield return () => new RtcmV3Msm4Msg1025(); - //yield return () => new RtcmV3Msm4Msg1075(); - //yield return () => new RtcmV3Msm4Msg1085(); - //yield return () => new RtcmV3Msm4Msg1095(); - //yield return () => new RtcmV3Msm4Msg1125(); + yield return () => new RtcmV3Msm5Msg1075(); + yield return () => new RtcmV3Msm5Msg1085(); + yield return () => new RtcmV3Msm5Msg1095(); + yield return () => new RtcmV3Msm5Msg1125(); + + yield return () => new RtcmV3Msg4094(); //yield return () => new RtcmV3Msm6Msg1076(); //yield return () => new RtcmV3Msm6Msg1086(); @@ -34,12 +39,15 @@ public static IEnumerable> ExtendedMessages //yield return () => new RtcmV3Msm6Msg1116(); //yield return () => new RtcmV3Msm6Msg1126(); - //yield return () => new RtcmV3MsmMsg1042(); - //yield return () => new RtcmV3MsmMsg1046(); + yield return () => new RtcmV3Message1042(); + yield return () => new RtcmV3Message1046(); //yield return () => new RtcmV3MsmMsg1023(); } } + /// + /// Registers extended messages. + /// public static RtcmV3Parser RegisterExtendedMessages(this RtcmV3Parser src) { foreach (var func in ExtendedMessages) From 4e33531e0e0bd86274b59e5381d9707b8d73a530 Mon Sep 17 00:00:00 2001 From: ndvorkina Date: Mon, 4 Sep 2023 21:34:58 +0300 Subject: [PATCH 2/3] 1046-messages is added. Ephemeris helper is added. Galileo start epoch is fixed. Tests are added. --- src/Asv.Gnss.Test/RTCMv3TestExt.cs | 51 ++- .../Parsers/RTCM/RtcmV3EphemerisHelper.cs | 82 +++++ src/Asv.Gnss/Parsers/RTCM/RtcmV3Helper.cs | 9 +- .../Messages/Ephemeris/RtcmV3Message1042.cs | 59 ++-- .../Messages/Ephemeris/RtcmV3Message1046.cs | 326 +++++++++++++++++- 5 files changed, 492 insertions(+), 35 deletions(-) create mode 100644 src/Asv.Gnss/Parsers/RTCM/RtcmV3EphemerisHelper.cs diff --git a/src/Asv.Gnss.Test/RTCMv3TestExt.cs b/src/Asv.Gnss.Test/RTCMv3TestExt.cs index 51c127a..a0b9a3b 100644 --- a/src/Asv.Gnss.Test/RTCMv3TestExt.cs +++ b/src/Asv.Gnss.Test/RTCMv3TestExt.cs @@ -1,4 +1,5 @@ -using System; +using Asv.IO; +using System; using System.Reactive.Linq; using Xunit; @@ -194,9 +195,10 @@ public void TestMsg1042() Assert.NotNull(msg); Assert.Equal("1042", msg.MessageStringId); var msg1042 = msg as RtcmV3Message1042; - var utc = new DateTime(2023, 6, 14); - var week = msg1042.GetWeek(utc); + Assert.Equal((uint)35, msg1042.SatellitePrn); Assert.Equal((uint)910, msg1042.WeekRaw); + var week = msg1042.GetWeek(new DateTime(2023, 6, 14)); + Assert.Equal(910, week); Assert.Equal(0, msg1042.Urai); Assert.True((-7.16227077646181e-11 - msg1042.IdotRaw) < double.Epsilon); Assert.Equal((uint)1, msg1042.Aode); @@ -249,7 +251,48 @@ public void TestMsg1046() } Assert.NotNull(msg); Assert.Equal("1046", msg.MessageStringId); - var msg1042 = msg as RtcmV3Message1042; + var msg1046 = msg as RtcmV3Message1046; + Assert.Equal((uint)5, msg1046.SatellitePrn); + Assert.Equal((uint)1242, msg1046.WeekRaw); + var week = msg1046.GetWeek(new DateTime(2023, 6, 14)); + Assert.Equal(1242, week); + Assert.Equal((uint)7, msg1046.IoDnav); + Assert.Equal((byte)107, msg1046.SvSisa); + Assert.True((msg1046.IdotRaw - 3.89945853385143e-11) < double.Epsilon ); + Assert.Equal((uint)311400, msg1046.TocRaw); + Assert.Equal(0.0, msg1046.Af2); + Assert.True((msg1046.Af1 - 3.524291969370097e-12) < double.Epsilon); + Assert.Equal(-5.805457476526499e-05, msg1046.Af0); + Assert.Equal(4.15625, msg1046.Crs); + Assert.True((msg1046.DeltaN - 1.1166321201017126e-09) < double.Epsilon); + Assert.True((0.9122577565722167 - msg1046.M0) < double.Epsilon); + Assert.True((1.5273690223693848e-07 - msg1046.Cuc) < double.Epsilon); + Assert.True((msg1046.e - 0.0002629421651363373) < double.Epsilon); + Assert.True((4.753470420837402e-06 - msg1046.Cus) < double.Epsilon); + Assert.True((5440.615968704224 - msg1046.Apow1_2) < double.Epsilon); + Assert.Equal((uint)311400, msg1046.ToeRaw); + Assert.True((2.421438694000244e-08 - msg1046.Cic) < double.Epsilon); + Assert.Equal(-0.7082697916775942, msg1046.OmegaBig0); + Assert.True((-4.6566128730773926e-08 - msg1046.Cis) < double.Epsilon); + Assert.True((0.30509960977360606 - msg1046.I0) < double.Epsilon); + Assert.Equal(241.78125, msg1046.Crc); + Assert.True((-0.36332833487540483 - msg1046.Omega) < double.Epsilon); + Assert.True((-1.833655005611945e-09 - msg1046.OmegaDot) < double.Epsilon); + Assert.True((msg1046.BGdE1E5a - 3.026798367500305e-09) < double.Epsilon); + Assert.True((msg1046.BGdE5bE1 - 3.4924596548080444e-09) < double.Epsilon); + Assert.Equal(0, msg1046.E5bSignalHealthFlag); + Assert.Equal(0, msg1046.E5bDataValidity); + Assert.Equal(0, msg1046.E1BSignalHealthFlag); + Assert.Equal(0, msg1046.E1BDataValidity); + Assert.Equal(0, msg1046.Reserved); + Assert.True(msg1046.IsE5bSignalOk); + Assert.False(msg1046.IsE5bSignalOuOfService); + Assert.False(msg1046.IsE5bSignalWillOuOfService); + Assert.False(msg1046.IsE5bSignalInTest); + Assert.True(msg1046.IsE1BSignalOk); + Assert.False(msg1046.E1BSignalOuOfService); + Assert.False(msg1046.E1BSignalWillOuOfService); + Assert.False(msg1046.E1BSignalInTest); } } diff --git a/src/Asv.Gnss/Parsers/RTCM/RtcmV3EphemerisHelper.cs b/src/Asv.Gnss/Parsers/RTCM/RtcmV3EphemerisHelper.cs new file mode 100644 index 0000000..7ce2207 --- /dev/null +++ b/src/Asv.Gnss/Parsers/RTCM/RtcmV3EphemerisHelper.cs @@ -0,0 +1,82 @@ +using System; +using Asv.IO; + +namespace Asv.Gnss +{ + public static class RtcmV3EphemerisHelper + { + /// + /// 2^-34 + /// + public const double P2_34 = 5.8207660913467407E-11; + + /// + /// 2^-44 + /// + public const double P2_44 = 1.4210854715202004E-14; + + /// + /// 2^-59 + /// + public const double P2_59 = 1.7347234759768071E-18; + + /// + /// 2^-66 + /// + public const double P2_66 = 1.35525271560688E-20; + + /// + /// Beidou started from 00:00:00UTC on Jan 1. 2006 of BDT. + /// + public static DateTime BdsStart => new(2006, 1, 1, 0, 0, 0, DateTimeKind.Utc); + + /// + /// The GST start epoch is defined as 13 seconds before midnight between 21st August and 22nd August 1999. + /// i.e. GST was equal 13 seconds at 22 nd August 1999 00:00:00 UTC. + /// + public static DateTime GalStart => new(1999, 8, 21, 23, 59, 47, DateTimeKind.Utc); + + /// + /// Returns the week and seconds between start and current date. + /// + /// + /// + /// + /// + public static void GetWeekFromTime(DateTime datum, DateTime time, ref int week, ref double seconds) + { + var dif = time - datum; + var weeks = (int)(dif.TotalDays / 7); + week = weeks; + dif = time - datum.AddDays(weeks * 7); + seconds = dif.TotalSeconds; + } + + /// + /// BDS Week number. Range 0 - 8191. Resolution - 1 week. + /// Roll-over every 8192 weeks starting fromm 00:00:00UTC on Jan 1. 2006 of BDT. + /// + public static int GetBdsWeek(DateTime utc, int week) + { + int w = 0; + var s = 0.0; + var gpsTime = RtcmV3Helper.Utc2Gps(utc); + var bdsTime = RtcmV3Helper.Gps2BeiDou(gpsTime); + GetWeekFromTime(BdsStart, bdsTime, ref w, ref s); + return week + (w - week + 1) / 8192 * 8192; + } + + /// + /// Galileo Week number. + /// Roll-over every 4096 (about 78 years). Galileo System Time (GST). + /// + public static int GetGalWeek(DateTime utc, int week) + { + int w = 0; + var s = 0.0; + var galTime = RtcmV3Helper.Utc2Gps(utc); + GetWeekFromTime(GalStart, galTime, ref w, ref s); + return week + (w - week + 1) / 4096 * 4096; + } + } + } \ No newline at end of file diff --git a/src/Asv.Gnss/Parsers/RTCM/RtcmV3Helper.cs b/src/Asv.Gnss/Parsers/RTCM/RtcmV3Helper.cs index fa79b3c..195f3bb 100644 --- a/src/Asv.Gnss/Parsers/RTCM/RtcmV3Helper.cs +++ b/src/Asv.Gnss/Parsers/RTCM/RtcmV3Helper.cs @@ -432,7 +432,7 @@ public static class RtcmV3Helper /// /// 2^-48 /// - public const double P2_48 = 3.552713678800501E-15; + public const double P2_46 = 3.552713678800501E-15; /// /// 2^-50 @@ -633,10 +633,13 @@ public static DateTime GetFromGps(int weeknumber, double seconds) var time = week.AddSeconds(seconds); return time; } - public static DateTime GetFromGalileo(int weeknumber, double seconds) { - var datum = new DateTime(1999, 8, 22, 0, 0, 0, DateTimeKind.Utc); + // var datum = new DateTime(1999, 8, 22, 0, 0, 0, DateTimeKind.Utc); + + // The GST start epoch is defined as 13 seconds before midnight between 21st August and 22nd August 1999. + // i.e. GST was equal 13 seconds at 22 nd August 1999 00:00:00 UTC. + var datum = new DateTime(1999, 8, 21, 23, 59, 47, DateTimeKind.Utc); var week = datum.AddDays(weeknumber * 7); var time = week.AddSeconds(seconds); return time; diff --git a/src/Asv.Gnss/Parsers/RTCM/V3/Messages/Ephemeris/RtcmV3Message1042.cs b/src/Asv.Gnss/Parsers/RTCM/V3/Messages/Ephemeris/RtcmV3Message1042.cs index ed011fd..de859af 100644 --- a/src/Asv.Gnss/Parsers/RTCM/V3/Messages/Ephemeris/RtcmV3Message1042.cs +++ b/src/Asv.Gnss/Parsers/RTCM/V3/Messages/Ephemeris/RtcmV3Message1042.cs @@ -9,11 +9,6 @@ namespace Asv.Gnss /// public class RtcmV3Message1042 : RtcmV3MessageBase { - /// - /// 2^-66 - /// - public const double P2_66 = 1.35525271560688E-20; - public static DateTime BdsStart => new(2006, 1, 1, 0, 0, 0, DateTimeKind.Utc); /// /// Rtcm Message Id /// @@ -45,7 +40,7 @@ protected override void DeserializeContent(ReadOnlySpan buffer, ref int bi IdotRaw = SpanBitHelper.GetBitS(buffer, ref bitIndex, 14) * RtcmV3Helper.P2_43; Aode = SpanBitHelper.GetBitU(buffer, ref bitIndex, 5); TocRaw = SpanBitHelper.GetBitU(buffer, ref bitIndex, 17) * 8; - A2 = SpanBitHelper.GetBitU(buffer, ref bitIndex, 11) * P2_66; + A2 = SpanBitHelper.GetBitU(buffer, ref bitIndex, 11) * RtcmV3EphemerisHelper.P2_66; A1 = SpanBitHelper.GetBitU(buffer, ref bitIndex, 22) * RtcmV3Helper.P2_50; A0 = SpanBitHelper.GetBitU(buffer, ref bitIndex, 24) * RtcmV3Helper.P2_33; Adoc = SpanBitHelper.GetBitU(buffer, ref bitIndex, 5); @@ -73,157 +68,187 @@ protected override void DeserializeContent(ReadOnlySpan buffer, ref int bi /// satellite number in all the constellations. /// public int SatelliteNumber { get; set; } + /// /// A BDS Satellite ID number from 1 to 37 refers to the PRN code of the BDS satellite. /// Satelilite ID's heigher then 37 are reserved. Range 1-63 /// public uint SatellitePrn { get; set; } + /// /// A BDS Satellite RINEX Code. /// public string SatelliteCode => RtcmV3Helper.Sat2Code(SatelliteNumber, (int)SatellitePrn); + /// /// BDS Week number. Range 0 - 8191. Resolution - 1 week. /// Roll-over every 8192 weeks starting fromm 00:00:00UTC on Jan 1. 2006 of BDT. /// public uint WeekRaw { get; set; } + /// /// BDS URAI Range 1 - 15. /// User Range Accuracy Index. /// public byte Urai { get; set; } + /// /// BDS IDOT Range +-9.31*10e-10. Resolution ^-43 /// Rate of inclination angle. Unit: pi/s /// public double IdotRaw { get; private set; } + /// /// BDS IDOT Range 0-31. /// Age of Data Ephemeris. /// public uint Aode { get; private set; } + /// /// The reference time of clock parameters. Unit: s. /// Range 0 - 604.792. Resolution: 8s. /// public uint TocRaw { get; private set; } + /// /// Clock correcton parameter Unit:s. /// Range: +-1.38E-17 s/s^2. Resolution: 2^-66 s/s^2 /// public double A2 { get; private set; } + /// /// Clock correcton parameter Unit:s. /// Range: +-1.86E-9 s/s^2. Resolution: 2^-50 s/s^2 /// public double A1 { get; private set; } + /// /// Clock correcton parameter Unit:s. /// Range: +-9.77E-4 s/s^2. Resolution: 2^-33 s/s^2 /// public double A0 { get; private set; } + /// /// Age of Data Clock. /// Range: 0-31. /// public uint Adoc { get; private set; } + /// /// Amplitude of sine harmonic correction term to the orbit radius. Unit: m. /// Range: +-2048. Resolution: 2^-6 m. /// public double Crs { get; private set; } + /// /// Mean moution difference from computed value. Unit: pi/s. /// Range: +-3.73E-9 pi/s. Resolution 2^-43 pi/s /// public double DeltaN { get; private set; } + /// /// Mean anomaly at reference time. Unit: pi. /// Range: +-pi. Resolution 2^-31 pi /// public double M0 { get; private set; } + /// /// Amplitude of cusine harmonic correction term to the argument of latitude. Unit: rad. /// Range: +-6.10E-5. Resolution: 2^-31 rad. /// public double Cuc { get; private set; } + /// /// Eccentricity. /// Range: 0 - 0.5. Resolution: 2^-33. /// public double e { get; private set; } + /// /// Amplitude of sine harmonic correction term to the argument of latitude. Unit: rad. /// Range: +-6.10E-5. Resolution: 2^-31 rad. /// public double Cus { get; private set; } + /// /// Square root of semi-major axis. Unit: m^1/2. /// Range: 0 - 8192 m^1/2. Resolution: 2^-19 m^1/2. /// public double Apow1_2 { get; private set; } + /// /// Ephemeris reference time. Unit: s. /// Range 0 - 604.792. Resolution: 8s. /// public uint ToeRaw { get; private set; } + /// /// Amplitude of cusine harmonic correction term to the argument of angle of inclination. Unit: rad. /// Range: +-6.10E-5. Resolution: 2^-31 rad. /// public double Cic { get; private set; } + /// /// Longitude of ascending node of orbital of phase computed according to reference time. Unit: pi. /// Range: +-pi. Resolution 2^-31 pi /// public double OmegaBig0 { get; private set; } + /// /// Amplitude of sine harmonic correction term to the argument of angle of inclination. Unit: rad. /// Range: +-6.10E-5. Resolution: 2^-31 rad. /// public double Cis { get; private set; } + /// /// Inclination angle at reference time. Unit: pi. /// Range: +-pi. Resolution 2^-31 pi /// public double I0 { get; private set; } + /// /// Amplitude of cosine harmonic correction term to the orbit radius. Unit: m. /// Range: +-2048. Resolution: 2^-6 m. /// public double Crc { get; private set; } + /// /// Argument of perigree. Unit: pi. /// Range: +-pi. Resolution 2^-31 pi /// public double Omega { get; private set; } + /// /// Rate of right ascension. Unit: pi/s. /// Range: +-9.54E-7 pi/s. Resolution: 2^-43 pi/s. /// public double OmegaBig { get; private set; } + /// /// Equipment Group Delay Differential. Unit: ns. /// Range: +-51.2 ns. Resolution: 0.1 ns. /// public double TGd1 { get; private set; } + /// /// Equipment Group Delay Differential. Unit: ns. /// Range: +-51.2 ns. Resolution: 0.1 ns. /// public double TGd2 { get; private set; } + /// /// Autonomous satelite health flag. /// Range: 0 - 1. Resolution: 1. /// public byte SvHealth { get; private set; } + /// /// BDS Week number. /// public int GetWeek(DateTime utc) { - return GetBdsWeek(utc, (int)WeekRaw); + return RtcmV3EphemerisHelper.GetBdsWeek(utc, (int)WeekRaw); } public DateTime GetToc() { @@ -238,27 +263,7 @@ public DateTime GetToe() { return RtcmV3Helper.GetFromBeiDou((int)WeekRaw, ToeRaw); } - public static int GetBdsWeek(DateTime utc, int week) - { - int w = 0; - var s = 0.0; - var gpsTime = RtcmV3Helper.Utc2Gps(utc); - var bdsTime = RtcmV3Helper.Gps2BeiDou(gpsTime); - GetFromTime(BdsStart, bdsTime, ref w, ref s); - return week + (w - week + 1) / 8192 * 8192; - } - public static void GetFromTime(DateTime datum, DateTime time, ref int week, ref double seconds) - { - var dif = time - datum; - var weeks = (int)(dif.TotalDays / 7); - - week = weeks; - - dif = time - datum.AddDays(weeks * 7); - - seconds = dif.TotalSeconds; - } } } diff --git a/src/Asv.Gnss/Parsers/RTCM/V3/Messages/Ephemeris/RtcmV3Message1046.cs b/src/Asv.Gnss/Parsers/RTCM/V3/Messages/Ephemeris/RtcmV3Message1046.cs index 8663691..421aa70 100644 --- a/src/Asv.Gnss/Parsers/RTCM/V3/Messages/Ephemeris/RtcmV3Message1046.cs +++ b/src/Asv.Gnss/Parsers/RTCM/V3/Messages/Ephemeris/RtcmV3Message1046.cs @@ -1,4 +1,5 @@ -using System; +using Asv.IO; +using System; namespace Asv.Gnss { @@ -18,9 +19,332 @@ public class RtcmV3Message1046 : RtcmV3MessageBase /// public override string Name => "Galileo I/NAV ephemeris information"; + /// protected override void DeserializeContent(ReadOnlySpan buffer, ref int bitIndex, int messageLength) { + var sys = NavigationSystemEnum.SYS_GAL; + + var prn = SpanBitHelper.GetBitU(buffer, ref bitIndex, 6); + var sat = RtcmV3Helper.satno(sys, (int)prn); + + if (sat == 0) + { + throw new Exception($"Rtcm3 1046 satellite number error: prn={prn}"); + } + + SatelliteNumber = sat; + SatellitePrn = prn; + WeekRaw = SpanBitHelper.GetBitU(buffer, ref bitIndex, 12); + IoDnav = SpanBitHelper.GetBitU(buffer, ref bitIndex, 10); + SvSisa = (byte)SpanBitHelper.GetBitU(buffer, ref bitIndex, 8); + IdotRaw = SpanBitHelper.GetBitS(buffer, ref bitIndex, 14) * RtcmV3Helper.P2_43; + TocRaw = SpanBitHelper.GetBitU(buffer, ref bitIndex, 14) * 60; + Af2 = SpanBitHelper.GetBitS(buffer, ref bitIndex, 6) * RtcmV3EphemerisHelper.P2_59; + Af1 = SpanBitHelper.GetBitS(buffer, ref bitIndex, 21) * RtcmV3Helper.P2_46; + Af0 = SpanBitHelper.GetBitS(buffer, ref bitIndex, 31) * RtcmV3EphemerisHelper.P2_34; + Crs = SpanBitHelper.GetBitS(buffer, ref bitIndex, 16) * RtcmV3Helper.P2_5; + DeltaN = SpanBitHelper.GetBitS(buffer, ref bitIndex, 16) * RtcmV3Helper.P2_43; + M0 = SpanBitHelper.GetBitS(buffer, ref bitIndex, 32) * RtcmV3Helper.P2_31; + Cuc = SpanBitHelper.GetBitS(buffer, ref bitIndex, 16) * RtcmV3Helper.P2_29; + e = SpanBitHelper.GetBitS(buffer, ref bitIndex, 32) * RtcmV3Helper.P2_33; + Cus = SpanBitHelper.GetBitS(buffer, ref bitIndex, 16) * RtcmV3Helper.P2_29; + Apow1_2 = SpanBitHelper.GetBitU(buffer, ref bitIndex, 32) * RtcmV3Helper.P2_19; + ToeRaw = SpanBitHelper.GetBitU(buffer, ref bitIndex, 14) * 60; + Cic = SpanBitHelper.GetBitS(buffer, ref bitIndex, 16) * RtcmV3Helper.P2_29; + OmegaBig0 = SpanBitHelper.GetBitS(buffer, ref bitIndex, 32) * RtcmV3Helper.P2_31; + Cis = SpanBitHelper.GetBitS(buffer, ref bitIndex, 16) * RtcmV3Helper.P2_29; + I0 = SpanBitHelper.GetBitS(buffer, ref bitIndex, 32) * RtcmV3Helper.P2_31; + Crc = SpanBitHelper.GetBitS(buffer, ref bitIndex, 16) * RtcmV3Helper.P2_5; + Omega = SpanBitHelper.GetBitS(buffer, ref bitIndex, 32) * RtcmV3Helper.P2_31; + OmegaDot = SpanBitHelper.GetBitS(buffer, ref bitIndex, 24) * RtcmV3Helper.P2_43; + BGdE1E5a = SpanBitHelper.GetBitS(buffer, ref bitIndex, 10) * RtcmV3Helper.P2_32; + BGdE5bE1 = SpanBitHelper.GetBitS(buffer, ref bitIndex, 10) * RtcmV3Helper.P2_32; + E5bSignalHealthFlag = (byte)SpanBitHelper.GetBitU(buffer, ref bitIndex, 2); + E5bDataValidity = (byte)SpanBitHelper.GetBitU(buffer, ref bitIndex, 1); + E1BSignalHealthFlag = (byte)SpanBitHelper.GetBitU(buffer, ref bitIndex, 2); + E1BDataValidity = (byte)SpanBitHelper.GetBitU(buffer, ref bitIndex, 1); + Reserved = (byte)SpanBitHelper.GetBitU(buffer, ref bitIndex, 1); + } + /// + /// Satellite number in all the constellations. + /// + public int SatelliteNumber { get; set; } + /// + /// A Galileo SVID parameter is coded with 6 bits. + /// However the ma constellation which can be accomodated within the I/NAV and F/NAV frames is 36 satellites + /// (3 planes of 12 satellites each) + /// + public uint SatellitePrn { get; set; } + /// + /// A BDS Satellite RINEX Code. + /// + public string SatelliteCode => RtcmV3Helper.Sat2Code(SatelliteNumber, (int)SatellitePrn); + /// + /// Galileo Week number. Range 0 - 4095 weeks. Resolution - 1 week. + /// Roll-over every 4096 (about 78 years). Galileo System Time (GST). + /// The GST start epoch is defined as 13 seconds before midnight between 21st August and 22nd August 1999. + /// i.e. GST was equal 13 seconds at 22 nd August 1999 00:00:00 UTC. + /// + public uint WeekRaw { get; set; } + /// + /// Issue of Data for navigation data (IODnav). + /// Each IoDNav has an associated reference time parameter disseminated within the batch. + /// Note: the broadcast group delay validity status and signal health status are not identified by any Issue of Data value unitless. + /// + public uint IoDnav { get; set; } + /// + /// SIS Accuracy + /// + public byte SvSisa { get; set; } + + /// + /// IDOT. Effective range attainable with the indicated bit allocation and scale factor. Resolution ^-43 + /// Rate of inclination angle. Unit: semi-circles/s + /// + public double IdotRaw { get; private set; } + /// + /// The reference time of clock parameters. Unit: s. + /// Range 0 - 604.792. Resolution: 60. + /// + public uint TocRaw { get; private set; } + /// + /// Clock correcton parameter Unit: s/s^2. + /// Effective range attainable with the indicated bit allocation and scale factor. Resolution: 2^-59. + /// + public double Af2 { get; private set; } + /// + /// Clock correcton parameter Unit:s/s. + /// Effective range attainable with the indicated bit allocation and scale factor. Resolution: 2^-46 + /// + public double Af1 { get; private set; } + /// + /// Clock correcton parameter Unit:s. + /// Effective range attainable with the indicated bit allocation and scale factor. Resolution: 2^-34 + /// + public double Af0 { get; private set; } + /// + /// Amplitude of sine harmonic correction term to the orbit radius. Unit: m. + /// Effective range attainable with the indicated bit allocation and scale factor. Resolution: 2^-5 m. + /// + public double Crs { get; private set; } + + /// + /// Mean moution difference from computed value. Unit: pi/s. + /// Effective range attainable with the indicated bit allocation and scale factor. Resolution 2^-43 pi/s + /// + public double DeltaN { get; private set; } + + /// + /// Mean anomaly at reference time. Unit: pi. + /// Effective range attainable with the indicated bit allocation and scale factor. Resolution 2^-31 pi + /// + public double M0 { get; private set; } + + /// + /// Amplitude of cusine harmonic correction term to the argument of latitude. Unit: rad. + /// Effective range attainable with the indicated bit allocation and scale factor. Resolution: 2^-29 rad. + /// + public double Cuc { get; private set; } + + /// + /// Eccentricity. + /// Effective range attainable with the indicated bit allocation and scale factor. Resolution: 2^-33. + /// + public double e { get; private set; } + + /// + /// Amplitude of sine harmonic correction term to the argument of latitude. Unit: rad. + /// Effective range attainable with the indicated bit allocation and scale factor. Resolution: 2^-29 rad. + /// + public double Cus { get; private set; } + + /// + /// Square root of semi-major axis. Unit: m^1/2. + /// Effective range attainable with the indicated bit allocation and scale factor. Resolution: 2^-19 m^1/2. + /// + public double Apow1_2 { get; private set; } + + /// + /// Ephemeris reference time. Unit: s. + /// Range 0 - 604.792. Resolution: 60. + /// + public uint ToeRaw { get; private set; } + + /// + /// Amplitude of cusine harmonic correction term to the argument of angle of inclination. Unit: rad. + /// Effective range attainable with the indicated bit allocation and scale factor. Resolution: 2^-29 rad. + /// + public double Cic { get; private set; } + + /// + /// Longitude of ascending node of orbital plane at weekly epoch. Unit: pi. + /// Effective range attainable with the indicated bit allocation and scale factor. Resolution 2^-31 pi + /// + public double OmegaBig0 { get; private set; } + + /// + /// Amplitude of sine harmonic correction term to the angle of inclination. Unit: rad. + /// Effective range attainable with the indicated bit allocation and scale factor. Resolution: 2^-29 rad. + /// + public double Cis { get; private set; } + + /// + /// Inclination angle at reference time. Unit: pi. + /// Effective range attainable with the indicated bit allocation and scale factor. Resolution 2^-31 pi + /// + public double I0 { get; private set; } + + /// + /// Amplitude of cosine harmonic correction term to the orbit radius. Unit: m. + /// Effective range attainable with the indicated bit allocation and scale factor. Resolution: 2^-5 m. + /// + public double Crc { get; private set; } + + /// + /// Argument of Perigree. Unit: pi. + /// Effective range attainable with the indicated bit allocation and scale factor. Resolution 2^-31 pi + /// + public double Omega { get; private set; } + + /// + /// Rate of right ascension. Unit: pi/s. + /// Effective range attainable with the indicated bit allocation and scale factor. Resolution: 2^-43 pi/s. + /// + public double OmegaDot { get; private set; } + + /// + /// E1/E5a Broadcast Group Delay. + /// Effective range attainable with the indicated bit allocation and scale factor. Resolution: 2^-32. + /// + public double BGdE1E5a { get; private set; } + + /// + /// E5b/E1 Broadcast Group Delay (reserved). + /// Effective range attainable with the indicated bit allocation and scale factor. Resolution: 2^-32. + /// + public double BGdE5bE1 { get; private set; } + + /// + /// E5a Signal Health Status. Bit Values are: + /// 0 - Signal OK + /// 1 - Signal out of service + /// 2 - Signal will be out of service + /// 3 - Signal Component currently in Test + /// + public byte E5bSignalHealthFlag { get; private set; } + + /// + /// The navigation data validity status transmitted on E5a. + /// + public byte E5bDataValidity { get; private set; } + + /// + /// The E5b Signal Health Status. Bit Values are: + /// 0 - Signal OK + /// 1 - Signal out of service + /// 2 - Signal will be out of service + /// 3 - Signal Component currently in Test + /// + public byte E1BSignalHealthFlag { get; private set; } + + /// + /// The navigation data validity status transmitted on E5b. + /// + public byte E1BDataValidity { get; private set; } + + /// + /// Galileo Week number. + /// + public int GetWeek(DateTime utc) + { + return RtcmV3EphemerisHelper.GetGalWeek(utc, (int)WeekRaw); + } + + + public DateTime GetToc() + { + return RtcmV3Helper.GetFromGalileo((int)WeekRaw, TocRaw); + } + public double Idot => Idot * RtcmV3Helper.SC2RAD; + public DateTime GetToc(DateTime utc) + { + return RtcmV3Helper.GetFromGalileo(GetWeek(utc), TocRaw); + } + public DateTime GetToe() + { + return RtcmV3Helper.GetFromGalileo((int)WeekRaw, ToeRaw); + } + + public bool IsE5bSignalOk + { + get + { + return ((SignalHealthy)E5bSignalHealthFlag & SignalHealthy.OK) == SignalHealthy.OK; + } + } + + public bool IsE5bSignalOuOfService + { + get + { + return ((SignalHealthy)E5bSignalHealthFlag & SignalHealthy.OutOfService) == SignalHealthy.OutOfService; + } + } + + public bool IsE5bSignalWillOuOfService + { + get + { + return ((SignalHealthy)E5bSignalHealthFlag & SignalHealthy.WillBeOutOfService) == SignalHealthy.WillBeOutOfService; + } + } + public bool IsE5bSignalInTest + { + get + { + return ((SignalHealthy)E5bSignalHealthFlag & SignalHealthy.InTest) == SignalHealthy.InTest; + } + } + + public bool IsE1BSignalOk + { + get + { + return ((SignalHealthy)E1BSignalHealthFlag & SignalHealthy.OK) == SignalHealthy.OK; + } + } + + public bool E1BSignalOuOfService + { + get + { + return ((SignalHealthy)E1BSignalHealthFlag & SignalHealthy.OutOfService) == SignalHealthy.OutOfService; + } + } + + public bool E1BSignalWillOuOfService + { + get + { + return ((SignalHealthy)E1BSignalHealthFlag & SignalHealthy.WillBeOutOfService) == SignalHealthy.WillBeOutOfService; + } + } + public bool E1BSignalInTest + { + get + { + return ((SignalHealthy)E1BSignalHealthFlag & SignalHealthy.InTest) == SignalHealthy.InTest; + } + } + + [Flags] + public enum SignalHealthy + { + OK = 0x0, + OutOfService= 0x1, + WillBeOutOfService = 0x2, + InTest = 0x3 } } } From 34f1203a5cd078aef36c137ce0f80351fa7a53d7 Mon Sep 17 00:00:00 2001 From: ndvorkina Date: Tue, 5 Sep 2023 16:13:00 +0300 Subject: [PATCH 3/3] commit after merge --- src/Asv.Gnss.Test/RTCMv3TestExt.cs | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/src/Asv.Gnss.Test/RTCMv3TestExt.cs b/src/Asv.Gnss.Test/RTCMv3TestExt.cs index a0b9a3b..9bfa01a 100644 --- a/src/Asv.Gnss.Test/RTCMv3TestExt.cs +++ b/src/Asv.Gnss.Test/RTCMv3TestExt.cs @@ -1,5 +1,4 @@ -using Asv.IO; -using System; +using System; using System.Reactive.Linq; using Xunit;