Context Navigation

source: ntrip/trunk/BNC/src/upload/bncrtnetuploadcaster.cpp@ 5633

Last change on this file since 5633 was 5510, checked in by mervart, 11 years ago

File size: 23.9 KB

Rev	Line
[3222]	1	/* -------------------------------------------------------------------------
	2	* BKG NTRIP Server
	3	* -------------------------------------------------------------------------
	4	*
[3224]	5	* Class: bncRtnetUploadCaster
[3222]	6	*
	7	* Purpose: Connection to NTRIP Caster
	8	*
	9	* Author: L. Mervart
	10	*
	11	* Created: 29-Mar-2011
	12	*
	13	* Changes:
	14	*
	15	* -----------------------------------------------------------------------*/
	16
	17	#include <math.h>
[3224]	18	#include "bncrtnetuploadcaster.h"
[3222]	19	#include "bncsettings.h"
[3224]	20	#include "bncephuser.h"
[3222]	21	#include "bncclockrinex.h"
	22	#include "bncsp3.h"
	23
	24	using namespace std;
	25
	26	// Constructor
	27	////////////////////////////////////////////////////////////////////////////
[3224]	28	bncRtnetUploadCaster::bncRtnetUploadCaster(const QString& mountpoint,
[3222]	29	const QString& outHost, int outPort,
	30	const QString& password,
	31	const QString& crdTrafo, bool CoM,
	32	const QString& sp3FileName,
	33	const QString& rnxFileName,
[4111]	34	int PID, int SID, int IOD, int iRow) :
[3273]	35	bncUploadCaster(mountpoint, outHost, outPort, password, iRow, 0) {
[3224]	36
[5130]	37	if (!outHost.isEmpty()) {
	38	_casterID += outHost;
	39	}
	40	if (!crdTrafo.isEmpty()) {
	41	_casterID += " " + crdTrafo;
	42	}
	43	if (!sp3FileName.isEmpty()) {
	44	_casterID += " " + sp3FileName;
	45	}
	46	if (!rnxFileName.isEmpty()) {
	47	_casterID += " " + rnxFileName;
	48	}
	49
[3222]	50	_crdTrafo = crdTrafo;
	51	_CoM = CoM;
[4111]	52	_PID = PID;
	53	_SID = SID;
	54	_IOD = IOD;
[3222]	55
[3224]	56	// Member that receives the ephemeris
	57	// ----------------------------------
	58	_ephUser = new bncEphUser();
[3222]	59
[3272]	60	bncSettings settings;
	61	QString intr = settings.value("uploadIntr").toString();
[4174]	62
	63	_samplRtcmEphCorr = settings.value("uploadSamplRtcmEphCorr").toDouble();
[4542]	64	int samplClkRnx = settings.value("uploadSamplClkRnx").toInt();
	65	int samplSp3 = settings.value("uploadSamplSp3").toInt() * 60;
[4174]	66
	67	if (_samplRtcmEphCorr == 0.0) {
[3753]	68	_usedEph = 0;
	69	}
	70	else {
	71	_usedEph = new QMap<QString, t_eph*>;
	72	}
[3272]	73
[3222]	74	// RINEX writer
	75	// ------------
	76	if (!rnxFileName.isEmpty()) {
[4174]	77	_rnx = new bncClockRinex(rnxFileName, intr, samplClkRnx);
[3222]	78	}
	79	else {
	80	_rnx = 0;
	81	}
	82
	83	// SP3 writer
	84	// ----------
	85	if (!sp3FileName.isEmpty()) {
[4174]	86	_sp3 = new bncSP3(sp3FileName, intr, samplSp3);
[3222]	87	}
	88	else {
	89	_sp3 = 0;
	90	}
	91
	92	// Set Transformation Parameters
	93	// -----------------------------
	94	if (_crdTrafo == "ETRF2000") {
[3890]	95	_dx = 0.0521;
	96	_dy = 0.0493;
	97	_dz = -0.0585;
	98	_dxr = 0.0001;
[3222]	99	_dyr = 0.0001;
	100	_dzr = -0.0018;
	101	_ox = 0.000891;
	102	_oy = 0.005390;
	103	_oz = -0.008712;
	104	_oxr = 0.000081;
	105	_oyr = 0.000490;
	106	_ozr = -0.000792;
[3890]	107	_sc = 1.34;
[3222]	108	_scr = 0.08;
	109	_t0 = 2000.0;
	110	}
	111	else if (_crdTrafo == "NAD83") {
[5345]	112	_dx = 0.99343;
	113	_dy = -1.90331;
	114	_dz = -0.52655;
	115	_dxr = 0.00079;
	116	_dyr = -0.00060;
	117	_dzr = -0.00134;
	118	_ox = -0.02591467;
	119	_oy = -0.00942645;
	120	_oz = -0.01159935;
	121	_oxr = -0.00006667;
	122	_oyr = 0.00075744;
	123	_ozr = 0.00005133;
	124	_sc = 1.71504;
[5348]	125	_scr = -0.10201;
[5345]	126	_t0 = 1997.0;
[3222]	127	}
	128	else if (_crdTrafo == "GDA94") {
[4963]	129	_dx = -0.08468;
	130	_dy = -0.01942;
	131	_dz = 0.03201;
	132	_dxr = 0.00142;
	133	_dyr = 0.00134;
	134	_dzr = 0.00090;
	135	_ox = 0.0004254;
	136	_oy = -0.0022578;
	137	_oz = -0.0024015;
	138	_oxr = -0.0015461;
	139	_oyr = -0.0011820;
	140	_ozr = -0.0011551;
	141	_sc = 9.710;
	142	_scr = 0.109;
[3222]	143	_t0 = 1994.0;
	144	}
	145	else if (_crdTrafo == "SIRGAS2000") {
[4956]	146	_dx = 0.0020;
	147	_dy = 0.0041;
	148	_dz = 0.0039;
[3222]	149	_dxr = 0.0000;
	150	_dyr = 0.0000;
	151	_dzr = 0.0000;
[4956]	152	_ox = 0.000170;
	153	_oy = -0.000030;
	154	_oz = 0.000070;
[3222]	155	_oxr = 0.000000;
	156	_oyr = 0.000000;
	157	_ozr = 0.000000;
	158	_sc = 0.000;
	159	_scr = 0.000;
	160	_t0 = 0000.0;
	161	}
	162	else if (_crdTrafo == "SIRGAS95") {
	163	_dx = 0.0077;
	164	_dy = 0.0058;
	165	_dz = -0.0138;
	166	_dxr = 0.0000;
	167	_dyr = 0.0000;
	168	_dzr = 0.0000;
	169	_ox = 0.000000;
	170	_oy = 0.000000;
	171	_oz = -0.000030;
	172	_oxr = 0.000000;
	173	_oyr = 0.000000;
	174	_ozr = 0.000000;
	175	_sc = 1.570;
	176	_scr = 0.000;
	177	_t0 = 0000.0;
	178	}
[5347]	179	else if (_crdTrafo == "DREF91") {
[5340]	180	_dx = -0.0118;
	181	_dy = 0.1432;
	182	_dz = -0.1117;
	183	_dxr = 0.0001;
	184	_dyr = 0.0001;
	185	_dzr = -0.0018;
	186	_ox = 0.003291;
	187	_oy = 0.006190;
	188	_oz = -0.011012;
	189	_oxr = 0.000081;
	190	_oyr = 0.000490;
	191	_ozr = -0.000792;
	192	_sc = 12.24;
	193	_scr = 0.08;
	194	_t0 = 2000.0;
	195	}
[3222]	196	else if (_crdTrafo == "Custom") {
	197	_dx = settings.value("trafo_dx").toDouble();
	198	_dy = settings.value("trafo_dy").toDouble();
	199	_dz = settings.value("trafo_dz").toDouble();
	200	_dxr = settings.value("trafo_dxr").toDouble();
	201	_dyr = settings.value("trafo_dyr").toDouble();
	202	_dzr = settings.value("trafo_dzr").toDouble();
	203	_ox = settings.value("trafo_ox").toDouble();
	204	_oy = settings.value("trafo_oy").toDouble();
	205	_oz = settings.value("trafo_oz").toDouble();
	206	_oxr = settings.value("trafo_oxr").toDouble();
	207	_oyr = settings.value("trafo_oyr").toDouble();
	208	_ozr = settings.value("trafo_ozr").toDouble();
	209	_sc = settings.value("trafo_sc").toDouble();
	210	_scr = settings.value("trafo_scr").toDouble();
	211	_t0 = settings.value("trafo_t0").toDouble();
	212	}
	213	}
	214
	215	// Destructor
	216	////////////////////////////////////////////////////////////////////////////
[3224]	217	bncRtnetUploadCaster::~bncRtnetUploadCaster() {
[3222]	218	if (isRunning()) {
	219	wait();
	220	}
	221	delete _rnx;
	222	delete _sp3;
	223	delete _ephUser;
[3753]	224	delete _usedEph;
[3222]	225	}
	226
	227	//
	228	////////////////////////////////////////////////////////////////////////////
[3224]	229	void bncRtnetUploadCaster::decodeRtnetStream(char* buffer, int bufLen) {
[3222]	230
	231	QMutexLocker locker(&_mutex);
	232
[3230]	233	// Append to internal buffer
	234	// -------------------------
[3222]	235	_rtnetStreamBuffer.append(QByteArray(buffer, bufLen));
[3230]	236
	237	// Select buffer part that contains last epoch
	238	// -------------------------------------------
	239	QStringList lines;
	240	int iEpoBeg = _rtnetStreamBuffer.lastIndexOf('*'); // begin of last epoch
	241	if (iEpoBeg == -1) {
[3226]	242	_rtnetStreamBuffer.clear();
	243	return;
[3222]	244	}
[3230]	245	_rtnetStreamBuffer = _rtnetStreamBuffer.mid(iEpoBeg);
	246
	247	int iEpoEnd = _rtnetStreamBuffer.lastIndexOf("EOE"); // end of last epoch
	248	if (iEpoEnd == -1) {
	249	return;
	250	}
[3226]	251	else {
[3230]	252	lines = _rtnetStreamBuffer.left(iEpoEnd).split('\n', QString::SkipEmptyParts);
	253	_rtnetStreamBuffer = _rtnetStreamBuffer.mid(iEpoEnd+3);
[3226]	254	}
[3222]	255
[3226]	256	if (lines.size() < 2) {
[3222]	257	return;
	258	}
	259
[3226]	260	// Keep the last unfinished line in buffer
	261	// ---------------------------------------
	262	int iLastEOL = _rtnetStreamBuffer.lastIndexOf('\n');
	263	if (iLastEOL != -1) {
	264	_rtnetStreamBuffer = _rtnetStreamBuffer.mid(iLastEOL+1);
	265	}
	266
	267	// Read first line (with epoch time)
	268	// ---------------------------------
	269	QTextStream in(lines[0].toAscii());
	270	QString hlp;
	271	int year, month, day, hour, min;
	272	double sec;
	273	in >> hlp >> year >> month >> day >> hour >> min >> sec;
	274	bncTime epoTime; epoTime.set( year, month, day, hour, min, sec);
	275
[5131]	276	emit(newMessage("bncRtnetUploadCaster: decode " +
	277	QByteArray(epoTime.datestr().c_str()) + " " +
[5132]	278	QByteArray(epoTime.timestr().c_str()) + " " +
	279	_casterID.toAscii(), false));
[4808]	280
[3222]	281	struct ClockOrbit co;
	282	memset(&co, 0, sizeof(co));
[3226]	283	co.GPSEpochTime = static_cast<int>(epoTime.gpssec());
	284	co.GLONASSEpochTime = static_cast<int>(fmod(epoTime.gpssec(), 86400.0))
[3222]	285	+ 3 * 3600 - gnumleap(year, month, day);
	286	co.ClockDataSupplied = 1;
	287	co.OrbitDataSupplied = 1;
	288	co.SatRefDatum = DATUM_ITRF;
[4111]	289	co.SSRIOD = _IOD;
	290	co.SSRProviderID = _PID; // 256 .. BKG, 257 ... EUREF
	291	co.SSRSolutionID = _SID;
[3222]	292
	293	struct Bias bias;
	294	memset(&bias, 0, sizeof(bias));
[3226]	295	bias.GPSEpochTime = co.GPSEpochTime;
	296	bias.GLONASSEpochTime = co.GLONASSEpochTime;
[3222]	297
[4753]	298	// Default Update Interval
	299	// -----------------------
[4754]	300	int clkUpdInd = 2; // 5 sec
	301	int ephUpdInd = clkUpdInd; // default
	302	if (_samplRtcmEphCorr == 10.0) {
	303	ephUpdInd = 3;
	304	}
	305	else if (_samplRtcmEphCorr == 15.0) {
	306	ephUpdInd = 4;
	307	}
	308	else if (_samplRtcmEphCorr == 30.0) {
	309	ephUpdInd = 5;
	310	}
	311	else if (_samplRtcmEphCorr == 60.0) {
	312	ephUpdInd = 6;
	313	}
	314	else if (_samplRtcmEphCorr == 120.0) {
	315	ephUpdInd = 7;
	316	}
	317	else if (_samplRtcmEphCorr == 240.0) {
	318	ephUpdInd = 8;
	319	}
	320	else if (_samplRtcmEphCorr == 300.0) {
	321	ephUpdInd = 9;
	322	}
	323	else if (_samplRtcmEphCorr == 600.0) {
	324	ephUpdInd = 10;
	325	}
	326	else if (_samplRtcmEphCorr == 900.0) {
	327	ephUpdInd = 11;
	328	}
[4753]	329
[4754]	330	co.UpdateInterval = clkUpdInd;
	331	bias.UpdateInterval = clkUpdInd;
[4753]	332
[3226]	333	for (int ii = 1; ii < lines.size(); ii++) {
	334
[3222]	335	QString prn;
[4991]	336	ColumnVector rtnAPC;
	337	ColumnVector rtnVel;
	338	ColumnVector rtnCoM;
	339	double rtnClk;
[3222]	340
	341	QTextStream in(lines[ii].toAscii());
	342
	343	in >> prn;
	344
[3753]	345	t_eph* eph = 0;
[3222]	346	const bncEphUser::t_ephPair* ephPair = _ephUser->ephPair(prn);
	347	if (ephPair) {
	348
[3754]	349	eph = ephPair->last;
	350
[4098]	351	// Use previous ephemeris if the last one is too recent
	352	// ----------------------------------------------------
	353	const int MINAGE = 60; // seconds
	354	if (ephPair->prev && eph->receptDateTime().isValid() &&
	355	eph->receptDateTime().secsTo(currentDateAndTimeGPS()) < MINAGE) {
	356	eph = ephPair->prev;
	357	}
[3754]	358
[3753]	359	// Make sure the clock messages refer to same IOD as orbit messages
	360	// ----------------------------------------------------------------
	361	if (_usedEph) {
[4174]	362	if (fmod(epoTime.gpssec(), _samplRtcmEphCorr) == 0.0) {
[3753]	363	(*_usedEph)[prn] = eph;
	364	}
	365	else {
	366	eph = 0;
	367	if (_usedEph->contains(prn)) {
	368	t_eph* usedEph = _usedEph->value(prn);
	369	if (usedEph == ephPair->last) {
	370	eph = ephPair->last;
	371	}
	372	else if (usedEph == ephPair->prev) {
	373	eph = ephPair->prev;
	374	}
	375	}
	376	}
	377	}
	378	}
	379
	380	if (eph) {
	381
[5503]	382	QMap<QString, double> codeBiases;
	383
[4991]	384	while (true) {
	385	QString key;
	386	int numVal = 0;
	387	in >> key >> numVal;
	388	if (in.status() != QTextStream::Ok) {
	389	break;
	390	}
	391	if (key == "APC") {
	392	rtnAPC.ReSize(3);
	393	in >> rtnAPC[0] >> rtnAPC[1] >> rtnAPC[2];
	394	}
	395	else if (key == "Clk") {
	396	in >> rtnClk;
	397	}
	398	else if (key == "Vel") {
	399	rtnVel.ReSize(3);
	400	in >> rtnVel[0] >> rtnVel[1] >> rtnVel[2];
	401	}
	402	else if (key == "CoM") {
	403	rtnCoM.ReSize(3);
	404	in >> rtnCoM[0] >> rtnCoM[1] >> rtnCoM[2];
	405	}
[5503]	406	else if (key == "CodeBias") {
	407	for (int ii = 0; ii < numVal; ii++) {
	408	QString type;
	409	double value;
	410	in >> type >> value;
	411	codeBiases[type] = value;
	412	}
	413	}
[4991]	414	else {
	415	for (int ii = 0; ii < numVal; ii++) {
	416	double dummy;
	417	in >> dummy;
	418	}
	419	}
	420	}
[3222]	421	struct ClockOrbit::SatData* sd = 0;
	422	if (prn[0] == 'G') {
	423	sd = co.Sat + co.NumberOfGPSSat;
	424	++co.NumberOfGPSSat;
	425	}
	426	else if (prn[0] == 'R') {
	427	sd = co.Sat + CLOCKORBIT_NUMGPS + co.NumberOfGLONASSSat;
	428	++co.NumberOfGLONASSSat;
	429	}
	430	if (sd) {
	431	QString outLine;
[3226]	432	processSatellite(eph, epoTime.gpsw(), epoTime.gpssec(), prn,
[4991]	433	rtnAPC, rtnClk, rtnVel, rtnCoM, sd, outLine);
[3222]	434	}
	435
	436	struct Bias::BiasSat* biasSat = 0;
	437	if (prn[0] == 'G') {
	438	biasSat = bias.Sat + bias.NumberOfGPSSat;
	439	++bias.NumberOfGPSSat;
	440	}
	441	else if (prn[0] == 'R') {
	442	biasSat = bias.Sat + CLOCKORBIT_NUMGPS + bias.NumberOfGLONASSSat;
	443	++bias.NumberOfGLONASSSat;
	444	}
	445
[5503]	446	// Code Biases
	447	// -----------
[3222]	448	if (biasSat) {
	449	biasSat->ID = prn.mid(1).toInt();
[5503]	450	biasSat->NumberOfCodeBiases = 0;
[3222]	451	if (prn[0] == 'G') {
[5503]	452	QMapIterator<QString, double> it(codeBiases);
	453	while (it.hasNext()) {
	454	it.next();
[5504]	455	if (it.key() == "1C") {
[5503]	456	int ii = biasSat->NumberOfCodeBiases; if (ii >= CLOCKORBIT_NUMBIAS) break;
	457	biasSat->NumberOfCodeBiases += 1;
	458	biasSat->Biases[ii].Type = CODETYPEGPS_L1_CA;
	459	biasSat->Biases[ii].Bias = it.value();
	460	}
[5504]	461	else if (it.key() == "1C") {
[5503]	462	int ii = biasSat->NumberOfCodeBiases; if (ii >= CLOCKORBIT_NUMBIAS) break;
	463	biasSat->NumberOfCodeBiases += 1;
	464	biasSat->Biases[ii].Type = CODETYPEGPS_L1_CA;
	465	biasSat->Biases[ii].Bias = it.value();
	466	}
[5504]	467	else if (it.key() == "1P") {
[5503]	468	int ii = biasSat->NumberOfCodeBiases; if (ii >= CLOCKORBIT_NUMBIAS) break;
	469	biasSat->NumberOfCodeBiases += 1;
	470	biasSat->Biases[ii].Type = CODETYPEGPS_L1_P;
	471	biasSat->Biases[ii].Bias = it.value();
	472	}
[5504]	473	else if (it.key() == "1W") {
[5503]	474	int ii = biasSat->NumberOfCodeBiases; if (ii >= CLOCKORBIT_NUMBIAS) break;
	475	biasSat->NumberOfCodeBiases += 1;
	476	biasSat->Biases[ii].Type = CODETYPEGPS_L1_Z;
	477	biasSat->Biases[ii].Bias = it.value();
	478	}
[5504]	479	else if (it.key() == "2C") {
[5503]	480	int ii = biasSat->NumberOfCodeBiases; if (ii >= CLOCKORBIT_NUMBIAS) break;
	481	biasSat->NumberOfCodeBiases += 1;
	482	biasSat->Biases[ii].Type = CODETYPEGPS_L2_CA;
	483	biasSat->Biases[ii].Bias = it.value();
	484	}
[5504]	485	else if (it.key() == "2D") {
[5503]	486	int ii = biasSat->NumberOfCodeBiases; if (ii >= CLOCKORBIT_NUMBIAS) break;
	487	biasSat->NumberOfCodeBiases += 1;
	488	biasSat->Biases[ii].Type = CODETYPEGPS_SEMI_CODELESS;
	489	biasSat->Biases[ii].Bias = it.value();
	490	}
[5504]	491	else if (it.key() == "2S") {
[5503]	492	int ii = biasSat->NumberOfCodeBiases; if (ii >= CLOCKORBIT_NUMBIAS) break;
	493	biasSat->NumberOfCodeBiases += 1;
	494	biasSat->Biases[ii].Type = CODETYPEGPS_L2_CM;
	495	biasSat->Biases[ii].Bias = it.value();
	496	}
[5504]	497	else if (it.key() == "2L") {
[5503]	498	int ii = biasSat->NumberOfCodeBiases; if (ii >= CLOCKORBIT_NUMBIAS) break;
	499	biasSat->NumberOfCodeBiases += 1;
	500	biasSat->Biases[ii].Type = CODETYPEGPS_L2_CL;
	501	biasSat->Biases[ii].Bias = it.value();
	502	}
[5504]	503	else if (it.key() == "2X") {
[5503]	504	int ii = biasSat->NumberOfCodeBiases; if (ii >= CLOCKORBIT_NUMBIAS) break;
	505	biasSat->NumberOfCodeBiases += 1;
	506	biasSat->Biases[ii].Type = CODETYPEGPS_L2_CML;
	507	biasSat->Biases[ii].Bias = it.value();
	508	}
[5504]	509	else if (it.key() == "2P") {
[5503]	510	int ii = biasSat->NumberOfCodeBiases; if (ii >= CLOCKORBIT_NUMBIAS) break;
	511	biasSat->NumberOfCodeBiases += 1;
	512	biasSat->Biases[ii].Type = CODETYPEGPS_L2_P;
	513	biasSat->Biases[ii].Bias = it.value();
	514	}
[5510]	515	else if (it.key() == "2W") {
[5503]	516	int ii = biasSat->NumberOfCodeBiases; if (ii >= CLOCKORBIT_NUMBIAS) break;
	517	biasSat->NumberOfCodeBiases += 1;
	518	biasSat->Biases[ii].Type = CODETYPEGPS_L2_Z;
	519	biasSat->Biases[ii].Bias = it.value();
	520	}
[5504]	521	else if (it.key() == "5I") {
[5503]	522	int ii = biasSat->NumberOfCodeBiases; if (ii >= CLOCKORBIT_NUMBIAS) break;
	523	biasSat->NumberOfCodeBiases += 1;
	524	biasSat->Biases[ii].Type = CODETYPEGPS_L5_I;
	525	biasSat->Biases[ii].Bias = it.value();
	526	}
[5504]	527	else if (it.key() == "5Q") {
[5503]	528	int ii = biasSat->NumberOfCodeBiases; if (ii >= CLOCKORBIT_NUMBIAS) break;
	529	biasSat->NumberOfCodeBiases += 1;
	530	biasSat->Biases[ii].Type = CODETYPEGPS_L5_Q;
	531	biasSat->Biases[ii].Bias = it.value();
	532	}
	533	}
[3222]	534	}
	535	else if (prn[0] == 'R') {
[5503]	536	QMapIterator<QString, double> it(codeBiases);
	537	while (it.hasNext()) {
	538	it.next();
[5504]	539	if (it.key() == "1C") {
[5503]	540	int ii = biasSat->NumberOfCodeBiases; if (ii >= CLOCKORBIT_NUMBIAS) break;
	541	biasSat->NumberOfCodeBiases += 1;
	542	biasSat->Biases[ii].Type = CODETYPEGLONASS_L1_CA;
	543	biasSat->Biases[ii].Bias = it.value();
	544	}
[5504]	545	else if (it.key() == "1P") {
[5503]	546	int ii = biasSat->NumberOfCodeBiases; if (ii >= CLOCKORBIT_NUMBIAS) break;
	547	biasSat->NumberOfCodeBiases += 1;
	548	biasSat->Biases[ii].Type = CODETYPEGLONASS_L1_P;
	549	biasSat->Biases[ii].Bias = it.value();
	550	}
[5504]	551	else if (it.key() == "2C") {
[5503]	552	int ii = biasSat->NumberOfCodeBiases; if (ii >= CLOCKORBIT_NUMBIAS) break;
	553	biasSat->NumberOfCodeBiases += 1;
	554	biasSat->Biases[ii].Type = CODETYPEGLONASS_L2_CA;
	555	biasSat->Biases[ii].Bias = it.value();
	556	}
[5504]	557	else if (it.key() == "2P") {
[5503]	558	int ii = biasSat->NumberOfCodeBiases; if (ii >= CLOCKORBIT_NUMBIAS) break;
	559	biasSat->NumberOfCodeBiases += 1;
	560	biasSat->Biases[ii].Type = CODETYPEGLONASS_L2_P;
	561	biasSat->Biases[ii].Bias = it.value();
	562	}
	563	}
[3222]	564	}
	565	}
	566	}
	567	}
[3227]	568
	569	QByteArray hlpBufferCo;
[3493]	570
	571	// Orbit and Clock Corrections together
	572	// ------------------------------------
[4174]	573	if (_samplRtcmEphCorr == 0.0) {
[3493]	574	if (co.NumberOfGPSSat > 0 \|\| co.NumberOfGLONASSSat > 0) {
	575	char obuffer[CLOCKORBIT_BUFFERSIZE];
	576	int len = MakeClockOrbit(&co, COTYPE_AUTO, 0, obuffer, sizeof(obuffer));
	577	if (len > 0) {
	578	hlpBufferCo = QByteArray(obuffer, len);
	579	}
[3222]	580	}
	581	}
[3493]	582
	583	// Orbit and Clock Corrections separately
	584	// --------------------------------------
	585	else {
	586	if (co.NumberOfGPSSat > 0) {
	587	char obuffer[CLOCKORBIT_BUFFERSIZE];
[4174]	588	if (fmod(epoTime.gpssec(), _samplRtcmEphCorr) == 0.0) {
[4754]	589	co.UpdateInterval = ephUpdInd;
[3591]	590	int len1 = MakeClockOrbit(&co, COTYPE_GPSORBIT, 1, obuffer, sizeof(obuffer));
[4754]	591	co.UpdateInterval = clkUpdInd;
[3493]	592	if (len1 > 0) {
	593	hlpBufferCo += QByteArray(obuffer, len1);
	594	}
	595	}
[3591]	596	int mmsg = (co.NumberOfGLONASSSat > 0) ? 1 : 0;
	597	int len2 = MakeClockOrbit(&co, COTYPE_GPSCLOCK, mmsg, obuffer, sizeof(obuffer));
[3493]	598	if (len2 > 0) {
	599	hlpBufferCo += QByteArray(obuffer, len2);
	600	}
	601	}
	602	if (co.NumberOfGLONASSSat > 0) {
	603	char obuffer[CLOCKORBIT_BUFFERSIZE];
[4174]	604	if (fmod(epoTime.gpssec(), _samplRtcmEphCorr) == 0.0) {
[4754]	605	co.UpdateInterval = ephUpdInd;
[3591]	606	int len1 = MakeClockOrbit(&co, COTYPE_GLONASSORBIT, 1, obuffer, sizeof(obuffer));
[4754]	607	co.UpdateInterval = clkUpdInd;
[3493]	608	if (len1 > 0) {
	609	hlpBufferCo += QByteArray(obuffer, len1);
	610	}
	611	}
	612	int len2 = MakeClockOrbit(&co, COTYPE_GLONASSCLOCK, 0, obuffer, sizeof(obuffer));
	613	if (len2 > 0) {
	614	hlpBufferCo += QByteArray(obuffer, len2);
	615	}
	616	}
	617	}
[3222]	618
[3493]	619	// Biases
	620	// ------
[3227]	621	QByteArray hlpBufferBias;
[3222]	622	if (bias.NumberOfGPSSat > 0 \|\| bias.NumberOfGLONASSSat > 0) {
	623	char obuffer[CLOCKORBIT_BUFFERSIZE];
	624	int len = MakeBias(&bias, BTYPE_AUTO, 0, obuffer, sizeof(obuffer));
	625	if (len > 0) {
[3227]	626	hlpBufferBias = QByteArray(obuffer, len);
[3222]	627	}
	628	}
[3227]	629
[4808]	630	_outBuffer += hlpBufferCo + hlpBufferBias;
[3222]	631	}
	632
	633	//
	634	////////////////////////////////////////////////////////////////////////////
[3224]	635	void bncRtnetUploadCaster::processSatellite(t_eph* eph, int GPSweek,
[4991]	636	double GPSweeks, const QString& prn,
	637	const ColumnVector& rtnAPC,
	638	double rtnClk,
	639	const ColumnVector& rtnVel,
	640	const ColumnVector& rtnCoM,
	641	struct ClockOrbit::SatData* sd,
	642	QString& outLine) {
[3222]	643
[4930]	644	// Broadcast Position and Velocity
	645	// -------------------------------
	646	ColumnVector xB(4);
	647	ColumnVector vB(3);
	648	eph->position(GPSweek, GPSweeks, xB.data(), vB.data());
	649
[4991]	650	// Precise Position
	651	// ----------------
	652	ColumnVector xP = _CoM ? rtnCoM : rtnAPC;
[3222]	653
[4930]	654	double dc = 0.0;
	655	if (_crdTrafo != "IGS08") {
	656	crdTrafo(GPSweek, xP, dc);
	657	}
	658
	659	// Difference in xyz
	660	// -----------------
	661	ColumnVector dx = xB.Rows(1,3) - xP;
[4991]	662	ColumnVector dv = vB - rtnVel;
[4930]	663
	664	// Difference in RSW
	665	// -----------------
[3222]	666	ColumnVector rsw(3);
[4930]	667	XYZ_to_RSW(xB.Rows(1,3), vB, dx, rsw);
[3222]	668
[4930]	669	ColumnVector dotRsw(3);
	670	XYZ_to_RSW(xB.Rows(1,3), vB, dv, dotRsw);
[3222]	671
[4930]	672	// Clock Correction
	673	// ----------------
[4991]	674	double dClk = rtnClk - (xB(4) - dc) * t_CST::c;
[3222]	675
	676	if (sd) {
[4930]	677	sd->ID = prn.mid(1).toInt();
	678	sd->IOD = eph->IOD();
	679	sd->Clock.DeltaA0 = dClk;
	680	sd->Orbit.DeltaRadial = rsw(1);
	681	sd->Orbit.DeltaAlongTrack = rsw(2);
	682	sd->Orbit.DeltaCrossTrack = rsw(3);
	683	sd->Orbit.DotDeltaRadial = dotRsw(1);
	684	sd->Orbit.DotDeltaAlongTrack = dotRsw(2);
	685	sd->Orbit.DotDeltaCrossTrack = dotRsw(3);
[3222]	686	}
	687
	688	outLine.sprintf("%d %.1f %s %3d %10.3f %8.3f %8.3f %8.3f\n",
[3255]	689	GPSweek, GPSweeks, eph->prn().toAscii().data(),
[3222]	690	eph->IOD(), dClk, rsw(1), rsw(2), rsw(3));
	691
[4991]	692	double relativity = -2.0 * DotProduct(xP, rtnVel) / t_CST::c;
	693	double sp3Clk = (rtnClk - relativity) / t_CST::c; // in seconds
	694
[3222]	695	if (_rnx) {
[4991]	696	_rnx->write(GPSweek, GPSweeks, prn, sp3Clk);
[3222]	697	}
	698	if (_sp3) {
[4991]	699	_sp3->write(GPSweek, GPSweeks, prn, rtnCoM, sp3Clk);
[3222]	700	}
	701	}
	702
	703	// Transform Coordinates
	704	////////////////////////////////////////////////////////////////////////////
[4803]	705	void bncRtnetUploadCaster::crdTrafo(int GPSWeek, ColumnVector& xyz,
	706	double& dc) {
[3222]	707
	708	// Current epoch minus 2000.0 in years
	709	// ------------------------------------
	710	double dt = (GPSWeek - (1042.0+6.0/7.0)) / 365.2422 * 7.0 + 2000.0 - _t0;
	711
	712	ColumnVector dx(3);
	713
	714	dx(1) = _dx + dt * _dxr;
	715	dx(2) = _dy + dt * _dyr;
	716	dx(3) = _dz + dt * _dzr;
	717
	718	static const double arcSec = 180.0 * 3600.0 / M_PI;
	719
	720	double ox = (_ox + dt * _oxr) / arcSec;
	721	double oy = (_oy + dt * _oyr) / arcSec;
	722	double oz = (_oz + dt * _ozr) / arcSec;
	723
	724	double sc = 1.0 + _sc * 1e-9 + dt * _scr * 1e-9;
	725
[4912]	726	// Specify approximate center of area
	727	// ----------------------------------
	728	ColumnVector meanSta(3);
	729
	730	if (_crdTrafo == "ETRF2000") {
[4973]	731	meanSta(1) = 3661090.0;
	732	meanSta(2) = 845230.0;
	733	meanSta(3) = 5136850.0;
[4908]	734	}
[4912]	735	else if (_crdTrafo == "NAD83") {
[4973]	736	meanSta(1) = -1092950.0;
	737	meanSta(2) = -4383600.0;
	738	meanSta(3) = 4487420.0;
[4912]	739	}
	740	else if (_crdTrafo == "GDA94") {
[4973]	741	meanSta(1) = -4052050.0;
	742	meanSta(2) = 4212840.0;
	743	meanSta(3) = -2545110.0;
[4912]	744	}
	745	else if (_crdTrafo == "SIRGAS2000") {
[4973]	746	meanSta(1) = 3740860.0;
	747	meanSta(2) = -4964290.0;
	748	meanSta(3) = -1425420.0;
[4912]	749	}
	750	else if (_crdTrafo == "SIRGAS95") {
[4973]	751	meanSta(1) = 3135390.0;
	752	meanSta(2) = -5017670.0;
	753	meanSta(3) = -2374440.0;
[4912]	754	}
[5343]	755	else if (_crdTrafo == "DREF91") {
	756	meanSta(1) = 3959579.0;
	757	meanSta(2) = 721719.0;
	758	meanSta(3) = 4931539.0;
	759	}
[4912]	760	else if (_crdTrafo == "Custom") {
	761	meanSta(1) = 0.0; // TODO
	762	meanSta(2) = 0.0; // TODO
	763	meanSta(3) = 0.0; // TODO
	764	}
[4908]	765
[4912]	766	// Clock correction proportional to topocentric distance to satellites
	767	// -------------------------------------------------------------------
	768	double rho = (xyz - meanSta).norm_Frobenius();
[4913]	769	dc = rho * (sc - 1.0) / sc / t_CST::c;
[4908]	770
[3222]	771	Matrix rMat(3,3);
	772	rMat(1,1) = 1.0;
	773	rMat(1,2) = -oz;
	774	rMat(1,3) = oy;
	775	rMat(2,1) = oz;
	776	rMat(2,2) = 1.0;
	777	rMat(2,3) = -ox;
	778	rMat(3,1) = -oy;
	779	rMat(3,2) = ox;
	780	rMat(3,3) = 1.0;
	781
	782	xyz = sc * rMat * xyz + dx;
	783	}
	784

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

Download in other formats: