geo_utm.h

GPS的串口读取数据过程,并且进行了坐标转换,转化为通用的坐标系

#include <math.h>

#define  AXIS        6378137
#define  K0          0.9996
#define  ECC         0.00669437999013
#define  EP2         0.006739496742227

#define  M_PI        (3.14159265358979323846) 
#define  RADDEG      (M_PI/180.0)
#define  FN	         (0.0)
#define  FS	         (10000000.0)
#define  FE	         (500000.0)

/*int geo_to_transverse_mercator(float lat,float lon, double* x, double* y )
{
	int status = 0;
		//if( !cnstsP  )
		//	return -1;
		//else if( (status = projection_limit_check(cnstsP, lat,lon)) < 0 )
		//	return -2;
		//else		{
	        double cfs[5],T2,T3,T4,T5;
		//	const proj_dfn *cnsts = (const proj_dfn*)cnstsP;
			double rphi = lat*RADDEG, rlam = lon*RADDEG, dlam = rlam;
			double dlam2 = dlam*dlam;
			double sphi = sin(rphi), cphi = cos(rphi), tphi = tan(rphi);
			double sp2 = sphi*sphi;
			double T = tphi*tphi, G = EP2*cphi*cphi, cp2 = cphi*cphi;
			double RN = AXIS / sqrt(1 - sp2 * ECC) 
			double N = K0*RN;
			double Q = 0.5*sphi*cphi*N;
			double T1 = meridian_distance( rphi,sphi, MDCFS )*K0*AXIS;
			
			T2 = Q;
			
			Q *= (cp2/12.0);
			cfs[0] = 5.0-T; cfs[1] = 9.0; cfs[2] = 4.0;
			T3 = Q*poly(G,2,cfs);

			Q *= (cp2/30.0);
			cfs[0] = 61.0; cfs[1] = -58.0; cfs[2] = 1.0;
			cfs[0] = poly(T,2,cfs);
			cfs[1] = 270.0 - 330.0*T;
			cfs[2] = 445.0 - 680.0*T;
			cfs[3] = 324.0 - 600.0*T;
			cfs[4] =  88.0 - 192.0*T;
			T4 = Q*poly( G,4,cfs );

			Q *= (cp2/56.0);
			cfs[0] = 1385.0; cfs[1] = -3111.0; cfs[2] = 543.0; cfs[3] = -1.0;
			T5 = Q*poly(T,3,cfs);

			cfs[0] = T1; cfs[1] = T2; cfs[2] = T3; cfs[3] = T4; cfs[4] = T5;
			*y = poly( dlam2, 4, cfs ) - M0*AXIS; 

			Q = N*cphi;
			T1 = Q;

			Q *= (cp2/6.0);
			T2 = Q*(1.0 - T + G);

			Q *= (cp2/20.0);
			cfs[0] = 5.0; cfs[1] = -18.0; cfs[2] = 1.0;
			cfs[0] = poly(T,2,cfs);
			cfs[1] = 14.0 - 58.0*T;
			cfs[2] = 13.0 - 64.0*T;
			cfs[3] = 4.0 - 24.0*T;
			T3 = Q*poly( G,3,cfs );

			Q *= (cp2/42.0);
			cfs[0] = 61.0; cfs[1] = -479.0; cfs[2] = 179.0; cfs[3] = -1.0;
			T4 = Q*poly(T,3,cfs);

			cfs[0] = T1; cfs[1] = T2; cfs[2] = T3; cfs[3] = T4;
			*x = dlam*poly( dlam2,3,cfs );

			return status;
		//}
}*/

double compute(double ecc,double rphi, double sphi, double cphi)
{
	double A0, A2, A4, A6;
	double e1, e2, e3;
	double result;
	e1 = ecc;
	e2 = ecc*ecc;
	e3 = e2*e1;
	A0 = 1.0 - e1/4.0 - 3.0*e2/64.0 - 5.0*e3/256.0;
	A2 = 3.0*e1/8.0 + 3.0*e2/32.0 + 45.0*e3/1024.0;
	A4 = 15.0*e2/256.0 + 45.0*e3/1024.0;
	A6 = 35.0*e3/3072.0;
	double sphi2 = 2 * sphi * cphi;
	double sphi3 = 3 * sphi - 4 * sphi * sphi * sphi;
	double cphi2 = cphi*cphi - sphi*sphi;
	double cphi3 = 4 * cphi * cphi * cphi - 3 * cphi;
	double sphi4 = 2 * sphi2 * cphi2;
	double sphi6 = 2 * sphi3 * cphi3;
	result = A0*rphi - A2*sphi2 + A4*sphi4 - A6*sphi6;
	return result;
}


int geo_to_transverse_mercator(float lat,float lon, double *x, double *y )
{
	    double T1, T2, T3;
		double rphi = lat*RADDEG, rlam = lon*RADDEG;// dlam = rlam;
		//double dlam2 = dlam*dlam;
		double sphi = sin(rphi), cphi = cos(rphi), tphi = tan(rphi);
		double sp2 = sphi*sphi, cp2 = cphi*cphi;
		double A = rlam * cphi;
		double A2 = A*A;
		double A4 = A2*A2;
		double T = tphi*tphi;
		double C = EP2*cp2;
		double RN = AXIS / sqrt(1 - sp2 * ECC);
		double N = K0*RN;
		//double Q = 0.5*sphi*cphi*N;
		//double T1 = meridian_distance( rphi,sphi, MDCFS )*K0*AXIS
		T1 = (1.0 - T + C) * A2 * A / 6.0;
		T2 = (5.0 - 18.0*T + T*T + 72.0*C - 58.0*EP2)*A4*A/120.0;
		*x = N * (A + T1 + T2);
		double M = AXIS * compute(ECC, rphi, sphi, cphi);
		T1 = (5.0 - T + 9.0*C + 4.0*C*C)*A4/24.0;
		T2 = (61.0 - 58.0*T + T + 600.0*C - 330.0*EP2)*A4*A2/720.0;
		T3 = A2/2.0 + T1 + T2;
		*y = K0 * (M + RN * tphi * T3);
		return 1;
}



short geo_to_utm(float lat, float lon, long *izone, double *east,double *north )
{
	    //const proj_dfn* cnsts = (proj_dfn*)tcnsts;	
		short status;
		float lam0;
		long zone = *izone;
		/*if(zone < 31)
			lam0 = 6 * zone - 183.0;
		else if(zone > 30)
			lam0 = 6 * zone + 177;*/
		
		if( zone <= 0 || zone > 60 )	// if a zone is provided use it, otherwise compute it. 
		{	zone = (long)(31.0 + lon / 6.0);
			if(zone >= 61) zone = 60;	
    		if(zone <= 0) zone = 1;
    		*izone = zone;
		}
		lam0 = zone * 6 - 183.0;
		lon = lon - lam0;
    	//lon -= (zone*6-183);		   // Change the longitude to offset 
		status = geo_to_transverse_mercator(lat,lon, east, north );
		*east += FE;
		if( *north < 0.0 ) *north = -(*north + FS);
		return status;
}