gpslib.cpp

这是一个GPS接收定位的原代码

	int i;
	for(i = 0; isdigit(cp[i]); i++)
		; // loop
	// printf("first digit string is %d long\n", i);
	switch(i){
		case 3:
		case 6:
			east += rdint(cp, 3)*100l+50;
			if(*cp == ' ')cp++;
			north += rdint(cp, 3)*100l+50;
			break;
		case 4:
		case 8:
			east += rdint(cp, 4)*10l+5;
			if(*cp == ' ')cp++;
			north += rdint(cp, 4)*10l+5;
			break;
		default: return 0;
	}
	// printf("%s gives East %ld, north %ld ", buff, east, north);
	cvtngrlatlongdeg(east, north, latres, longres);
	*northres = north; *eastres = east;
	return 1;
}

#include <stdarg.h>
int printwhinge(const char *fmt ...){
	char buff[512];
	va_list argptr;

	va_start(argptr, fmt);
	vsprintf(buff, fmt, argptr);
	va_end(argptr);
	QMessageBox::information(0, "Error", buff);
	return 0; 
}

int getlandmark(double *latp, double *longp,
	char *desp, int desplen, char *type, int tlen){
	// for the moment, don't do anything with the description
	if(desp) *desp = 0;
	if(!lmfp) return 0;

	char buff[512];
	double newlat, newlong;

	while(fgets(buff, sizeof(buff), lmfp)){
		// Turn any return into end of string
		char *cp = strchr(buff, '\r');
		if(cp)
			*cp = 0;
		cp = strchr(buff, '\n');
		if(cp)
			*cp = 0;
		cp = buff;
		if(*cp  == '#' || *cp == 0)
			continue;
		if(isalpha(*cp)){
			// presume ngr
			long north, east;

			if(!_cvtngrstr(cp, &east, &north, &newlat, &newlong))
				return printwhinge("getlandmark: bad NGR %s", cp);
			// printf("%s gives East %ld, north %ld ", buff, east, north);
			*latp = newlat; *longp = newlong;

			// printf("from NGR, lat %f, long %f\n", newlat, newlong);
			// convert the other way for interest's sake!
			// getngrfromdegs(newlat, newlong, buff);
			// printf("convert back, get %s\n", buff);

		}else{
			// presume lat/long
			const char *oldcp = cp;
			double latdegs = getlatitude(cp);
			if(latdegs > 100) return printwhinge("getlandmark: bad latitude %s", oldcp);

			if(*cp == ',') cp++; // skip delim
			// printf("Lat fract degs %f ", latdegs);
			oldcp = cp;
			double longdegs = getlongitude(cp);
			if(longdegs > 500) return printwhinge("getlandmark: bad longitude %s", oldcp);

			// printf("Long fract degs %f\n", longdegs);
			*latp = latdegs; *longp = longdegs;
		}
		if(*cp && !isalpha(*cp)) cp++;
		const char * commapos = strchr(cp, ',');
		int stlen = strlen(cp);
		if(commapos){
			stlen = int(commapos-cp);
			while(isspace(*++commapos))
				; // skip spaces
			if(type) strcpy(type, commapos);
		}else{
			if(type) *type = 0;
		}
		if(desp){ strncpy(desp, cp, stlen); desp[stlen] = 0; }
		return 1;
	}
	return 0;
}

//////////////////////////////// OSGB TRIG ///////////////////////////////

const double f0 = .9996012717;
const double b = 6356256.91*f0;
const double a = 6377563.396*f0;
const double e2 = (a*a-b*b)/(a*a);
const double n = .00167322025;
const double deg2rad = atan2(1,1) / 45;
const double lambdazero= -2 * deg2rad;
const double Nzero = -100000L;
const double Ezero = 400000L;

// Developed arc of a meridian from latitude 1 to 2 (degrees)
// returns metres
void capm(double lat1, double lat2, double *res){
	 const double phi2 = lat2 * deg2rad;
	 const double phi1 = lat1 * deg2rad;
	 const double nsq = n * n;
	 const double ncub = nsq * n;
	 const double s1 = sin(phi2 - phi1);
	 const double s2 = s1*s1;
	 const double s3 = s2*s1;
	 const double c1 = cos(phi1 + phi2);
	 const double c2 = c1*c1;
	 const double c3 = c2*c1;
	 const double f1 = (1.0 + n + 5.0/4.0 * (nsq + ncub)) * (phi2 - phi1);
	 const double f2 = (3.0 * (n + nsq) + 21.0 / 8.0 * ncub) * s1 * c1;
	 const double f3 = (15.0 / 8.0 * (nsq + ncub)) * s2 * c2;
	 const double f4 = (35.0 / 24.0 * ncub) * s3 * c3;
	 // cout << "Factors of M: f1 " << f1 << " f2 " << f2 << " f3 " <<
	 //			f3 << " f4 " << f4 << endl;
	 // f4 = f3 = 0;
	 *res = (f1 - f2 + f3 - f4) * b;
}

int cvtongr(double latdeg, double longitdeg, String &res,
		double &eres, double &nres){

	 double lat = latdeg*deg2rad;
	 double sinlat = sin(lat);
	 double coslat = cos(lat);
	 double sinlat2 = sinlat*sinlat;
	 double coslat2 = coslat*coslat;
	 double coslat3 = coslat2*coslat;
	 double tanlat = tan(lat);
	 double tanlat2 = tanlat*tanlat;
	 double vee = a/sqrt(1-e2*sinlat2);
	 double rho = vee*(1-e2)/(1-e2*sinlat2);
		// alternate calculation - seems to agree
		// double tmp = (1-e2*sinlat2);
		// cout << "altrho = " << a*(1-e2)/sqrt(tmp*tmp*tmp)<<endl;
	 const double nee2 = vee/rho-1;
	 const double longit = longitdeg*deg2rad;
	 double capM; capm(49, latdeg, &capM);
	 const double capP = longit-lambdazero;
	 const double eI = capM+Nzero;
	 const double eII = vee/2*sinlat*coslat;
	 const double eIII = vee/24*sinlat*coslat3*(5-tanlat2+9*nee2);
	 const double eIIIa = vee/720*sinlat*coslat2*coslat3*(61-58*tanlat2+tanlat2*tanlat2);
	 const double North = eI + capP*capP*eII + capP*capP*capP*capP*(eIII) +
			capP*capP*capP*capP*capP*capP*eIIIa;
	 const double eIV = vee*coslat;
	 const double eV = vee/6*coslat3*(vee/rho-tanlat2);
	 const double eVI = vee/120*coslat3*coslat2*(5-18*tanlat2+tanlat2*tanlat2+14*nee2
		- 58*tanlat2*nee2);
	 const double East = Ezero+capP*eIV+capP*capP*capP*eV+capP*capP*capP*capP*capP*eVI;
	 eres = East;
	 nres = North;
	 // Make Grid Letters for it
	 if(East > 700000.0 || North > 1300000.0)
		return 0;	// Can't allocate grid letters
	 if(East < 0 || North < 0)
		return 0;	// Can't allocate grid letters
	 char glet1[3][2] = {
		{'S', 'T'},
		{'N', 'O'},
		{'H', 'J'}
	 };
	 int ebit = int(East/500000.0);
	 int nbit = int(North/500000.0);
	 char NGL1 = glet1[nbit][ebit];
	 ebit = int(East/100000.0)%5;
	 nbit = int(North/100000.0)%5;
	 char NGL2 = ngrletters[(4-nbit)*5+ebit];
	 long eastings = long((East+5)/10)%10000L;
	 long northings = long((North+5)/10)%10000L;
	 char buff[32];
	 res.sprintf("%c%c %4.4ld %4.4ld", NGL1, NGL2, eastings, northings);
	 return res.nchars();
}
int getngrfromdegs(double latdeg, double longitdeg, String &res){
	double e,n;	// to throw away
	return cvtongr(latdeg, longitdeg, res, e, n);
}

void cvtngrlatlongdeg(double east, double north, double *lat, double *longit){
	double newlat = (north-Nzero)/a+49*deg2rad;
	// printf("cvtngrlatlong, newlat (deg) %f\n", newlat/deg2rad);
	double capM, diff;
	int loopcount = 0; // belt and braces to stop looping
	do{
		if(loopcount >= 8) QMessageBox::information(0, "Aaagh", "Looping in cvtngrlatlongdeg");
		capm(49, newlat/deg2rad, &capM);
		diff = north-Nzero-capM;
		newlat = (diff)/a+newlat;
		// printf("cvtngrlatlong, newlat (deg) %f\n", newlat/deg2rad);
	}while(diff > .001 && ++loopcount < 10);
	double sinlat = sin(newlat);
	double coslat = cos(newlat);
	double sinlat2 = sinlat*sinlat;
	//double coslat2 = coslat*coslat;
	//double coslat3 = coslat2*coslat;
	double tanlat = tan(newlat);
	double tanlat2 = tanlat*tanlat;
	//double tanlat2 = tanlat*tanlat;
	double vee = a/sqrt(1-e2*sinlat2);
	double rho = vee*(1-e2)/(1-e2*sinlat2);
	double nee2 = vee/rho-1;
	const double eVII = tanlat/(2*rho*vee);
	const double vee2 = vee*vee;
	const double vee3 = vee2*vee;
	// printf("tanlat %f, vee %f, rho %f\neVII %g\n", tanlat, vee, rho, eVII);
	const double eVIII = tanlat/(24*rho*vee3)*(5+3*tanlat2+nee2-9*tanlat2*nee2);
	// printf("eVIII = %g\n", eVIII);
	const double eIX = tanlat/(720*rho*vee3*vee2)*(61+90*tanlat2+45*tanlat2*tanlat2);
	// printf("eIX = %g\n", eIX);
	const double Et = east - Ezero;
	const double Et2 = Et*Et;
	const double Et3 = Et2*Et;
	const double Et4 = Et2*Et2;
	const double Et5 = Et3*Et2;
	// const double Et6 = Et3*Et3;
   const double Et7 = Et4*Et3;
	const double truelat = newlat - Et2*eVII + Et4*eVIII - Et4*Et2*eIX;
	*lat = truelat/deg2rad;
	// printf("True lat = %f (deg) %f\n", truelat, truelat/deg2rad);

	// secant = 1/cos(x)
	const double seclat = 1/coslat;
	const double eX = seclat/vee;
	// printf("eX = %g\n", eX);
	const double eXI = seclat/(6*vee3)*(vee/rho + 2*tanlat2);
	// printf("eXI = %g\n", eXI);
	const double tanlat4 = tanlat2*tanlat2;
	const double eXII = seclat/(120*vee2*vee3)*(5 + 28*tanlat2 + 24*tanlat4);
	const double eXIIA = seclat/(5040*vee2*vee3*vee2)*(61 + 662*tanlat2 + 1320*tanlat4 + 720*tanlat4*tanlat2);
	// printf("eXII = %g, eXIIA = %g\n", eXII, eXIIA);
	const double truelong = lambdazero + Et*eX - Et3*eXI + Et5*eXII-Et7*eXIIA;
	// printf("truelong %f (deg) %f\n", truelong, truelong/deg2rad);
	*longit = truelong/deg2rad;
}

// Convert from fractional degrees to d/m/s
String deg_min_secs(double val){
	// May be a better way of doing this, but let's be clear;
	// force rounding down
	bool negative = val < 0;
	val = fabs(val);
	int degrees = int(val);
	val -= degrees;
	int minutes = int(val*60);
	val -= minutes/60.0;
	int seconds = int(val*3600);
	String rval;
	rval.sprintf("%3.3d %2.2d\" %2.2d'", degrees, minutes, seconds);
	return rval;
}

String dmsLat(double val){
	String rval = deg_min_secs(fabs(val));
	rval += val < 0 ? 'S' : 'N';
	return rval;
}
String dmsLong(double val){
	String rval = deg_min_secs(fabs(val));
	rval += val < 0 ? 'W' : 'E';
	return rval;
}