postime.c

GPS导航定位程序

#include "includes.h"

/* Number of days into the year at the start of each month (ignoring leap
   years). */

static int doy[12] = {0,31,59,90,120,151,181,212,243,273,304,334};

/****************************************************************************
* Function: void CurrentTIC(unsigned long *ctic)
*
* Gets the current TIC number.
*
* Input: None.
*
* Output: ctic - pointer to the TIC number.
*
* Return Value: None.
****************************************************************************/
void CurrentTIC(unsigned long *ctic)
{
    disable();
    *ctic = TIC;
    enable();
    return;
}

/****************************************************************************
* Function: void TICToReceiverTime(unsigned long curTIC, int *gwk,
*                                  double *gsec)
*
* Converts from the current TIC value to the receive time (expressed by GPS
* week number and seconds into the week).
*
* Input: None.
*
* Output: gwk - pointer to the GPS week number.
*         gsec - pointer to the seconds into the GPS week.
*
* Return Value: None.
****************************************************************************/
void TICToReceiverTime(unsigned long curTIC, int *gwk, double *gsec)
{
    PROTECT++;
    *gwk = CurClkModel.Zwk;
    *gsec = CurClkModel.Zsec + curTIC*TIC_PERIOD;
    PROTECT--;

    while(*gsec>=SECONDS_IN_WEEK)
    {
        *gsec -= SECONDS_IN_WEEK;
        *gwk += 1;
    }

    while(*gsec<0.0)
    {
        *gsec += SECONDS_IN_WEEK;
        *gwk -= 1;
    }
}

/****************************************************************************
* Function: TICToGpsTime(unsigned long curTIC, int *gwk, double *gsec)
*
* Converts from the current TIC number to GPS time (expressed by GPS week
* number and seconds into week). This effectively corrects for the receiver
* clock error.
*
* Input: curTIC - the current TIC number.
*
* Output: gwk - pointer to the GPS week number.
*         gsec - pointer to the seconds into the GPS week.
*
* Return Value: None.
****************************************************************************/
void TICToGpsTime(unsigned long curTIC, int *gwk, double *gsec)
{
    TICToReceiverTime(curTIC,gwk,gsec);             /* Get receiver time. */

    /* Correct for the clock error. */

    PROTECT++;
    *gwk -= CurClkModel.RCOwk;
    *gsec -= CurClkModel.RCOsec + 
             CurClkModel.RCOrate*(1.0-CurClkModel.RCOrate)*TIC_PERIOD*
             ((double)curTIC-(double)CurClkModel.RCOtic);
    PROTECT--;

    while(*gsec>=SECONDS_IN_WEEK)
    {
        *gsec -= SECONDS_IN_WEEK;
        *gwk += 1;
    }
    while(*gsec<0.0)
    {
        *gsec += SECONDS_IN_WEEK;
        *gwk -= 1;
    }
}

/****************************************************************************
* Function: void GpsTimeToGregorianDate(int gwk, double gsec, int *y, int *m, 
*                                       int *d, int *hh, int *mm, double *ss)
*
* Converts from a GPS time expressed by week number and seconds into week
* into a Gregorian date of the form year/month/day hour:minute:second.
*
* Input: gwk - GPS week number.
*        gsec - GPS second into week.
*
* Output: y - pointer to year.
*         m - pointer to month.
*         d - pointer to day.
*         hh - pointer to hour.
*         mm - pointer to minute.
*         ss - pointer to second.
*
* Return Value: None.
****************************************************************************/
void GpsTimeToGregorianDate(int gwk, double gsec, int *y, int *m, int *d,
                            int *hh, int *mm, double *ss)
{
    long j,t1,t2,t3,t4;
    double xhh,xmm;

    j = 723248L + 7*gwk + (int)(gsec/SECONDS_IN_DAY);
    *y = (j-121.5)/365.2425;
L10:
    t1 = (*y)*365.25;
    t2 = (*y)/100.0;
    t3 = (*y)/400.0;
    *m = (j-t1+t2-t3) / 30.6001;
    t4 = (*m)*30.6001;
    *d = j-t1+t2-t3-t4;
    if((*m) > 3) goto L20;
    *y -= 1;
    goto L10;
L20:
    if((*m) <= 13) goto L30;
    *m -= 12;
    *y += 1;
L30: 
    *m -= 1;
    xhh = fmod(gsec,SECONDS_IN_DAY) / SECONDS_IN_HOUR;
    *hh = xhh;
    xmm = (xhh-*hh)*SECONDS_IN_MINUTE;
    *mm = xmm;
    *ss = (xmm-*mm)*SECONDS_IN_MINUTE;
}

/****************************************************************************
* Function: void GregorianDateToGpsTime(int y, int m, int d, int hh, int mm,
*                      int ss, int *gwk, double *gsec)
*
* Convert Gregorian date and time to GPS time. The conversion is not
* perfectly general because:
*
* 1) UTC leap seconds are ignored.
* 2) Century leap years except 2000 are not handled.
* 3) Dates before midnight Jan 5/Jan 6, 1980 are not handled.
* 4) Integer overflow of day number occurs for years > 2068.
*
* Input: y - the year (1980-2068).
*        m - the month (1=January, 12=December).
*        d - the day.
*        hh - the hour.
*        mm - the minute.
*        ss - the second.
*
* Output: gwk - pointer to the GPS week number.
*         gsec - pointer to the GPS seconds into the week.
*
* Return Value: None.
****************************************************************************/
void GregorianDateToGpsTime(int y, int m, int d, int hh, int mm, int ss,
                            int *gwk, double *gsec)
{
    int years_elapsed;                                /* Years since 1980. */
    int days_elpased;             /* Days elapsed since Jan 5/Jan 6, 1980. */
    int leap_days;                   /* Leap days since Jan 5/Jan 6, 1980. */

    years_elapsed = y - 1980;

    leap_days = years_elapsed/4 + 1;
    if ((years_elapsed%4)==0 && m<=2)
        leap_days--;

    days_elpased = years_elapsed*365 + doy[m-1] + d + leap_days - 6;

    /* Convert time to GPS weeks and seconds */

    *gwk = days_elpased/7;
    *gsec = (days_elpased%7)*SECONDS_IN_DAY + hh*SECONDS_IN_HOUR 
            + mm*SECONDS_IN_MINUTE + ss;
}

/****************************************************************************
* Function: void LatLonHgtToXYZ(double lat, double lon, double hgt,
*                               double *x, double *y, double *z)
*
* Converts from WGS-84 latitude, longitude and height and X, Y and Z
* rectangular co-ordinates. For more information: Simo H. Laurila,
* "Electronic Surveying and Navigation", John Wiley & Sons (1976).
*
* Input: lat - the latitude, radians.
*        lon - the longitude, radians.
*        hgt - the height, m.
*
* Output: x - pointer to the x co-ordinate, m.
*         y - pointer to the y co-ordinate, m.
*         z - pointer to the z co-ordinate, m.
*         
* Return Value: None.
****************************************************************************/
void LatLonHgtToXYZ(double lat, double lon, double hgt,
                    double *x, double *y, double *z)
{
    double n;         /* WGS-84 Radius of curvature in the prime vertical. */
    double a;                                    /* WGS-84 semimajor axis. */
    double e;                                /* WGS-84 first eccentricity. */
    double ome2;                                   /* WGS-84 (1.0E0 - e2). */
    double clat;                                              /* cos(lat). */
    double slat;                                              /* sin(lat). */
    double clon;                                              /* cos(lon). */
    double slon;                                              /* sin(lon). */
    double d,nph;

    a = 6378137.0E0;
    e = 0.0818191908426E0;
    ome2 = 0.99330562000987;

    clat = cos(lat);
    slat = sin(lat);
    clon = cos(lon);
    slon = sin(lon);
    d = e*slat;

    n = a/sqrt(1.0E0-d*d);
    nph = n + hgt;

    *x = nph*clat*clon;
    *y = nph*clat*slon;
    *z = (ome2*n+hgt)*slat;
}

/****************************************************************************
* Function: void XYZToLatLonHgt(double *x, double *y, double *z,
*                               double lat, double lon, double hgt)
*
* Converts from WGS-84 X, Y and Z rectangular co-ordinates to latitude,
* longitude and height. For more information: Simo H. Laurila,
* "Electronic Surveying and Navigation", John Wiley & Sons (1976).
*
* Input: x - the x co-ordinate, m.
*        y - the y co-ordinate, m.
*        z - the z co-ordinate, m.
*
* Output: lat - pointer to the latitude, radians.
*         lon - pointer to the longitude, radians.
*         hgt - pointer to the height, m.
*
* Return Value: None.
****************************************************************************/
void XYZToLatLonHgt(double x, double y, double z,
                    double *lat, double *lon, double *hgt)
{
    double a;                                    /* WGS-84 semimajor axis. */
    double b;                            /* WGS-84 semiminor (polar) axis. */
    double e;                                /* WGS-84 first eccentricity. */
    double e2;                       /* WGS-84 first eccentricity squared. */
    double eps;                                      /* Convergence limit. */
    double dpi2;                                     /* Convergence limit. */
    double n,d,nph,rho,latold,hgtold;

    eps = 1.0E-13;
    dpi2 = 1.570796326794897E0;

    a = 6378137.0E0;
    b = 6356752.3142E0;
    e = 0.0818191908426E0;
    e2 = 0.00669437999013E0;

    /* If the position is on the Z axis, the computation must be handled as
       a special case to keep it from blowing up. */

    rho = sqrt(x*x+y*y);
    if (rho <= eps)
    {
        *lat = dpi2;                 /* Come here if we are on the Z axis. */
        if(z<0.0)
            *lat = -(*lat);
        *lon = 0.0E0;
        *hgt = fabs(z) - b;
        return;
    }

    /* Come here in the typical case.  Since latitude and spheroid height
       depend on one another, the solution must be done iteratively. */

    *lon = atan2(y,x);
    *lat = atan2(z,rho);
    *hgt = rho/cos(*lat);

    latold = *lat + 1.0E0;
    hgtold = *hgt + 1.0E0;
    while(fabs(*lat-latold)>=eps || fabs(*hgt-hgtold)>=0.01)
    {
        /* Require latitude to converge to about the precision of the
           machine, and height to the precision of about a centimeter. */

        latold = *lat;
        hgtold = *hgt;
        d = e*sin(latold);
        n = a/sqrt(1.0E0-d*d);
        *hgt = rho/cos(latold) - n;
        nph = n + *hgt;
        d = 1.0E0 - e2*(n/nph);
        *lat = atan2(z,rho*d);
    }
}

/****************************************************************************
* Function: void LatLonToNorthEastUp(double lat, double lon, double t[3][3])
*
* Determines a transformation matrix which rotates vectors in the
* geocentric xyz co-ordinate system to the topocentric (observer centered)
* North-East-Up system.
*
* Input: lat - the observer latitude, radians.
*        lon - the observer longitude, radians.
*
* Output: t[3][3] - the transformation matrix.
*
* Return Value: None.
****************************************************************************/
void LatLonToNorthEastUp(double lat, double lon, double t[3][3])
{
    double slat;                                              /* sin(lat). */
    double clat;                                              /* cos(lat). */
    double slon;                                              /* sin(lon). */
    double clon;                                              /* cos(lat). */

    slat = sin(lat);
    clat = cos(lat);