llh_ecef.cpp

OpenSource GPS is software for x86 PCs that allows you to acquire, track and demodulate signals from

double const a=6378137.0,b=6356752.314; //  WGS-84 ellipsoid parameters

struct ecef
{
  double x,y,z;
};


struct llh
{
  double lat,lon,hae;
};



/*******************************************************************************
FUNCTION ecef_to_llh(ecef pos)
RETURNS  position in llh structure

PARAMETERS
			pos   ecef

PURPOSE    Convert a position in in cartesian ecef coordinates to
			  Geodetic WGS 84 coordinates

Based on equations found in Hoffman-Wellinhoff

WRITTEN BY
	Clifford Kelley

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

llh ecef_to_llh(ecef pos)
{
  double p,n,thet,esq,epsq;
  llh result;
  p=sqrt(pos.x*pos.x+pos.y*pos.y);
  thet=atan(pos.z*a/(p*b));
  esq =1.0-b*b/(a*a);
  epsq=a*a/(b*b)-1.0;
  result.lat=atan((pos.z+epsq*b*pow(sin(thet),3))/(p-esq*a*pow(cos(thet),3)));
  result.lon=atan2(pos.y,pos.x);
  n=a*a/sqrt(a*a*cos(result.lat)*cos(result.lat) +
	 b*b*sin(result.lat)*sin(result.lat));
  result.hae=p/cos(result.lat)-n;
  return(result);
}

/*******************************************************************************
FUNCTION llh_to_ecef(llh pos)
RETURNS  position in ecef structure

PARAMETERS
			pos   llh

PURPOSE    Convert a position in Geodetic WGS 84 coordinates to cartesian
			  ecef coordinates

Based on equations found in Hoffman-Wellinhoff

WRITTEN BY
	Clifford Kelley

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

ecef llh_to_ecef(llh pos)
{
  double n;
  ecef result;
  n=a*a/sqrt(a*a*cos(pos.lat)*cos(pos.lat)+b*b*sin(pos.lat)*sin(pos.lat));
  result.x=(n+pos.hae)*cos(pos.lat)*cos(pos.lon);
  result.y=(n+pos.hae)*cos(pos.lat)*sin(pos.lon);
  result.z=(b*b/(a*a)*n+pos.hae)*sin(pos.lat);
  return(result);
}