From 5ffe5fe11ab797766b7933a680ae4503cb46589f Mon Sep 17 00:00:00 2001 From: Dieter Spaar Date: Wed, 18 Jul 2012 22:18:16 +0200 Subject: [PATCH] ubx-parse.c: Add more debugging code, and make it optional --- ubx-parse.c | 63 +++++++++++++++++++++++++++++++++++++++++++++++------ 1 file changed, 56 insertions(+), 7 deletions(-) diff --git a/ubx-parse.c b/ubx-parse.c index c3d0f70..9389e1f 100644 --- a/ubx-parse.c +++ b/ubx-parse.c @@ -27,6 +27,19 @@ #include "ubx.h" #include "ubx-parse.h" +#define DEBUG 1 +#if DEBUG + #define printd(x, args ...) printf(x, ## args) +#else + #define printd(x, args ...) +#endif + +#define DEBUG1 0 +#if DEBUG1 + #define printd1(x, args ...) printf(x, ## args) +#else + #define printd1(x, args ...) +#endif /* Helpers */ @@ -67,13 +80,18 @@ _ubx_msg_parse_nav_posllh(struct ubx_hdr *hdr, void *pl, int pl_len, void *ud) struct ubx_nav_posllh *nav_posllh = pl; struct gps_assist_data *gps = ud; - //printf("[.] NAV_POSLLH\n"); + printd("[.] NAV_POSLLH\n"); gps->fields |= GPS_FIELD_REFPOS; gps->ref_pos.latitude = (double)(nav_posllh->lat) * 1e-7; gps->ref_pos.longitude = (double)(nav_posllh->lon) * 1e-7; gps->ref_pos.altitude = (double)(nav_posllh->height) * 1e-3; + + printd(" TOW %lu\n", nav_posllh->itow); + printd(" latitude %f\n", gps->ref_pos.latitude); + printd(" longitude %f\n", gps->ref_pos.longitude); + printd(" altitude %f\n", gps->ref_pos.altitude); } static void @@ -82,14 +100,22 @@ _ubx_msg_parse_aid_ini(struct ubx_hdr *hdr, void *pl, int pl_len, void *ud) struct ubx_aid_ini *aid_ini = pl; struct gps_assist_data *gps = ud; - //printf("[.] AID_INI\n"); + printd("[.] AID_INI\n"); /* Extract info for "Reference Time" */ gps->fields |= GPS_FIELD_REFTIME; gps->ref_time.wn = aid_ini->wn; gps->ref_time.tow = (double)aid_ini->tow * 1e-3; - + gps->ref_time.when = time(NULL); + + printd(" WN %d\n", gps->ref_time.wn); + printd(" TOW %ld\n", aid_ini->tow); + + if((aid_ini->flags & 0x03) != 0x03) { /* time and pos valid ? */ + fprintf(stderr, "Postion and/or time not valid (0x%lx)", aid_ini->flags); + } + // FIXME: We could extract ref position as well but we need it in // WGS84 geodetic coordinates and it's provided as ecef, so // we need a lot of math ... @@ -101,11 +127,13 @@ _ubx_msg_parse_aid_hui(struct ubx_hdr *hdr, void *pl, int pl_len, void *ud) struct ubx_aid_hui *aid_hui = pl; struct gps_assist_data *gps = ud; - //printf("[.] AID_HUI\n"); + printd("[.] AID_HUI\n"); if (aid_hui->flags & 0x2) { /* UTC parameters valid */ struct gps_utc_model *utc = &gps->utc; + printd(" UTC\n"); + gps->fields |= GPS_FIELD_UTC; utc->a0 = double_to_fixedpoint(aid_hui->utc_a0, -30); @@ -121,6 +149,8 @@ _ubx_msg_parse_aid_hui(struct ubx_hdr *hdr, void *pl, int pl_len, void *ud) if (aid_hui->flags & 0x04) { /* Klobuchar parameters valid */ struct gps_ionosphere_model *iono = &gps->ionosphere; + printd(" IONOSPHERE\n"); + gps->fields |= GPS_FIELD_IONOSPHERE; iono->alpha_0 = float_to_fixedpoint(aid_hui->klob_a0, -30); @@ -140,12 +170,23 @@ _ubx_msg_parse_aid_alm(struct ubx_hdr *hdr, void *pl, int pl_len, void *ud) struct ubx_aid_alm *aid_alm = pl; struct gps_assist_data *gps = ud; - //printf("[.] AID_ALM %d - %d\n", aid_alm->sv_id, aid_alm->gps_week); + if(pl_len == 8) /* length if not available */ + return; + + if(pl_len != sizeof(struct ubx_aid_alm)) { + fprintf(stderr, "pl_len != sizeof(struct ubx_aid_alm) (%d)\n", pl_len); + return; + } + + printd("[.] AID_ALM %2ld - %ld (nsv = %d)\n", aid_alm->sv_id, aid_alm->gps_week, gps->almanac.n_sv); if (aid_alm->gps_week) { + int i = gps->almanac.n_sv++; gps->fields |= GPS_FIELD_ALMANAC; gps->almanac.wna = aid_alm->gps_week & 0xff; - gps_unpack_sf45_almanac(aid_alm->alm_words, &gps->almanac.svs[gps->almanac.n_sv++]); + gps_unpack_sf45_almanac(aid_alm->alm_words, &gps->almanac.svs[i]); + /* set satellite ID this way, otherwise it will be wrong */ + gps->almanac.svs[i].sv_id = aid_alm->sv_id; } } @@ -155,7 +196,15 @@ _ubx_msg_parse_aid_eph(struct ubx_hdr *hdr, void *pl, int pl_len, void *ud) struct ubx_aid_eph *aid_eph = pl; struct gps_assist_data *gps = ud; - //printf("[.] AID_EPH %d - %s\n", aid_eph->sv_id, aid_eph->present ? "present" : "not present"); + if(pl_len == 8) /* length if not available */ + return; + + if(pl_len != sizeof(struct ubx_aid_eph)) { + fprintf(stderr, "pl_len != sizeof(struct ubx_aid_eph) (%d)\n", pl_len); + return; + } + + printd("[.] AID_EPH %2ld - %s (nsv = %d)\n", aid_eph->sv_id, aid_eph->present ? "present" : "", gps->ephemeris.n_sv); if (aid_eph->present) { int i = gps->ephemeris.n_sv++;