navcom.c

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

#endif /*  __UNUSED__ */
        session->gpsdata.epe = rms*1.96;
#ifdef __UNUSED__
        session->gpsdata.fix.eph = hrms*1.96;
        session->gpsdata.fix.epv = alt_sd*1.96;
#endif /*  __UNUSED__ */
        session->gpsdata.sentence_time = gpstime_to_unix((int)week, tow/1000.0)
                - session->context->leap_seconds;
        gpsd_report(LOG_PROG,
                    "Navcom: received packet type 0xb5 (Pseudorange Noise Statistics)\n");
        gpsd_report(LOG_IO,
                    "Navcom: epe = %f\n", session->gpsdata.epe);
        return TIME_SET | PERR_SET;
    } else {
        /* Ignore this message block */
        if (!session->driver.navcom.warned) {
            gpsd_report(LOG_WARN,
                        "Navcom: received packet type 0xb5 (Pseudorange Noise Statistics) ignored "
                                " - sizeof(double) == 64 bits required\n");
            session->driver.navcom.warned = true;
        }
        return 0; /* Block ignored - wrong sizeof(double) */
    }
}

/* LBM DSP Status Block */
static gps_mask_t handle_0xd3(struct gps_device_t *session UNUSED)
{
    /* This block contains status information about the
       unit's L-band (Inmarsat) module.  There is nothing
       interesting in it for our purposes so we do not deal
       with it.  This callback is purely to a) stop
       "unrecognised packet" messages appearing in the log
       and b) explain what it is for the curious */
    return 0;  /* Nothing updated */
}

/* Identification Block */
static gps_mask_t handle_0xae(struct gps_device_t *session)
{
    char *engconfstr, *asicstr;
    unsigned char *buf = session->packet.outbuffer + 3;
    size_t    msg_len = (size_t)getuw(buf, 1);
    u_int8_t  engconf = getub(buf, 3);
    u_int8_t  asic    = getub(buf, 4);
    u_int8_t swvermaj = getub(buf, 5);
    u_int8_t swvermin = getub(buf, 6);
    u_int16_t dcser   = getuw(buf, 7);
    u_int8_t  dcclass = getub(buf, 9);
    u_int16_t rfcser  = getuw(buf, 10);
    u_int8_t  rfcclass= getub(buf, 12);
    /*@ -stringliteralnoroomfinalnull -type @*/
    u_int8_t  softtm[17] = "\0\0\0\0\0\0\0\0\0\0\0\0\0\0\0\0\0";
    u_int8_t  bootstr[17] = "\0\0\0\0\0\0\0\0\0\0\0\0\0\0\0\0\0";
    u_int8_t  ioptm[17] = "\0\0\0\0\0\0\0\0\0\0\0\0\0\0\0\0\0";
    /*@ +stringliteralnoroomfinalnull +type @*/
    u_int8_t  iopvermaj  = (u_int8_t)0x00;
    u_int8_t  iopvermin  = (u_int8_t)0x00;
    u_int8_t  picver = (u_int8_t)0x00;
    u_int8_t  slsbn = (u_int8_t)0x00;
    u_int8_t  iopsbn = (u_int8_t)0x00;
    memcpy(softtm, &buf[13], 16);
    memcpy(bootstr, &buf[29], 16);
    if (msg_len == 0x0037) { /* No IOP */
        slsbn = getub(buf, 53);
    } else { /* IOP Present */
        iopvermaj  = getub(buf, 53);
        iopvermin  = getub(buf, 54);
        memcpy(ioptm, &buf[55], 16);
        picver     = getub(buf, 71);
        slsbn      = getub(buf, 72);
        iopsbn     = getub(buf, 73);
    }

    switch(engconf)
    {
    case 0x00:
        engconfstr = "Unknown/Undefined";
        break;
    case 0x01:
        engconfstr = "NCT 2000 S";
        break;
    case 0x02:
        engconfstr = "NCT 2000 D";
        break;
    case 0x03:
        engconfstr = "Startfire Single";
        break;
    case 0x04:
        engconfstr = "Starfire Dual";
        break;
    case 0x05:
        engconfstr = "Pole Mount RTK (Internal Radio)";
        break;
    case 0x06:
        engconfstr = "Pole Mount GIS (LBM)";
        break;
    case 0x07:
        engconfstr = "Black Box RTK (Internal Radio)";
        break;
    case 0x08:
        engconfstr = "Black Box GIS (LBM)";
        break;
    case 0x80:
        engconfstr = "R100";
        break;
    case 0x81:
        engconfstr = "R200";
        break;
    case 0x82:
        engconfstr = "R210";
        break;
    case 0x83:
        engconfstr = "R300";
        break;
    case 0x84:
        engconfstr = "R310";
        break;
    default:
        engconfstr = "?";
    }

    switch(asic)
    {
    case 0x01:
        asicstr = "A-ASIC";
        break;
    case 0x02:
        asicstr = "B-ASIC";
        break;
    case 0x03:
        asicstr = "C-ASIC";
        break;
    case 0x04:
        asicstr = "M-ASIC";
        break;
    default:
        asicstr = "?";
    }

    gpsd_report(LOG_PROG, "Navcom: received packet type 0xae (Identification Block)\n");
    if(msg_len == 0x0037) {
        gpsd_report(LOG_INF, "Navcom: ID Data: "
                    "%s %s Ver. %u.%u.%u, DC S/N: %u.%u, RF S/N: %u.%u, "
                    "Build ID: %s, Boot software: %s\n",
                    engconfstr, asicstr, swvermaj, swvermin, slsbn, dcser, dcclass,
                    rfcser, rfcclass, softtm, bootstr);
    } else {
        gpsd_report(LOG_INF, "Navcom: ID Data: "
                    "%s %s Ver. %u.%u.%u, DC S/N: %u.%u, RF S/N: %u.%u, "
                    "Build ID: %s, Boot software: %s, "
                    "IOP Ver.: %u.%u.%u, PIC: %u, IOP Build ID: %s\n",
                    engconfstr, asicstr, swvermaj, swvermin, slsbn, dcser, dcclass,
                    rfcser, rfcclass, softtm, bootstr, iopvermaj, iopvermin, iopsbn,
                    picver, ioptm);
    }

    /*@ -formattype @*/
    (void)snprintf(session->subtype, sizeof(session->subtype),
             "%s %s Ver. %u.%u.%u S/N %u.%u %u.%u",
             engconfstr, asicstr, swvermaj, swvermin, slsbn, dcser, dcclass,
             rfcser, rfcclass);
    /*@ +formattype @*/
    return DEVICEID_SET;
}

/* Clock Drift and Offset */
static gps_mask_t handle_0xef(struct gps_device_t *session)
{
    unsigned char *buf = session->packet.outbuffer + 3;
    u_int16_t week = getuw(buf, 3);
    u_int32_t tow = getul(buf, 5);
    int8_t osc_temp = getsb(buf, 9);
    u_int8_t nav_status = getub(buf, 10);
    union long_double l_d;
    double nav_clock_offset;
    union int_float i_f;
    float nav_clock_drift;
    float osc_filter_drift_est;
    int32_t time_slew = (int32_t)getsl(buf, 27);
    if (sizeof(double) == 8) {
        nav_clock_offset = getd(buf, 11);
    } else {
        nav_clock_offset = NAN;
    }
    if (sizeof(float) == 4) {    
        nav_clock_drift = getf(buf, 19);
        osc_filter_drift_est = getf(buf, 23);
    } else {
        nav_clock_drift = NAN;
        osc_filter_drift_est = NAN;
    }
    
    session->gpsdata.sentence_time = gpstime_to_unix((int)week, tow/1000.0) 
            - session->context->leap_seconds;
    
    gpsd_report(LOG_PROG,
                "Navcom: received packet type 0xef (Clock Drift and Offset)\n");
    gpsd_report(LOG_IO,
                "Navcom: oscillator temp. = %d, nav. status = 0x%02x, "
                "nav. clock offset = %f, nav. clock drift = %f, "
                "osc. filter drift est. = %f, acc.time slew value = %f\n",
                osc_temp, nav_status, nav_clock_offset, nav_clock_drift,
                osc_filter_drift_est, time_slew);
    return TIME_SET;
}


/*@ +charint @*/
gps_mask_t navcom_parse(struct gps_device_t *session, unsigned char *buf, size_t len)
{
    unsigned char cmd_id;
    unsigned char *payload;
    unsigned int  msg_len;

    if (len == 0)
	return 0;

    cmd_id = getub(buf, 3);
    payload = &buf[6];
    msg_len = getuw(buf, 4);
   
    /*@ -usedef -compdef @*/
    gpsd_report(LOG_RAW, "Navcom: packet type 0x%02x, length %d: %s\n",
        cmd_id, msg_len, gpsd_hexdump(buf, len));
    /*@ +usedef +compdef @*/

    (void)snprintf(session->gpsdata.tag, sizeof(session->gpsdata.tag),
		   "0x%02x",cmd_id);

    switch (cmd_id)
    {
    case 0x06:
        return handle_0x06(session);
    case 0x15:
	return handle_0x15(session);
    case 0x81:
        return handle_0x81(session);
    case 0x83:
        return handle_0x83(session);
    case 0x86:
	return handle_0x86(session);
    case 0xae:
        return handle_0xae(session);
    case 0xb0:
        return handle_0xb0(session);
    case 0xb1:
	return handle_0xb1(session);
    case 0xb5:
	return handle_0xb5(session);
    case 0xd3:
        return handle_0xd3(session);
    case 0xef:
        return handle_0xef(session);
    default:
        gpsd_report(LOG_PROG,
                    "Navcom: received packet type 0x%02x, length %d - unknown or unimplemented\n",
		    cmd_id, msg_len);
	return 0;
    }
}
/*@ -charint @*/

static gps_mask_t navcom_parse_input(struct gps_device_t *session)
{
    gps_mask_t st;

    if (session->packet.type == NAVCOM_PACKET){
	st = navcom_parse(session, session->packet.outbuffer, session->packet.outbuflen);
	session->gpsdata.driver_mode = 1;  /* binary */
	return st;
#ifdef NMEA_ENABLE
    } else if (session->packet.type == NMEA_PACKET) {
	st = nmea_parse((char *)session->packet.outbuffer, session);
	session->gpsdata.driver_mode = 0;  /* NMEA */
	return st;
#endif /* NMEA_ENABLE */
    } else
	return 0;
}


/* this is everything we export */
struct gps_type_t navcom_binary =
{
    .type_name      = "Navcom binary",  	/* full name of type */
    .trigger        = "\x02\x99\x66",           /* Every packet begins with this */
    .channels       = NAVCOM_CHANNELS,		/* 12 L1 + 12 L2 + 2 Inmarsat L-Band */
    .probe_wakeup   = navcom_ping,		/* wakeup to be done before hunt */
    .probe_detect   = NULL,			/* no probe */
    .probe_subtype  = navcom_probe_subtype,	/* subtype probing */
#ifdef ALLOW_RECONFIGURE
    .configurator   = NULL,			/* no reconfigure */
#endif /* ALLOW_RECONFIGURE */
    .get_packet     = generic_get,		/* use generic one */
    .parse_packet   = navcom_parse_input,	/* parse message packets */
    .rtcm_writer    = pass_rtcm,		/* send RTCM data straight */
    .speed_switcher = navcom_speed,		/* we do change baud rates */
    .mode_switcher  = NULL,			/* there is not a mode switcher */
    .rate_switcher  = NULL,			/* no sample-rate switcher */
    .cycle_chars    = -1,			/* ignore, no rate switch */
#ifdef ALLOW_RECONFIGURE
    .revert         = NULL,			/* no reversion code */
#endif /* ALLOW_RECONFIGURE */
    .wrapup         = NULL,			/* ignore, no wrapup */
    .cycle          = 1,			/* updates every second */
};

#endif /* defined(NAVCOM_ENABLE) && defined(BINARY_ENABLE) */