postime.c

GPS导航定位程序

    slon = sin(lon);
    clon = cos(lon);

    t[0][0] = -slat*clon;
    t[0][1] = -slat*slon;
    t[0][2] = clat;
    t[1][0] = -slon;
    t[1][1] = clon;
    t[1][2] = 0.0;
    t[2][0] = clat*clon;
    t[2][1] = clat*slon;
    t[2][2] = slat;
}

/****************************************************************************
* Function: void LatLonToDegMin(double lat, double lon,
*                               char latdegmin[], char londegmin[])
*
* Converts latitude and longitude (in radians) into text strings expressed as
* degrees, minutes and fractional minutes.
*
* Input: lat - latitude, radians.
*        lon - longitude, radians.
*
* Output: latdegmin[] - text string for the latitude.
*         londegmin[] - text string for the longitude.
*
* Return Value: None.
****************************************************************************/
void LatLonToDegMin(double lat, double lon,
                    char latdegmin[], char londegmin[])
{
    char hemi;                                          /* The hemisphere. */
    int degrees;
    double minutes;

    /* First do the latitude. */

    lat = lat * R2D;                                /* Convert to degrees. */
    if(lat<0.0)                                     /* Get the hemisphere. */
    {
        hemi = 'S';
        lat = -lat;
    }
    else
        hemi = 'N';
        
    degrees = (int)lat;                                  /* Whole degrees. */
    minutes = (long)((lat-degrees)*600000.0 + 0.5)/10000.0;
    if(minutes>=60.0)
    {
        degrees++;
        minutes -= 60.0;
    }
    sprintf(latdegmin,"%c %2i%c%07.4f'",hemi,degrees,DEGREE,minutes);

    /* Now do the longitude. */

    lon = lon * R2D;                                /* Convert to degrees. */
    if(lon<0.0)                                     /* Get the hemisphere. */
    {
        hemi = 'W';
        lon = -lon;
    }
    else
        hemi = 'E';

    degrees = (int)lon;                                  /* Whole degrees. */
    minutes = (long)((lon-degrees)*600000.0 + 0.5)/10000.0;
    if(minutes>=60.0)
    {
        degrees++;
        minutes -= 60.0;
    }
    sprintf(londegmin,"%c%3i%c%07.4f'",hemi,degrees,DEGREE,minutes);
}

/****************************************************************************
* Function: int PosStrToLatLonHgt(char *s,
*                                 double *lat, double *lon, double *hgt)
*
* Coverts a position string containing latitude, longitude and height into
* the respective components (accounting for the sign due to the hemisphere).
*
* Input: s - pointer to the position string.
*
* Output: lat - pointer to the latitude, radians.
*         lon - pointer to the longitude, radians.
*         hgt - pointer to the height, m.
*
* Return Value: TRUE or FALSE depending upon string validity.
****************************************************************************/
int PosStrToLatLonHgt(char *s, double *lat, double *lon, double *hgt)
{
    double templat,templon,temphgt;
    int i,lathemi,lonhemi;

    if(s[0]==0)
        return(FALSE);                               /* Null input string. */

    /* Find the N or S. */

    i=0;
    while(TRUE)
    {
        if(s[i]==0)
            return(FALSE);
        if(s[i]=='N')
        {
            lathemi = 1;
            i++;
            break;
        }
        if(s[i]=='S')
        {
            lathemi = -1;
            i++;
            break;
        }
        i++;
    }

    /* Pick up the latitude degrees. */

    while(TRUE)
    {
        if(s[i]==0)
            return(FALSE);
        if(isdigit(s[i]))
            break;
        i++;
    }
    templat = atoi(&s[i]);
    while(TRUE)
    {
        if(s[i]==0)
            return(FALSE);
        if(isdigit(s[i])==0)
            break;
        i++;
    }
    while(TRUE)
    {
        if(s[i]==0)
            return(FALSE);
        if(isdigit(s[i]))
            break;
        i++;
    }
    templat += atof(&s[i])/60.0;
    if(templat>90.0)
        return(FALSE);
    templat *= lathemi/R2D;
    while(TRUE)
    {
        if(s[i]==0)
            return(FALSE);
        if(isdigit(s[i])==0 && s[i]!='.')
            break;
        i++;
    }

    /* Find the E or W. */

    while(TRUE)
    {
        if(s[i]==0)
            return(FALSE);
        if(s[i]=='E')
        {
            lonhemi = 1;
            i++;
            break;
        }
        if(s[i]=='W')
        {
            lonhemi=-1;
            i++;
            break;
        }
        i++;
    }

    /* Pick up the longitude degrees. */

    while(TRUE)
    {
        if(s[i]==0)
            return(TRUE);
        if(isdigit(s[i]))
            break;
            i++;
    }
    templon = atoi(&s[i]);
    while(TRUE)
    {
        if(s[i]==0)
        return(FALSE);
        if(isdigit(s[i])==0)
            break;
        i++;
    }
    while(TRUE)
    {
        if(s[i]==0)
            return(FALSE);
        if(isdigit(s[i]))
            break;
        i++;
    }
    templon += atof(&s[i])/60.0;
    if(templon>180.0)
        return(FALSE);
    templon *= lonhemi/R2D;

    /* Find the height (last numerical value in the string.) */

    while(TRUE)
    {
        if(s[i]==0)
            return(FALSE);
        if(isdigit(s[i])==0 && s[i]!='.')
            break;
        i++;
    }
    while(TRUE)
    {
        if(s[i]==0)
            break;
        if(isdigit(s[i]))
            break;
        if(s[i]=='-')
            break;
        if(s[i]=='.')
            break;
        i++;
    }
    temphgt = atof(&s[i]);

    *lat = templat;
    *lon = templon;
    *hgt = temphgt;

    return(TRUE);
}

/* These tables are used to convert from day of year to month and date. */

static int RegYearMonthTable[13] =
                            {0,31,59,90,120,151,181,212,243,273,304,334,365};
static int LeapYearMonthTable[13] =
                            {0,31,60,91,121,152,182,213,244,274,305,335,366};

/****************************************************************************
* Function: void GpsTimeToUTCDate(int gwk, double gsec,
*                       int *y, int *m, int *d, int *hh, int *mm, double *ss)
*
* Converts from GPS time in week number and seconds to the UTC date and time.
*
* Input: gwk - the GPS week number.
*        gsec the GPS seconds into the week.
*
* Output: y - pointer to the UTC year.
*         m - pointer to the UTC month.
*         d - pointer to the UTC day.
*         hh - pointer to the UTC hour.
*         mm - pointer to the UTC min.
*         ss - pointer to the UTC sec.
*
* Return Value: None.
****************************************************************************/
void GpsTimeToUTCDate(int gwk, double gsec, int *y, int *m, int *d,
                      int *hh, int *mm, double *ss)
{
    int i;
    int UTCRefWeek;
    int SchedLeapSecEventWeek;
    int PriorLeapSecs;
    int TodaysLeapSecs;
    int HourOfUTCDay;
    int MinuteOfUTCHour;
    int CompletedLeapDays;
    int CompletedUTCYears;
    int DayOfUTCYear;
    int PrevYearsLeapDays;

    unsigned long WholeUTCSecs;
    unsigned long UTCDayNumber;
    unsigned long SecOfDay;
    unsigned long UTCSecNumber;

    double FractUTCSecs;
    double TimeToLeapSecondEvent;
    double dt;
    double SecOfUTCMinute;

    /* Adjust GPS weeks/seconds to guarantee that seconds is in the
       range 0 - 604800.0. */

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

    /* If a valid broadcast UTC model is available, do the following:

       1) Pick up the cumulative prior leap seconds count.
       2) Determine whether a leap second event is scheduled for the
          end of the current day.
       3) Use the UTC polynomial to convert from the GPS time base
          to the UTC time base. */

    if(ionoutc.vflg)
    {
        /* Convert from GPS time to UTC time, using the broadcast UTC
           model. */

        PROTECT++;

        /* Unwrap the week parameters in the UTC model, assuming that the
           difference between the untruncated week parameters and the
           current GPS week does not exceed 127. */

        UTCRefWeek = gwk + (int)(ionoutc.wnt - (gwk&0xFF));
        if(gwk-UTCRefWeek>127)
            UTCRefWeek += 256;
        else if(gwk-UTCRefWeek<-127)
            UTCRefWeek -= 256;

        SchedLeapSecEventWeek = gwk + (int)(ionoutc.wnlsf - (gwk&0xFF));
        if(gwk-SchedLeapSecEventWeek>127)
            SchedLeapSecEventWeek += 256;
        else if(gwk-SchedLeapSecEventWeek<-127)
            SchedLeapSecEventWeek -= 256;

        /* Change from GPS to UTC time reference. */