xyz2lla.c

根据GPS官方网站提供的gps定位方法而编写的C语言gps定位代码

#include <math.h>
#include "gpsconst.h"


void XYZ2LLA(double *Xi0, double *Xo)
{
	
  	/*ECEF X [m], Y [m] , Z[m]*/
  	/*lat [rad], lon [rad] alt [m]*/
  	/*this procedure converts WGS-84 ECEF XYZ to Lat, Lon, Alt [above ellipsoid]*/
  	double p, T, sT, cT, N;
	double Xi[3];

	Xi[0] = Xi0[0] - OS_DX;
	Xi[1] = Xi0[1] - OS_DY;
	Xi[2] = Xi0[2] - OS_DZ;

  	p = sqrt(Xi[0] * Xi[0] + Xi[1] * Xi[1]);
  	T = atan(Xi[2] * a / (p * b));
  	sT = sin(T);
  	cT = cos(T);
  	Xo[0] = atan((Xi[2] + e2sqr * b * sT * sT * sT) / (p - e1sqr * a * cT * cT * cT));
  	if (Xi[0] == 0.0)
    		Xo[1] = pi / 2.0;
  	else
    		Xo[1] = atan(Xi[1] / Xi[0]);
  	N = a / sqrt(1.0 - e1sqr * sin(Xo[0]) * sin(Xo[0]));
  	Xo[2] = p / cos(Xo[0]) - N;

}

/*****************************************************************************/

void LLA2XYZ(double *Xi, double *Xo)
{
  	/*lat [rad], lon [rad] alt [m]*/
  	/*ECEF X [m], Y [m] , Z[m]*/
  	/*this procedure converts WGS-84 Lat, Lon and Alt [above ellipsoid]
    	to ECEF XYZ*/
  	double N;

  	N = a / sqrt(1.0 - e1sqr * sin(Xi[0]) * sin(Xi[0]));


  	Xo[0] = (N + Xi[2]) * cos(Xi[0]) * cos(Xi[1]);
  	Xo[1] = (N + Xi[2]) * cos(Xi[0]) * sin(Xi[1]);
  	Xo[2] = (N * (1.0 - e1sqr) + Xi[2]) * sin(Xi[0]);

	Xo[0] += OS_DX;
	Xo[1] += OS_DY;
	Xo[2] += OS_DZ;
	
}