Context Navigation

source: ntrip/trunk/BNC/src/RTCM3/ephEncoder.cpp@ 7844

Last change on this file since 7844 was 7844, checked in by stuerze, 8 years ago
bug fixed in GLONASS ephemeris encoding
File size: 14.3 KB

Rev	Line
[5852]	1
	2	#include "ephEncoder.h"
[6139]	3
[5852]	4	using namespace std;
	5
	6
	7	// build up RTCM3 for GPS
	8	////////////////////////////////////////////////////////////////////////////
	9	int t_ephEncoder::RTCM3(const t_ephGPS& eph, unsigned char *buffer) {
	10
	11	unsigned char *startbuffer = buffer;
	12	buffer= buffer+3;
	13	int size = 0;
	14	int numbits = 0;
	15	unsigned long long bitbuffer = 0;
[6819]	16	int ura = indexFromAccuracy(eph._ura, eph.type());
[6814]	17	if (eph.type() == t_eph::QZSS) {
	18	GPSADDBITS(12, 1044)
[6819]	19	GPSADDBITS(4,eph._prn.number())
	20	GPSADDBITS(16, static_cast<int>(eph._TOC.gpssec())>>4)
	21	GPSADDBITSFLOAT(8, eph._clock_driftrate, 1.0/static_cast<double>(1<<30)
	22	/static_cast<double>(1<<25))
	23	GPSADDBITSFLOAT(16, eph._clock_drift, 1.0/static_cast<double>(1<<30)
	24	/static_cast<double>(1<<13))
	25	GPSADDBITSFLOAT(22, eph._clock_bias, 1.0/static_cast<double>(1<<30)
	26	/static_cast<double>(1<<1))
	27	GPSADDBITS(8, eph._IODE)
	28	GPSADDBITSFLOAT(16, eph._Crs, 1.0/static_cast<double>(1<<5))
	29	GPSADDBITSFLOAT(16, eph._Delta_n, M_PI/static_cast<double>(1<<30)
	30	/static_cast<double>(1<<13))
	31	GPSADDBITSFLOAT(32, eph._M0, M_PI/static_cast<double>(1<<30)/static_cast<double>(1<<1))
	32	GPSADDBITSFLOAT(16, eph._Cuc, 1.0/static_cast<double>(1<<29))
	33	GPSADDBITSFLOAT(32, eph._e, 1.0/static_cast<double>(1<<30)/static_cast<double>(1<<3))
	34	GPSADDBITSFLOAT(16, eph._Cus, 1.0/static_cast<double>(1<<29))
	35	GPSADDBITSFLOAT(32, eph._sqrt_A, 1.0/static_cast<double>(1<<19))
	36	GPSADDBITS(16, static_cast<int>(eph._TOEsec)>>4)
	37	GPSADDBITSFLOAT(16, eph._Cic, 1.0/static_cast<double>(1<<29))
	38	GPSADDBITSFLOAT(32, eph._OMEGA0, M_PI/static_cast<double>(1<<30)
	39	/static_cast<double>(1<<1))
	40	GPSADDBITSFLOAT(16, eph._Cis, 1.0/static_cast<double>(1<<29))
	41	GPSADDBITSFLOAT(32, eph._i0, M_PI/static_cast<double>(1<<30)/static_cast<double>(1<<1))
	42	GPSADDBITSFLOAT(16, eph._Crc, 1.0/static_cast<double>(1<<5))
	43	GPSADDBITSFLOAT(32, eph._omega, M_PI/static_cast<double>(1<<30)
	44	/static_cast<double>(1<<1))
	45	GPSADDBITSFLOAT(24, eph._OMEGADOT, M_PI/static_cast<double>(1<<30)
	46	/static_cast<double>(1<<13))
	47	GPSADDBITSFLOAT(14, eph._IDOT, M_PI/static_cast<double>(1<<30)
	48	/static_cast<double>(1<<13))
	49	GPSADDBITS(2,eph._L2Codes)
	50	GPSADDBITS(10, eph._TOC.gpsw())
	51	GPSADDBITS(4, ura)
	52	GPSADDBITS(6, eph._health)
	53	GPSADDBITSFLOAT(8, eph._TGD, 1.0/static_cast<double>(1<<30)/static_cast<double>(1<<1))
	54	GPSADDBITS(10, eph._IODC)
	55	GPSADDBITS(1, eph._fitInterval)
	56	GPSADDBITS(3, 0) /* padding */
[6814]	57	}
	58	else {
	59	GPSADDBITS(12, 1019)
[6819]	60	GPSADDBITS(6,eph._prn.number())
	61	GPSADDBITS(10, eph._TOC.gpsw())
	62	GPSADDBITS(4, ura)
	63	GPSADDBITS(2,eph._L2Codes)
	64	GPSADDBITSFLOAT(14, eph._IDOT, M_PI/static_cast<double>(1<<30)
	65	/static_cast<double>(1<<13))
	66	GPSADDBITS(8, eph._IODE)
	67	GPSADDBITS(16, static_cast<int>(eph._TOC.gpssec())>>4)
	68	GPSADDBITSFLOAT(8, eph._clock_driftrate, 1.0/static_cast<double>(1<<30)
	69	/static_cast<double>(1<<25))
	70	GPSADDBITSFLOAT(16, eph._clock_drift, 1.0/static_cast<double>(1<<30)
	71	/static_cast<double>(1<<13))
	72	GPSADDBITSFLOAT(22, eph._clock_bias, 1.0/static_cast<double>(1<<30)
	73	/static_cast<double>(1<<1))
	74	GPSADDBITS(10, eph._IODC)
	75	GPSADDBITSFLOAT(16, eph._Crs, 1.0/static_cast<double>(1<<5))
	76	GPSADDBITSFLOAT(16, eph._Delta_n, M_PI/static_cast<double>(1<<30)
	77	/static_cast<double>(1<<13))
	78	GPSADDBITSFLOAT(32, eph._M0, M_PI/static_cast<double>(1<<30)/static_cast<double>(1<<1))
	79	GPSADDBITSFLOAT(16, eph._Cuc, 1.0/static_cast<double>(1<<29))
	80	GPSADDBITSFLOAT(32, eph._e, 1.0/static_cast<double>(1<<30)/static_cast<double>(1<<3))
	81	GPSADDBITSFLOAT(16, eph._Cus, 1.0/static_cast<double>(1<<29))
	82	GPSADDBITSFLOAT(32, eph._sqrt_A, 1.0/static_cast<double>(1<<19))
	83	GPSADDBITS(16, static_cast<int>(eph._TOEsec)>>4)
	84	GPSADDBITSFLOAT(16, eph._Cic, 1.0/static_cast<double>(1<<29))
	85	GPSADDBITSFLOAT(32, eph._OMEGA0, M_PI/static_cast<double>(1<<30)
	86	/static_cast<double>(1<<1))
	87	GPSADDBITSFLOAT(16, eph._Cis, 1.0/static_cast<double>(1<<29))
	88	GPSADDBITSFLOAT(32, eph._i0, M_PI/static_cast<double>(1<<30)/static_cast<double>(1<<1))
	89	GPSADDBITSFLOAT(16, eph._Crc, 1.0/static_cast<double>(1<<5))
	90	GPSADDBITSFLOAT(32, eph._omega, M_PI/static_cast<double>(1<<30)
	91	/static_cast<double>(1<<1))
	92	GPSADDBITSFLOAT(24, eph._OMEGADOT, M_PI/static_cast<double>(1<<30)
	93	/static_cast<double>(1<<13))
	94	GPSADDBITSFLOAT(8, eph._TGD, 1.0/static_cast<double>(1<<30)/static_cast<double>(1<<1))
[7844]	95	GPSADDBITS(6, eph._health)
[6819]	96	GPSADDBITS(1, eph._L2PFlag)
	97	GPSADDBITS(1, eph._fitInterval)
[6814]	98	}
[5852]	99
	100	startbuffer[0]=0xD3;
	101	startbuffer[1]=(size >> 8);
	102	startbuffer[2]=size;
	103	unsigned long i = CRC24(size+3, startbuffer);
	104	buffer[size++] = i >> 16;
	105	buffer[size++] = i >> 8;
	106	buffer[size++] = i;
	107	size += 3;
	108	return size;
	109	}
	110
	111	// build up RTCM3 for GLONASS
	112	////////////////////////////////////////////////////////////////////////////
	113	int t_ephEncoder::RTCM3(const t_ephGlo& eph, unsigned char *buffer)
	114	{
	115
	116	int size = 0;
	117	int numbits = 0;
	118	long long bitbuffer = 0;
	119	unsigned char *startbuffer = buffer;
	120	buffer= buffer+3;
	121
	122	GLONASSADDBITS(12, 1020)
[5853]	123	GLONASSADDBITS(6, eph._prn.number())
	124	GLONASSADDBITS(5, 7+eph._frequency_number)
[5852]	125	GLONASSADDBITS(1, 0)
	126	GLONASSADDBITS(1, 0)
	127	GLONASSADDBITS(2, 0)
[7844]	128	int tki = static_cast<int>(eph._tki)+36060;
	129	if (tki > 86400) {tki -= 86400;}
	130	GLONASSADDBITS(5, tki/(60*60))
	131	GLONASSADDBITS(6, (tki/60)%60)
	132	GLONASSADDBITS(1, (tki/30)%30)
	133	GLONASSADDBITS(1, eph._health)
[5852]	134	GLONASSADDBITS(1, 0)
[5853]	135	unsigned long long timeofday = (static_cast<int>(eph._tt.gpssec()+36060-eph._gps_utc)%86400);
[5852]	136	GLONASSADDBITS(7, timeofday/60/15)
[5853]	137	GLONASSADDBITSFLOATM(24, eph._x_velocity*1000, 1000.0/static_cast<double>(1<<20))
	138	GLONASSADDBITSFLOATM(27, eph._x_pos*1000, 1000.0/static_cast<double>(1<<11))
	139	GLONASSADDBITSFLOATM(5, eph._x_acceleration*1000, 1000.0/static_cast<double>(1<<30))
	140	GLONASSADDBITSFLOATM(24, eph._y_velocity*1000, 1000.0/static_cast<double>(1<<20))
	141	GLONASSADDBITSFLOATM(27, eph._y_pos*1000, 1000.0/static_cast<double>(1<<11))
	142	GLONASSADDBITSFLOATM(5, eph._y_acceleration*1000, 1000.0/static_cast<double>(1<<30))
	143	GLONASSADDBITSFLOATM(24, eph._z_velocity*1000, 1000.0/static_cast<double>(1<<20))
	144	GLONASSADDBITSFLOATM(27,eph._z_pos*1000, 1000.0/static_cast<double>(1<<11))
	145	GLONASSADDBITSFLOATM(5, eph._z_acceleration*1000, 1000.0/static_cast<double>(1<<30))
[5852]	146	GLONASSADDBITS(1, 0)
[5853]	147	GLONASSADDBITSFLOATM(11, eph._gamma, 1.0/static_cast<double>(1<<30)
[5852]	148	/static_cast<double>(1<<10))
	149	GLONASSADDBITS(2, 0) /* GLONASS-M P */
	150	GLONASSADDBITS(1, 0) /* GLONASS-M ln(3) */
[5853]	151	GLONASSADDBITSFLOATM(22, eph._tau, 1.0/static_cast<double>(1<<30))
[5852]	152	GLONASSADDBITS(5, 0) /* GLONASS-M delta tau */
[5853]	153	GLONASSADDBITS(5, eph._E)
[5852]	154	GLONASSADDBITS(1, 0) /* GLONASS-M P4 */
	155	GLONASSADDBITS(4, 0) /* GLONASS-M FT */
	156	GLONASSADDBITS(11, 0) /* GLONASS-M NT */
	157	GLONASSADDBITS(2, 0) /* GLONASS-M active? */
	158	GLONASSADDBITS(1, 0) /* GLONASS additional data */
	159	GLONASSADDBITS(11, 0) /* GLONASS NA */
	160	GLONASSADDBITS(32, 0) /* GLONASS tau C */
	161	GLONASSADDBITS(5, 0) /* GLONASS-M N4 */
	162	GLONASSADDBITS(22, 0) /* GLONASS-M tau GPS */
	163	GLONASSADDBITS(1, 0) /* GLONASS-M ln(5) */
	164	GLONASSADDBITS(7, 0) /* Reserved */
	165
	166	startbuffer[0]=0xD3;
	167	startbuffer[1]=(size >> 8);
	168	startbuffer[2]=size;
	169	unsigned long i = CRC24(size+3, startbuffer);
	170	buffer[size++] = i >> 16;
	171	buffer[size++] = i >> 8;
	172	buffer[size++] = i;
	173	size += 3;
	174	return size;
	175	}
	176
	177	// build up RTCM3 for Galileo
	178	////////////////////////////////////////////////////////////////////////////
	179	int t_ephEncoder::RTCM3(const t_ephGal& eph, unsigned char *buffer) {
	180	int size = 0;
	181	int numbits = 0;
	182	long long bitbuffer = 0;
	183	unsigned char *startbuffer = buffer;
	184	buffer= buffer+3;
	185
[6819]	186	int SISA = indexFromAccuracy(eph._SISA, eph.type());
[6799]	187
[6812]	188	GALILEOADDBITS(12, eph._inav ? 1046 : 1045)
[5853]	189	GALILEOADDBITS(6, eph._prn.number())
[6869]	190	GALILEOADDBITS(12, eph._TOC.galw())
[5853]	191	GALILEOADDBITS(10, eph._IODnav)
[6819]	192	GALILEOADDBITS(8, SISA)
[5853]	193	GALILEOADDBITSFLOAT(14, eph._IDOT, M_PI/static_cast<double>(1<<30)
[5852]	194	/static_cast<double>(1<<13))
[5853]	195	GALILEOADDBITS(14, eph._TOC.gpssec()/60)
	196	GALILEOADDBITSFLOAT(6, eph._clock_driftrate, 1.0/static_cast<double>(1<<30)
[5852]	197	/static_cast<double>(1<<29))
[5853]	198	GALILEOADDBITSFLOAT(21, eph._clock_drift, 1.0/static_cast<double>(1<<30)
[5852]	199	/static_cast<double>(1<<16))
[5853]	200	GALILEOADDBITSFLOAT(31, eph._clock_bias, 1.0/static_cast<double>(1<<30)
[5852]	201	/static_cast<double>(1<<4))
[5853]	202	GALILEOADDBITSFLOAT(16, eph._Crs, 1.0/static_cast<double>(1<<5))
	203	GALILEOADDBITSFLOAT(16, eph._Delta_n, M_PI/static_cast<double>(1<<30)
[5852]	204	/static_cast<double>(1<<13))
[5853]	205	GALILEOADDBITSFLOAT(32, eph._M0, M_PI/static_cast<double>(1<<30)/static_cast<double>(1<<1))
	206	GALILEOADDBITSFLOAT(16, eph._Cuc, 1.0/static_cast<double>(1<<29))
	207	GALILEOADDBITSFLOAT(32, eph._e, 1.0/static_cast<double>(1<<30)/static_cast<double>(1<<3))
	208	GALILEOADDBITSFLOAT(16, eph._Cus, 1.0/static_cast<double>(1<<29))
	209	GALILEOADDBITSFLOAT(32, eph._sqrt_A, 1.0/static_cast<double>(1<<19))
	210	GALILEOADDBITS(14, eph._TOEsec/60)
	211	GALILEOADDBITSFLOAT(16, eph._Cic, 1.0/static_cast<double>(1<<29))
	212	GALILEOADDBITSFLOAT(32, eph._OMEGA0, M_PI/static_cast<double>(1<<30)
[5852]	213	/static_cast<double>(1<<1))
[5853]	214	GALILEOADDBITSFLOAT(16, eph._Cis, 1.0/static_cast<double>(1<<29))
	215	GALILEOADDBITSFLOAT(32, eph._i0, M_PI/static_cast<double>(1<<30)/static_cast<double>(1<<1))
	216	GALILEOADDBITSFLOAT(16, eph._Crc, 1.0/static_cast<double>(1<<5))
	217	GALILEOADDBITSFLOAT(32, eph._omega, M_PI/static_cast<double>(1<<30)
[5852]	218	/static_cast<double>(1<<1))
[5853]	219	GALILEOADDBITSFLOAT(24, eph._OMEGADOT, M_PI/static_cast<double>(1<<30)
[5852]	220	/static_cast<double>(1<<13))
[5853]	221	GALILEOADDBITSFLOAT(10, eph._BGD_1_5A, 1.0/static_cast<double>(1<<30)
[5852]	222	/static_cast<double>(1<<2))
[6812]	223	if(eph._inav)
[5852]	224	{
[5853]	225	GALILEOADDBITSFLOAT(10, eph._BGD_1_5B, 1.0/static_cast<double>(1<<30)
[5852]	226	/static_cast<double>(1<<2))
[5853]	227	GALILEOADDBITS(2, static_cast<int>(eph._E5bHS))
[6812]	228	GALILEOADDBITS(1, eph._e5bDataInValid ? 1 : 0)
[6818]	229	GALILEOADDBITS(2, static_cast<int>(eph._E1_bHS))
	230	GALILEOADDBITS(1, eph._e1DataInValid ? 1 : 0)
[5852]	231	}
	232	else
	233	{
[5853]	234	GALILEOADDBITS(2, static_cast<int>(eph._E5aHS))
[6812]	235	GALILEOADDBITS(1, eph._e5aDataInValid ? 1 : 0)
[5852]	236	}
	237
[6818]	238	GALILEOADDBITS((eph._inav ? 2 : 7), 0)
[5852]	239
	240	startbuffer[0]=0xD3;
	241	startbuffer[1]=(size >> 8);
	242	startbuffer[2]=size;
	243	unsigned long i = CRC24(size+3, startbuffer);
	244	buffer[size++] = i >> 16;
	245	buffer[size++] = i >> 8;
	246	buffer[size++] = i;
	247	size += 3;
	248	return size;
	249	}
	250
[6615]	251	// build up RTCM3 for SBAS
	252	////////////////////////////////////////////////////////////////////////////
[6631]	253	int t_ephEncoder::RTCM3(const t_ephSBAS& eph, unsigned char* buffer) {
	254	int size = 0;
	255	int numbits = 0;
	256	long long bitbuffer = 0;
	257	unsigned char *startbuffer = buffer;
	258	buffer= buffer+3;
	259
[6819]	260	int ura = indexFromAccuracy(eph._ura, eph.type());
[6631]	261	SBASADDBITS(12, 1043)
	262	SBASADDBITS(6, eph._prn.number()-20)
	263	SBASADDBITS(8, eph._IODN)
	264	SBASADDBITS(13, static_cast<int>(eph._TOC.daysec())>>4)
[6819]	265	SBASADDBITS(4, ura)
[6631]	266	SBASADDBITSFLOAT(30, eph._x_pos, 0.08)
	267	SBASADDBITSFLOAT(30, eph._y_pos, 0.08)
	268	SBASADDBITSFLOAT(25, eph._z_pos, 0.4)
	269	SBASADDBITSFLOAT(17, eph._x_velocity, 0.000625)
	270	SBASADDBITSFLOAT(17, eph._y_velocity, 0.000625)
	271	SBASADDBITSFLOAT(18, eph._z_velocity, 0.004)
	272	SBASADDBITSFLOAT(10, eph._x_acceleration, 0.0000125)
	273	SBASADDBITSFLOAT(10, eph._y_acceleration, 0.0000125)
	274	SBASADDBITSFLOAT(10, eph._z_acceleration, 0.0000625)
	275	SBASADDBITSFLOAT(12, eph._agf0, 1.0/static_cast<double>(1<<30)/static_cast<double>(1<<1))
	276	SBASADDBITSFLOAT(8, eph._agf1, 1.0/static_cast<double>(1<<30)/static_cast<double>(1<<10))
[6635]	277	SBASADDBITS(2,0);
[6631]	278
	279	startbuffer[0]=0xD3;
	280	startbuffer[1]=(size >> 8);
	281	startbuffer[2]=size;
	282	unsigned long i = CRC24(size+3, startbuffer);
	283	buffer[size++] = i >> 16;
	284	buffer[size++] = i >> 8;
	285	buffer[size++] = i;
	286	size += 3;
	287	return size;
[6403]	288	}
	289
[6615]	290	// build up RTCM3 for BDS
	291	////////////////////////////////////////////////////////////////////////////
	292	int t_ephEncoder::RTCM3(const t_ephBDS& eph, unsigned char* buffer) {
	293	int size = 0;
	294	int numbits = 0;
	295	long long bitbuffer = 0;
	296	unsigned char *startbuffer = buffer;
	297	buffer= buffer+3;
	298
[6819]	299	int URA = indexFromAccuracy(eph._URA, eph.type());
[6615]	300	BDSADDBITS(12, RTCM3ID_BDS)
	301	BDSADDBITS(6, eph._prn.number())
[6812]	302	BDSADDBITS(13, eph._TOC.bdsw() - 1356.0)
[6819]	303	BDSADDBITS(4, URA);
[6615]	304	BDSADDBITSFLOAT(14, eph._IDOT, M_PI/static_cast<double>(1<<30)/static_cast<double>(1<<13))
	305	BDSADDBITS(5, eph._AODE)
[6812]	306	BDSADDBITS(17, static_cast<int>(eph._TOC.bdssec())>>3)
[6622]	307	BDSADDBITSFLOAT(11, eph._clock_driftrate, 1.0/static_cast<double>(1<<30)
	308	/static_cast<double>(1<<30)/static_cast<double>(1<<6))
[6615]	309	BDSADDBITSFLOAT(22, eph._clock_drift, 1.0/static_cast<double>(1<<30)/static_cast<double>(1<<20))
	310	BDSADDBITSFLOAT(24, eph._clock_bias, 1.0/static_cast<double>(1<<30)/static_cast<double>(1<<3))
	311	BDSADDBITS(5, eph._AODC)
	312	BDSADDBITSFLOAT(18, eph._Crs, 1.0/static_cast<double>(1<<6))
	313	BDSADDBITSFLOAT(16, eph._Delta_n, M_PI/static_cast<double>(1<<30)/static_cast<double>(1<<13))
	314	BDSADDBITSFLOAT(32, eph._M0, M_PI/static_cast<double>(1<<30)/static_cast<double>(1<<1))
	315	BDSADDBITSFLOAT(18, eph._Cuc, 1.0/static_cast<double>(1<<30)/static_cast<double>(1<<1))
	316	BDSADDBITSFLOAT(32, eph._e, 1.0/static_cast<double>(1<<30)/static_cast<double>(1<<3))
	317	BDSADDBITSFLOAT(18, eph._Cus, 1.0/static_cast<double>(1<<30)/static_cast<double>(1<<1))
	318	BDSADDBITSFLOAT(32, eph._sqrt_A, 1.0/static_cast<double>(1<<19))
[6812]	319	BDSADDBITS(17, static_cast<int>(eph._TOE.bdssec())>>3)
[6615]	320	BDSADDBITSFLOAT(18, eph._Cic, 1.0/static_cast<double>(1<<30)/static_cast<double>(1<<1))
	321	BDSADDBITSFLOAT(32, eph._OMEGA0, M_PI/static_cast<double>(1<<30)/static_cast<double>(1<<1))
	322	BDSADDBITSFLOAT(18, eph._Cis, 1.0/static_cast<double>(1<<30)/static_cast<double>(1<<1))
	323	BDSADDBITSFLOAT(32, eph._i0, M_PI/static_cast<double>(1<<30)/static_cast<double>(1<<1))
[6619]	324	BDSADDBITSFLOAT(18, eph._Crc, 1.0/static_cast<double>(1<<6))
[6615]	325	BDSADDBITSFLOAT(32, eph._omega, M_PI/static_cast<double>(1<<30)/static_cast<double>(1<<1))
	326	BDSADDBITSFLOAT(24, eph._OMEGADOT, M_PI/static_cast<double>(1<<30)/static_cast<double>(1<<13))
	327	BDSADDBITSFLOAT(10, eph._TGD1, 0.0000000001)
	328	BDSADDBITSFLOAT(10, eph._TGD2, 0.0000000001)
	329	BDSADDBITS(1, eph._SatH1)
[6627]	330	BDSADDBITS(1, 0) /* reserved bit, fill up 8 bits */
[6615]	331
	332	startbuffer[0]=0xD3;
	333	startbuffer[1]=(size >> 8);
	334	startbuffer[2]=size;
	335	unsigned long i = CRC24(size+3, startbuffer);
	336	buffer[size++] = i >> 16;
	337	buffer[size++] = i >> 8;
	338	buffer[size++] = i;
	339	size += 3;
	340	return size;
[6403]	341	}
[7053]	342

Note: See TracBrowser for help on using the repository browser.

Download in other formats: