IMU_DUAL/nmea/rtkcmn.c

#include "rtklib.h"

static const double gpst0[] = {1980, 1, 6, 0, 0, 0}; /* gps time reference */
//static const double gst0[] = {1999, 8, 22, 0, 0, 0}; /* galileo system time reference */
//static const double bdt0[] = {2006, 1, 1, 0, 0, 0};  /* beidou time reference */
//
static double leaps[MAXLEAPS + 1][7] = {/* leap seconds (y,m,d,h,m,s,utc-gpst) */
                                        {2017, 1, 1, 0, 0, 0, -18},
                                        {2015, 7, 1, 0, 0, 0, -17},
                                        {2012, 7, 1, 0, 0, 0, -16},
                                        {2009, 1, 1, 0, 0, 0, -15},
                                        {2006, 1, 1, 0, 0, 0, -14},
                                        {1999, 1, 1, 0, 0, 0, -13},
                                        {1997, 7, 1, 0, 0, 0, -12},
                                        {1996, 1, 1, 0, 0, 0, -11},
                                        {1994, 7, 1, 0, 0, 0, -10},
                                        {1993, 7, 1, 0, 0, 0, -9},
                                        {1992, 7, 1, 0, 0, 0, -8},
                                        {1991, 1, 1, 0, 0, 0, -7},
                                        {1990, 1, 1, 0, 0, 0, -6},
                                        {1988, 1, 1, 0, 0, 0, -5},
                                        {1985, 7, 1, 0, 0, 0, -4},
                                        {1983, 7, 1, 0, 0, 0, -3},
                                        {1982, 7, 1, 0, 0, 0, -2},
                                        {1981, 7, 1, 0, 0, 0, -1},
                                        {0}};
/* time to calendar day/time ---------------------------------------------------
 * convert gtime_t struct to calendar day/time
 * args   : gtime_t t        I   gtime_t struct
 *          double *ep       O   day/time {year,month,day,hour,min,sec}
 * return : none
 * notes  : proper in 1970-2037 or 1970-2099 (64bit time_t)
 *-----------------------------------------------------------------------------*/
//extern void time2epoch(gtime_t t, double *ep)
//{
//    const int mday[] = {/* # of days in a month */
//                        31, 28, 31, 30, 31, 30, 31, 31, 30, 31, 30, 31, 31, 28, 31, 30, 31, 30, 31, 31, 30, 31, 30, 31,
//                        31, 29, 31, 30, 31, 30, 31, 31, 30, 31, 30, 31, 31, 28, 31, 30, 31, 30, 31, 31, 30, 31, 30, 31};
//    int days, sec, mon, day;
//
//    /* leap year if year%4==0 in 1901-2099 */
//    days = (int)(t.time / 86400);
//    sec = (int)(t.time - (time_t)days * 86400);
//    for (day = days % 1461, mon = 0; mon < 48; mon++)
//    {
//        if (day >= mday[mon])
//            day -= mday[mon];
//        else
//            break;
//    }
//    ep[0] = 1970 + days / 1461 * 4 + mon / 12;
//    ep[1] = mon % 12 + 1;
//    ep[2] = day + 1;
//    ep[3] = sec / 3600;
//    ep[4] = sec % 3600 / 60;
//    ep[5] = sec % 60 + t.sec;
//}
/* time to gps time ------------------------------------------------------------
 * convert gtime_t struct to week and tow in gps time
 * args   : gtime_t t        I   gtime_t struct
 *          int    *week     IO  week number in gps time (NULL: no output)
 * return : time of week in gps time (s)
 *-----------------------------------------------------------------------------*/
extern double time2gpst(gtime_t t, int *week)
{
    gtime_t t0 = epoch2time(gpst0);
    time_t sec = t.time - t0.time;
    int w = (int)(sec / (86400 * 7));

    if (week)
        *week = w;
    return (double)(sec - (double)w * 86400 * 7) + t.sec;
}
extern gtime_t timeadd(gtime_t t, double sec)
{
    double tt;

    t.sec += sec;
    tt = floor(t.sec);
    t.time += (int)tt;
    t.sec -= tt;
    return t;
}
/* time difference -------------------------------------------------------------
 * difference between gtime_t structs
 * args   : gtime_t t1,t2    I   gtime_t structs
 * return : time difference (t1-t2) (s)
 *-----------------------------------------------------------------------------*/
extern double timediff(gtime_t t1, gtime_t t2)
{
    return difftime(t1.time, t2.time) + t1.sec - t2.sec;
}
/* gpstime to utc --------------------------------------------------------------
 * convert gpstime to utc considering leap seconds
 * args   : gtime_t t        I   time expressed in gpstime
 * return : time expressed in utc
 * notes  : ignore slight time offset under 100 ns
 *-----------------------------------------------------------------------------*/
extern gtime_t gpst2utc(gtime_t t)
{
    gtime_t tu;
    int i;

    for (i = 0; leaps[i][0] > 0; i++)
    {
        tu = timeadd(t, leaps[i][6]);
        if (timediff(tu, epoch2time(leaps[i])) >= 0.0)
            return tu;
    }
    return t;
}
/* utc to gpstime --------------------------------------------------------------
 * convert utc to gpstime considering leap seconds
 * args   : gtime_t t        I   time expressed in utc
 * return : time expressed in gpstime
 * notes  : ignore slight time offset under 100 ns
 *-----------------------------------------------------------------------------*/
extern gtime_t utc2gpst(gtime_t t)
{
    int i;

    for (i = 0; leaps[i][0] > 0; i++)
    {
        if (timediff(t, epoch2time(leaps[i])) >= 0.0)
            return timeadd(t, -leaps[i][6]);
    }
    return t;
}
/* gpstime to bdt --------------------------------------------------------------
 * convert gpstime to bdt (beidou navigation satellite system time)
 * args   : gtime_t t        I   time expressed in gpstime
 * return : time expressed in bdt
 * notes  : ref [8] 3.3, 2006/1/1 00:00 BDT = 2006/1/1 00:00 UTC
 *          no leap seconds in BDT
 *          ignore slight time offset under 100 ns
 *-----------------------------------------------------------------------------*/
//extern gtime_t gpst2bdt(gtime_t t)
//{
//    return timeadd(t, -14.0);
//}
/* bdt to gpstime --------------------------------------------------------------
 * convert bdt (beidou navigation satellite system time) to gpstime
 * args   : gtime_t t        I   time expressed in bdt
 * return : time expressed in gpstime
 * notes  : see gpst2bdt()
 *-----------------------------------------------------------------------------*/
//extern gtime_t bdt2gpst(gtime_t t)
//{
//    return timeadd(t, 14.0);
//}
/* transform geodetic to ecef position -----------------------------------------
 * transform geodetic position to ecef position
 * args   : double *pos      I   geodetic position {lat,lon,h} (rad,m)
 *          double *r        O   ecef position {x,y,z} (m)
 * return : none
 * notes  : WGS84, ellipsoidal height
 *-----------------------------------------------------------------------------*/
//extern void pos2ecef(const double *pos, double *r)
//{
//    double sinp = sin(pos[0]), cosp = cos(pos[0]), sinl = sin(pos[1]), cosl = cos(pos[1]);
//    double e2 = FE_WGS84 * (2.0 - FE_WGS84), v = RE_WGS84 / sqrt(1.0 - e2 * sinp * sinp);
//
//    r[0] = (v + pos[2]) * cosp * cosl;
//    r[1] = (v + pos[2]) * cosp * sinl;
//    r[2] = (v * (1.0 - e2) + pos[2]) * sinp;
//}