车载导航仪GPS开发基础

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;


}