1.2.3WGS84->LLA转换问题
在分析其日志的过程中，遇到一个问题，就是TimGPS二进制日志保存的坐标数据是以WGS84大地坐标系为准的，大地坐标系的XYZ轴如下：
要得到经纬度和海拔(LLA：Longitude/Latitude/Altitude)坐标，需要一些椭球体、基准面及地图投影的知识。
后来从网上找到一个Fortune77写的WGS84->GLL换算函数，将其转成C/C++的函数:
void wgsxyz2lla(double x,double y,double z,double *lat,double *lon,double *alt)
{
double pi = 3.14159265357;
longA_EARTH = 6378137;
double flattening = 1/298.257223563;
double NAV_E2 = (2-flattening)*flattening; // also e^2
double rad2deg = 180/pi;
double wlon,wlat,walt,rhosqrd,rho,templat,tempalt,rhoerror,zerror;
double slat,clat,q,r_n,drdl,invdet,aa,bb,cc,dd;
if ((x == 0.0) & (y == 0.0))
wlon = 0.0;
else
wlon = atan2(y, x)*rad2deg;
if ((x == 0.0) & (y == 0.0) & (z == 0.0))
{
printf("WGS xyz at center of earth");
wlon = 360;
wlat = 360;
walt = 6378137;
}
else
{
rhosqrd = x*x + y*y;
rho = sqrt(rhosqrd);
templat = atan2(z, rho);
tempalt = sqrt(rhosqrd + z*z) - A_EARTH;
rhoerror = 1000.0;
zerror = 1000.0;
while ((Dabs(rhoerror) > 1e-6) | (Dabs(zerror) > 1e-6))
{
slat = sin(templat);
clat = cos(templat);
q = 1 - NAV_E2*slat*slat;
r_n = A_EARTH/sqrt(q);
drdl = r_n*NAV_E2*slat*clat/q; // d(r_n)/d(latitutde)
rhoerror = (r_n + tempalt)*clat - rho;
zerror = (r_n*(1 - NAV_E2) + tempalt)*slat - z;
aa = drdl*clat - (r_n + tempalt)*slat;
bb = clat;
cc = (1 - NAV_E2)*(drdl*slat + r_n*clat);
dd = slat;
invdet = 1.0/(aa*dd - bb*cc);
templat = templat - invdet*(+dd*rhoerror -bb*zerror);
tempalt = tempalt - invdet*(-cc*rhoerror +aa*zerror);
}
wlat = templat*rad2deg;
walt = tempalt;
}
if(lon )*lon = wlon;
if(lat )*lat = wlat;
if(alt )*alt = walt;
}
