navcom.c

来自「gpsd, a popular GPS daemon.」· C语言 代码 · 共 1,249 行 · 第 1/4 页

#define SF_BETA2 (65536)
/* 2^16 */
#define SF_BETA3 (65536)
    unsigned char *buf = session->packet.outbuffer + 3;
    u_int16_t week = getuw(buf, 3);
    u_int32_t tow = getul(buf, 5);
    int8_t alpha0 = getsb(buf, 9);
    int8_t alpha1 = getsb(buf, 10);
    int8_t alpha2 = getsb(buf, 11);
    int8_t alpha3 = getsb(buf, 12);
    int8_t beta0 = getsb(buf, 13);
    int8_t beta1 = getsb(buf, 14);
    int8_t beta2 = getsb(buf, 15);
    int8_t beta3 = getsb(buf, 16);
    int32_t a1 = getsl(buf, 17);
    int32_t a0 = getsl(buf, 21);
    u_int8_t tot = getub(buf, 25);
    u_int8_t wnt = getub(buf, 26);
    int8_t dtls = getsb(buf, 27);
    u_int8_t wnlsf = getub(buf, 28);
    u_int8_t dn = getub(buf, 29);
    int8_t dtlsf = getsb(buf, 30);

    /*@ +charint +relaxtypes @*/
    /* Ref.: ICD-GPS-200C 20.3.3.5.2.4 */
    if ((week%256)*604800+tow/1000.0 < wnlsf*604800+dn*86400) {
        /* Effectivity time is in the future, use dtls */
        session->context->leap_seconds = (int)dtls;
    } else {
        /* Effectivity time is not in the future, use dtlsf */
        session->context->leap_seconds = (int)dtlsf;
    }
    /*@ -relaxtypes -charint @*/
    
    session->gpsdata.sentence_time = gpstime_to_unix((int)week, tow/1000.0)
            - session->context->leap_seconds;
    
    gpsd_report(LOG_PROG,
                "Navcom: received packet type 0x83 (Ionosphere and UTC Data)\n");
    gpsd_report(LOG_IO,
                "Navcom: Scaled parameters follow:\n");
    gpsd_report(LOG_IO,
                "Navcom: GPS Week: %u, GPS Time of Week: %lu (GPS Time: %f)\n",
                week, tow, week*604800+tow/1000.0);
    gpsd_report(LOG_IO,
                "Navcom: a0: %12.4E, a1: %12.4E, a2: %12.4E, a3: %12.4E, "
                "b0: %12.4E, b1: %12.4E, b2: %12.4E, b3: %12.4E\n",
                (double)alpha0*SF_ALPHA0, (double)alpha1*SF_ALPHA1,
                (double)alpha2*SF_ALPHA2, (double)alpha3*SF_ALPHA3,
                (double)beta0*SF_BETA0, (double)beta1*SF_BETA1,
                (double)beta2*SF_BETA2, (double)beta3*SF_BETA3);
    gpsd_report(LOG_IO,
                "Navcom: A0: %19.12E, A1: %19.12E\n", (double)a0*SF_A0, (double)a1*SF_A1);
    gpsd_report(LOG_IO,
                "Navcom: UTC Ref. Time: %u, UTC Ref. Week: %u, dTls: %d\n",
                (unsigned long)tot*SF_TOT, wnt, dtls);
    gpsd_report(LOG_IO,
               "Navcom: Week of leap seconds: %u, Day number of leap seconds: %u, dTlsf: %d\n",
               wnlsf, dn, dtlsf);
    
    return 0; /* No flag for update of leap seconds (Not part of a fix) */
    
#undef SF_A0
#undef SF_A1
#undef SF_TOT
#undef SF_ALPHA0
#undef SF_ALPHA1
#undef SF_ALPHA2
#undef SF_ALPHA3
#undef SF_BETA0
#undef SF_BETA1
#undef SF_BETA2
#undef SF_BETA3
}

/* Acknowledgement (without error) */
static gps_mask_t handle_0x06(struct gps_device_t *session)
{
    unsigned char *buf = session->packet.outbuffer + 3;
    u_int8_t cmd_id = getub(buf, 3);
    u_int8_t port = getub(buf, 4);
    session->driver.navcom.physical_port = port; /* This tells us which serial port was used last */
    gpsd_report(LOG_PROG,
                "Navcom: received packet type 0x06 (Acknowledgement (without error))\n");
    /*@ -type @*/
    gpsd_report(LOG_IO,
                "Navcom: acknowledged command id 0x%02x on port %c\n",
                cmd_id, (port==0?'A':(port==1?'B':'?')));
    /*@ +type @*/
    return 0; /* Nothing updated */
}

/* Negative Acknowledge */
static gps_mask_t handle_0x15(struct gps_device_t *session)
{
    size_t n;
    unsigned char *buf = session->packet.outbuffer + 3;
    size_t msg_len = (size_t)getuw(buf, 1);
    /*@ -type @*/
    u_int8_t port, cmd_id = getub(buf, 3);
    gpsd_report(LOG_PROG,
                "Navcom: received packet type 0x15 (Negative Acknowledge)\n");
    for (n=4; n<(msg_len-2); n+=2) {
        u_int8_t err_id = getub(buf, n);
        u_int8_t err_desc = getub(buf, n+1);
        gpsd_report(LOG_IO,
                    "Navcom: error id = 0x%02x, error description = 0x%02x\n",
                   err_id, err_desc);
    }
    port = getub(buf, n);
    gpsd_report(LOG_IO,
                "Navcom: negative acknowledge was for command id 0x%02x on port %c\n",
                cmd_id, (port==0?'A':(port==1?'B':'?')));
    /*@ -type @*/
    return 0; /* Nothing updated */
}

/* PVT Block */
static gps_mask_t handle_0xb1(struct gps_device_t *session)
{
    unsigned int n;
    unsigned char *buf = session->packet.outbuffer + 3;
    uint16_t week;
    uint32_t tow;
    uint32_t sats_used;
    int32_t lat, lon;
    /* Resolution of lat/lon values (2^-11) */
#define LL_RES (0.00048828125)
    uint8_t lat_fraction, lon_fraction;
    /* Resolution of lat/lon fractions (2^-15) */
#define LL_FRAC_RES (0.000030517578125)
    uint8_t nav_mode;
    int32_t ellips_height, altitude;
    /* Resolution of height and altitude values (2.0^-10) */
#define EL_RES (0.0009765625)
    double vel_north, vel_east, vel_up;
    /* Resolution of velocity values (2.0^-10) */
#define VEL_RES (0.0009765625)
    double track;
    uint8_t fom, gdop, pdop, hdop, vdop, tdop, tfom;
    /* This value means "undefined" */
#define DOP_UNDEFINED (255)
    
    int16_t ant_height_adj;
    int32_t set_delta_up;
    /* Resolution of delta north, east, and up,
       and ant. height adjustment values (1mm) */
#define D_RES (0.001)

#ifdef __UNUSED__
    /* Other values provided by the PVT block which we
       may want to provide in the future.  At the present
       moment, the gpsd protocol does not have a mechanism
       to make this available to the user */
    uint8_t dgps_conf;
    uint16_t max_dgps_age;
    uint8_t ext_nav_mode;
    int32_t set_delta_north, set_delta_east;
    uint8_t nav_failure_code;
#endif /* __UNUSED__ */

    /* Timestamp */
    week = (uint16_t)getuw(buf, 3);
    tow = (uint32_t)getul(buf, 5);
    session->gpsdata.fix.time = session->gpsdata.sentence_time = gpstime_to_unix((int)week, tow/1000.0) - session->context->leap_seconds;

    /* Satellites used */
    sats_used = (uint32_t)getul(buf, 9);
    session->gpsdata.satellites_used = 0;
    for (n = 0; n < 31; n++) {
    	if ((sats_used & (0x01 << n)) != 0)
    	  session->gpsdata.used[session->gpsdata.satellites_used++] = (int)(n+1);
    }

    /* Get latitude, longitude */
    lat = getsl(buf, 13);
    lon = getsl(buf, 17);
    lat_fraction = (uint8_t)(getub(buf, 21) >> 4);
    lon_fraction = (uint8_t)(getub(buf, 21) & 0x0f);

    session->gpsdata.fix.latitude = (double)(lat * LL_RES + lat_fraction * LL_FRAC_RES ) / 3600;
    session->gpsdata.fix.longitude = (double)(lon * LL_RES + lon_fraction * LL_FRAC_RES ) / 3600;

    /* Nav mode */
    nav_mode = (uint8_t)getub(buf, 22);
    if (-nav_mode & 0x80) {
        session->gpsdata.status = STATUS_NO_FIX;
        session->gpsdata.fix.mode = MODE_NO_FIX;
    } else {
        session->gpsdata.fix.mode = (nav_mode & 0x40 ? MODE_3D : MODE_2D);
        session->gpsdata.status = (nav_mode & 0x03 ? STATUS_DGPS_FIX : STATUS_FIX);
    }
    
    /* Height Data */
    ellips_height = getsl(buf, 23);
    altitude = getsl(buf, 27);

    ant_height_adj = getsw(buf, 51);
    set_delta_up = getsl(buf, 79);
    
    session->gpsdata.fix.altitude = (double)(altitude * EL_RES)
            + (ant_height_adj * D_RES) + (set_delta_up * D_RES);
    session->gpsdata.separation = (double)(ellips_height - altitude)*EL_RES
            + (ant_height_adj * D_RES) + (set_delta_up * D_RES);

    /* Speed Data */
    vel_north = (double)getsl24(buf, 31);
    vel_east = (double)getsl24(buf, 34);
    /* vel_up = getsl24(buf, 37); */
    vel_up = (double)getsl24(buf, 37);
    
    track = atan2(vel_east, vel_north);
    if (track < 0)
    	track += 2 * PI;
    session->gpsdata.fix.track = track * RAD_2_DEG;
    /*@ -evalorder @*/
    session->gpsdata.fix.speed = sqrt(pow(vel_east,2) + pow(vel_north,2)) * VEL_RES;
    /*@ +evalorder @*/
    session->gpsdata.fix.climb = vel_up * VEL_RES;

    /* Quality indicators */
    /*@ -type @*/
    fom  = getub(buf, 40);
    gdop = getub(buf, 41);
    pdop = getub(buf, 42);
    hdop = getub(buf, 43);
    vdop = getub(buf, 44);
    tdop = getub(buf, 45);
    tfom = getub(buf, 46);
    /*@ +type @*/

    /* splint apparently gets confused about C promotion rules. */
    /* "Assignment of arbitrary unsigned integral type to double" on these */
#ifndef S_SPLINT_S
    session->gpsdata.fix.eph = fom/100.0*1.96/*Two sigma*/;
    /* FIXME - Which units is tfom in (spec doesn't say) and
               which units does gpsd require? (docs don't say) */
    session->gpsdata.fix.ept = tfom*1.96/*Two sigma*/;
    /* FIXME This cannot possibly be right */
    /* I cannot find where to get VRMS from in the Navcom output, though,
       and this value seems to agree with the output from other software */
    session->gpsdata.fix.epv = (double)fom/(double)hdop*(double)vdop/100.0*1.96/*Two sigma*/;

    if (gdop == DOP_UNDEFINED)
        session->gpsdata.gdop = NAN;
    else
        session->gpsdata.gdop = gdop/10.0;
    if (pdop == DOP_UNDEFINED)
        session->gpsdata.pdop = NAN;
    else
        session->gpsdata.pdop = pdop/10.0;
    if (hdop == DOP_UNDEFINED)
        session->gpsdata.hdop = NAN;
    else
        session->gpsdata.hdop = hdop/10.0;
    if (vdop == DOP_UNDEFINED)
        session->gpsdata.vdop = NAN;
    else
        session->gpsdata.vdop = vdop/10.0;
    if (tdop == DOP_UNDEFINED)
        session->gpsdata.tdop = NAN;
    else
        session->gpsdata.tdop = tdop/10.0;
#endif /* S_SPLINT_S */

    gpsd_report(LOG_PROG, "Navcom: received packet type 0xb1 (PVT Report)\n");
    gpsd_report(LOG_IO, "Navcom: navigation mode %s (0x%02x) - %s - %s\n",
                (-nav_mode&0x80?"invalid":"valid"), nav_mode,
                (nav_mode&0x40?"3D":"2D"), (nav_mode&0x03?"DGPS":"GPS"));
    gpsd_report(LOG_IO, "Navcom: latitude = %f, longitude = %f, altitude = %f, geoid = %f\n",
                session->gpsdata.fix.latitude, session->gpsdata.fix.longitude,
                session->gpsdata.fix.altitude, session->gpsdata.separation);
    gpsd_report(LOG_IO,
                "Navcom: velocities: north = %f, east = %f, up = %f (track = %f, speed = %f)\n",
                vel_north*VEL_RES, vel_east*VEL_RES, vel_up*VEL_RES,
                session->gpsdata.fix.track, session->gpsdata.fix.speed);
    gpsd_report(LOG_IO,
                "Navcom: hrms = %f, vrms = %f, gdop = %f, pdop = %f, "
                "hdop = %f, vdop = %f, tdop = %f\n",
                session->gpsdata.fix.eph, session->gpsdata.fix.epv,
                session->gpsdata.gdop, session->gpsdata.pdop,
                session->gpsdata.hdop, session->gpsdata.vdop, session->gpsdata.tdop);
#undef D_RES
#undef LL_RES
#undef LL_FRAC_RES
#undef EL_RES
#undef VEL_RES
#undef DOP_UNDEFINED
    
    return LATLON_SET | ALTITUDE_SET | CLIMB_SET | SPEED_SET | TRACK_SET | TIME_SET
        | STATUS_SET | MODE_SET | USED_SET | HERR_SET | VERR_SET | TIMERR_SET | DOP_SET 
        | CYCLE_START_SET;
}

/* Packed Ephemeris Data */
static gps_mask_t handle_0x81(struct gps_device_t *session)
{
    /* Scale factors for everything */
    /* 2^-31 */
#define SF_TGD       (.000000000465661287307739257812)
    /* 2^4 */
#define SF_TOC     (16)
    /* 2^-55 */
#define SF_AF2       (.000000000000000027755575615628)
    /* 2^-43 */
#define SF_AF1       (.000000000000113686837721616029)
    /* 2^-31 */
#define SF_AF0       (.000000000465661287307739257812)
    /* 2^-5 */
#define SF_CRS       (.031250000000000000000000000000)
    /* 2^-43 */
#define SF_DELTA_N   (.000000000000113686837721616029)
    /* 2^-31 */