SonTek ADP - Page 35

SonTek
ADP Software Manual Version 6.42 (November 1, 2000)
29
#define Ae 6378137.0 /* Earth's semi-major axis */
#define Flat 0.0033528107 /* Earth's flattening */
void LlhXyz(double Lat, double Lon, double Height, double xyz[])
{
double rlat, rlon, clat, slat, clon, slon, g1, g2, g3;
rlat = dtr*Lat;
rlon = dtr*Lon;
clat = cos(rlat);
slat = sin(rlat);
clon = cos(rlon);
slon = sin(rlon);
g1 = Ae/sqrt(1 - (2 - Flat)*Flat*slat*slat);
g2 = g1*(1 - Flat)*(1 - Flat) + Height;
g3 = (g1 + Height)*clat;
xyz[0] = g3*clon;
xyz[1] = g3*slon;
xyz[2] = g2*slat;
}
/*===========================================================*/
/* XyzToEnu */
/* Given a vector in the earth centered, earth fixed */
/* coordinate system, and a position lat/lon on the surface */
/* of the Earth, this funtion will compute the components */
/* of that vector in the local East/North/Up system. */
/* */
void XyzToEnu(double Lat, double Lon, double xyz[], double Enu[])
{
double rlat, rlon, clat, slat, clon, slon;
rlat = DegToRad(Lat);
rlon = DegToRad(Lon);
clat = cos(rlat);
slat = sin(rlat);
clon = cos(rlon);
slon = sin(rlon);
Enu[0] = -xyz[0]*slon + xyz[1]*clon;
Enu[1] = -xyz[0]*slat*clon - xyz[1]*slat*slon + xyz[2]*clat;
Enu[2] = xyz[0]*clat*clon + xyz[1]*clat*slon + xyz[2]*slat;
}
/*===========================================================*/
void ComputeGpsVel( void )
{
int i,j,k,l,m,n;
double dxyz[3], denu[3], dt;
double xyz0[3], xyz1[3];
LlhXyz(StartGpsPos.Lat, StartGpsPos.Lon, 0.0, xyz0 );
LlhXyz(EndGpsPos.Lat, EndGpsPos.Lon, 0.0, xyz1 );
for(i=0;i<3;i++) dxyz[i] = xyz1[i] - xyz0[i];
XyzToEnu(StartGpsPos.Lat, StartGpsPos.Lon, dxyz, denu);
dt = EndGpsPos.Utc - StartGpsPos.Utc;
if(dt < 0) dt += 604800.0; /* correct for week crossover */
if(dt >= 1)
{
GpsVeast = denu[0] / dt;