navcom.c

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

#define SF_M0        (.000000000465661287307739257812)
    /* 2^-29 */
#define SF_CUC       (.000000001862645149230957031250)
    /* 2^-33 */
#define SF_E         (.000000000116415321826934814453)
    /* 2^-29 */
#define SF_CUS       (.000000001862645149230957031250)
    /* 2^-19 */
#define SF_SQRT_A    (.000001907348632812500000000000)
    /* 2^4 */
#define SF_TOE     (16)
    /* 2^-29 */
#define SF_CIC       (.000000001862645149230957031250)
    /* 2^-31 */
#define SF_OMEGA0    (.000000000465661287307739257812)
    /* 2^-29 */
#define SF_CIS       (.000000001862645149230957031250)
    /* 2^-31 */
#define SF_I0        (.000000000465661287307739257812)
    /* 2^-5 */
#define SF_CRC       (.031250000000000000000000000000)
    /* 2^-31 */
#define SF_OMEGA     (.000000000465661287307739257812)
    /* 2^-43 */
#define SF_OMEGADOT  (.000000000000113686837721616029)
    /* 2^-43 */
#define SF_IDOT      (.000000000000113686837721616029)

    unsigned char *buf = session->packet.outbuffer + 3;
    u_int8_t prn = getub(buf, 3);
    u_int16_t week = getuw(buf, 4);
    u_int32_t tow = getul(buf, 6);
    u_int16_t iodc = getuw(buf, 10);
    /* And now the fun starts... everything that follows is
       raw GPS data minus parity */
    /* Subframe 1, words 3 to 10 minus parity */
    u_int16_t wn = (getuw_be(buf, 12)&0xffc0)>>6;
    u_int8_t cl2 = (getub(buf, 13)&0x30)>>4;
    u_int8_t ura = getub(buf, 13)&0x0f;
    u_int8_t svh = (getub(buf, 14)&0xfc)>>2;
    /* We already have IODC from earlier in the message, so
       we do not decode again */
/*    u_int16_t iodc = (getub(buf, 14)&0x03)<<8;*/
    u_int8_t l2pd = (getub(buf, 15)&0x80)>>7;
    int8_t tgd = getsb(buf, 26);
/*    iodc |= getub(buf, 27);*/
    u_int16_t toc = getuw_be(buf, 28);
    int8_t af2 = getsb(buf, 30);
    int16_t af1 = getsw_be(buf, 31);
    /*@ -shiftimplementation @*/ 
    int32_t af0 = getsl24_be(buf, 33)>>2;
    /*@ +shiftimplementation @*/ 
    /* Subframe 2, words 3 to 10 minus parity */
    u_int8_t iode = getub(buf, 36);
    int16_t crs = getsw_be(buf, 37);
    int16_t delta_n = getsw_be(buf, 39);
    int32_t m0 = getsl_be(buf, 41);
    int16_t cuc = getsw_be(buf, 45);
    u_int32_t e = getul_be(buf, 47);
    int16_t cus = getsw_be(buf, 51);
    u_int32_t sqrt_a = getul_be(buf, 53);
    u_int16_t toe = getuw_be(buf, 57);
    /* NOTE - Fit interval & AODO not collected */
    /* Subframe 3, words 3 to 10 minus parity */
    int16_t cic = getsw_be(buf, 60);
    int32_t Omega0 = getsl_be(buf, 62);
    int16_t cis = getsw_be(buf, 66);
    int32_t i0 = getsl_be(buf, 68);
    int16_t crc = getsw_be(buf, 72);
    int32_t omega = getsl_be(buf, 74);
    int32_t Omegadot = getsl24_be(buf, 78);
    /*@ -predboolothers @*/
    /* Question: What is the proper way of shifting a signed int 2 bits to 
     * the right, preserving sign? Answer: integer division by 4. */
    int16_t idot = (int16_t)(((getsw_be(buf, 82)&0xfffc)/4)|(getub(buf, 82)&80?0xc000:0x0000));
    /*@ +predboolothers @*/
    char time_str[24];
    (void)unix_to_iso8601(gpstime_to_unix((int)wn, (double)(toc*SF_TOC)), time_str, sizeof(time_str));
    
    gpsd_report(LOG_PROG,
                "Navcom: received packet type 0x81 (Packed Ephemeris Data)\n");
    gpsd_report(LOG_IO,
                "Navcom: PRN: %u, Epoch: %u (%s), SV clock bias/drift/drift rate: %#19.12E/%#19.12E/%#19.12E\n",
                prn, toc*SF_TOC, time_str, ((double)af0)*SF_AF0, ((double)af1)*SF_AF1, ((double)af2)*SF_AF2);
    gpsd_report(LOG_IO,
                "Navcom: IODE (!AODE): %u Crs: %19.12e, Delta n: %19.12e, M0: %19.12e\n",
                iode, (double)crs*SF_CRS, (double)delta_n*SF_DELTA_N*M_PI, (double)m0*SF_M0*M_PI);
    gpsd_report(LOG_IO,
                "Navcom: Cuc: %19.12e, Eccentricity: %19.12e, Cus: %19.12e, A^1/2: %19.12e\n",
                (double)cuc*SF_CUC, (double)e*SF_E, (double)cus*SF_CUS, (double)sqrt_a*SF_SQRT_A);
    gpsd_report(LOG_IO,
                "Navcom: TOE: %u, Cic: %19.12e, Omega %19.12e, Cis: %19.12e\n",
                toe*SF_TOE, (double)cic*SF_CIC, (double)Omega0*SF_OMEGA0*M_PI,
                (double)cis*SF_CIS);
    gpsd_report(LOG_IO,
                "Navcom: i0: %19.12e, Crc: %19.12e, omega: %19.12e, Omega dot: %19.12e\n",
                (double)i0*SF_I0*M_PI, (double)crc*SF_CRC, (double)omega*SF_OMEGA*M_PI,
                (double)Omegadot*SF_OMEGADOT*M_PI);
    gpsd_report(LOG_IO,
                "Navcom: IDOT: %19.12e, Codes on L2: 0x%x, GPS Week: %u, L2 P data flag: %x\n",
                (double)idot*SF_IDOT*M_PI, cl2, week-(week%1024)+wn, l2pd);
    gpsd_report(LOG_IO,
                "Navcom: SV accuracy: 0x%x, SV health: 0x%x, TGD: %f, IODC (!AODC): %u\n",
                ura, svh, (double)tgd*SF_TGD, iodc);
    gpsd_report(LOG_IO,
                "Navcom: Transmission time: %u\n",
                tow);
    
#undef SF_TGD
#undef SF_TOC
#undef SF_AF2
#undef SF_AF1
#undef SF_AF0
#undef SF_CRS
#undef SF_DELTA_N
#undef SF_M0
#undef SF_CUC
#undef SF_E
#undef SF_CUS
#undef SF_SQRT_A
#undef SF_TOE
#undef SF_CIC
#undef SF_OMEGA0
#undef SF_CIS
#undef SF_I0
#undef SF_CRC
#undef SF_OMEGA
#undef SF_OMEGADOT
#undef SF_IDOT

    return 0;
}

/* Channel Status */
static gps_mask_t handle_0x86(struct gps_device_t *session)
{
    size_t n, i;
    u_int8_t prn, tracking_status, ele, ca_snr, p2_snr, log_channel, hw_channel;
    u_int16_t azm, dgps_age;
    unsigned char *buf = session->packet.outbuffer + 3;
    size_t msg_len = (size_t)getuw(buf, 1);
    u_int16_t week = getuw(buf, 3);
    u_int32_t tow = getul(buf, 5);
    u_int8_t eng_status = getub(buf, 9);
    u_int16_t sol_status = getuw(buf, 10);
    u_int8_t sats_visible = getub(buf, 12);
    u_int8_t sats_tracked = getub(buf, 13);
    u_int8_t sats_used = getub(buf, 14);
    u_int8_t pdop = getub(buf, 15);

    /* Timestamp and PDOP */
    session->gpsdata.sentence_time = gpstime_to_unix((int)week, tow/1000.0)
            - session->context->leap_seconds;
    session->gpsdata.pdop = (int)pdop / 10.0;

    /* Satellite count */
    session->gpsdata.satellites = (int)sats_visible;
    session->gpsdata.satellites_used = (int)sats_used;

    /* Fix mode */
    switch(sol_status & 0x05)
    {
    case 0x05:
	session->gpsdata.status = STATUS_DGPS_FIX;
	break;
    case 0x01:
	session->gpsdata.status = STATUS_FIX;
	break;
    default:
	session->gpsdata.status = STATUS_NO_FIX;
    }

    /*@ -predboolothers @*/
    gpsd_report(LOG_PROG,
               "Navcom: received packet type 0x86 (Channel Status) "
               "- satellites: visible = %u, tracked = %u, used = %u\n",
	       sats_visible, sats_tracked, sats_used);
    gpsd_report(LOG_IO,
               "Navcom: engine status = 0x%x, almanac = %s, time = 0x%x, pos = 0x%x\n",
               eng_status&0x07, (eng_status&0x08?"valid":"invalid"),
               eng_status&0x30>>4, eng_status&0xc0>>6);
    /*@ +predboolothers @*/

    /* Satellite details */
    i = 0;
    for(n = 17; n < msg_len; n += 14) {
	if(i >= MAXCHANNELS) {
            gpsd_report(LOG_ERROR,
                        "Navcom: packet type 0x86: too many satellites!\n");
            gpsd_zero_satellites(&session->gpsdata);
            return ERROR_SET;
	}
        prn = getub(buf, n);
	tracking_status = getub(buf, n+1);
        log_channel = getub(buf, n+2);
	ele = getub(buf, n+5);
	azm =  getuw(buf, n+6);
	ca_snr = getub(buf, n+8);
	p2_snr = getub(buf, n+10);
        dgps_age = getuw(buf, n+11);
	hw_channel = getub(buf, n+13);
	/*@ -predboolothers +charint @*/
        /* NOTE - In theory, I think one would check for hw channel number to
           see if one is dealing with a GPS or other satellite, but the
           channel numbers reported bear no resemblance to what the spec
           says should be.  So I check for the fact that if all three 
           values below are zero, one is not interested on this satellite */
        if (!(ele == 0 && azm == 0 && dgps_age == 0)) {
            session->gpsdata.PRN[i] = (int)prn;
            session->gpsdata.elevation[i] = (int)ele;
            session->gpsdata.azimuth[i] = (int)azm;
            session->gpsdata.ss[i++] = (p2_snr ? p2_snr : ca_snr) / 4;
        }
        gpsd_report(LOG_IO,
                    "Navcom: prn = %3u, ele = %02u, azm = %03u, snr = %d (%s), "
                    "dgps age = %.1fs, log ch = %d, hw ch = 0x%02x\n",
                    prn, ele, azm, session->gpsdata.ss, (p2_snr?"P2":"C/A"),
                    (double)dgps_age*0.1, log_channel&0x3f, hw_channel);
        gpsd_report(LOG_IO,
                    "Navcom:            sol. valid = %c, clock = %s, pos. = %s, "
                    "height = %s, err. code = 0x%x\n",
                    (sol_status&0x01?'Y':'N'), (sol_status&0x02?"stable":"unstable"),
                    (sol_status&0x04?"dgps":"unaided"), (sol_status&0x08?"solved":"constrained"),
                    (sol_status&0x01?0x00:sol_status&0x0f00>>8));
	/*@ +predboolothers -charint @*/
    }

    return PDOP_SET | SATELLITE_SET | STATUS_SET;
}

/* Raw Meas. Data Block */
static gps_mask_t handle_0xb0(struct gps_device_t *session)
{
    /* L1 wavelength (299792458m/s / 1575420000Hz) */
#define LAMBDA_L1 (.190293672798364880476317426464)
    size_t n;
    unsigned char *buf = session->packet.outbuffer + 3;
    size_t msg_len = (size_t)getuw(buf, 1);
    u_int16_t week = getuw(buf, 3);
    u_int32_t tow = getul(buf, 5);
    u_int8_t tm_slew_acc = getub(buf, 9);
    u_int8_t status = getub(buf, 10);
    
    char time_str[24];
    (void)unix_to_iso8601(gpstime_to_unix((int)week, (double)tow/1000.0), time_str, sizeof(time_str));

    gpsd_report(LOG_PROG,
                "Navcom: received packet type 0xb0 (Raw Meas. Data Block)\n");
    /*@ -predboolothers @*/
    gpsd_report(LOG_IO,
                "Navcom: Epoch = %s, time slew accumulator = %u (1/1023mS), status = 0x%02x "
                "(%sclock %s - %u blocks follow)\n",
                time_str,
                tm_slew_acc, status, (status&0x80?"channel time set - ":""),
                (status&0x40?"stable":"not stable"), status&0x0f);
    /*@ +predboolothers @*/
    for (n=11; n<msg_len-1; n+=16) {
        u_int8_t sv_status = getub(buf, n);
        u_int8_t ch_status = getub(buf, n+1);
        u_int32_t ca_pseudorange = getul(buf, n+2);
	/* integer division by 16 is a sign-preserving right shift of 4 bits */
        int32_t l1_phase = getsl24(buf, n+6) / 16;
        u_int8_t l1_slips = (u_int8_t)(getsl24(buf, n+6) & 0x0f);
        int16_t p1_ca_pseudorange = getsw(buf, n+9);
        int16_t p2_ca_pseudorange = getsw(buf, n+11);
        int32_t l2_phase = getsl24(buf, n+13) / 16;
        u_int8_t l2_slips = (u_int8_t)(getsl24(buf, n+13) & 0x0f);
	/*@ -predboolothers +charint @*/
        double c1 = (sv_status&0x80? (double)ca_pseudorange/16.0*LAMBDA_L1 : NAN);
        double l1 = (sv_status&0x80? (double)ca_pseudorange/16.0 + (double)l1_phase/256.0 : NAN);
        double l2 = (sv_status&0x20? ((double)ca_pseudorange/16.0
                    + (double)p2_ca_pseudorange/16.0)*(120.0/154.0)
                    +(double)l2_phase/256.0 : NAN);
        double p1 = (sv_status&0x40? c1 + (double)p1_ca_pseudorange/16.0*LAMBDA_L1 : NAN);
        double p2 = (sv_status&0x20? c1 + (double)p2_ca_pseudorange/16.0*LAMBDA_L1 : NAN);
        gpsd_report(LOG_IO+1,
                    "Navcom: >> sv status = 0x%02x (PRN %u - C/A & L1 %s - P1 %s - P2 & L2 %s)\n",
                    sv_status, (sv_status&0x1f), (sv_status&0x80?"valid":"invalid"),
                    (sv_status&0x40?"valid":"invalid"), (sv_status&0x20?"valid":"invalid"));
        gpsd_report(LOG_IO+1,
                    "Navcom: >>> ch status = 0x%02x (Logical channel: %u - CA C/No: %u dBHz) "
                    "sL1: %u, sL2: %u\n",
                    ch_status, ch_status&0x0f, ((ch_status&0xf0)>>4)+35, l1_slips, l2_slips);
        gpsd_report(LOG_IO+1,
                    "Navcom: >>> C1: %14.3f, L1: %14.3f, L2: %14.3f, P1: %14.3f, P2: %14.3f\n",
                    c1, l1, l2, p1, p2);
	/*@ +predboolothers -charint @*/
    }
#undef LAMBDA_L1
    return 0; /* Raw measurements not yet implemented in gpsd */
}

/* Pseudorange Noise Statistics */
static gps_mask_t handle_0xb5(struct gps_device_t *session)
{
    if(sizeof(double) == 8) {
        union long_double l_d;
        unsigned char *buf = session->packet.outbuffer + 3;
        u_int16_t week = getuw(buf, 3);
        u_int32_t tow = getul(buf, 5);
        double rms = getd(buf, 9);
#ifdef __UNUSED__
        /* Reason why it's unused is these figures do not agree
        with those obtained from the PVT report (handle_0xb1).
        The figures from 0xb1 do agree with the values reported
        by Navcom's PC utility */
        double ellips_maj = getd(buf, 17);
        double ellips_min = getd(buf, 25);
        double ellips_azm = getd(buf, 33);
        double lat_sd = getd(buf, 41);
        double lon_sd = getd(buf, 49);
        double alt_sd = getd(buf, 57);
        double hrms = sqrt(pow(lat_sd, 2) + pow(lon_sd, 2));