From 0e890f9549b60bcfd08dfb0288c258ecccf545f7 Mon Sep 17 00:00:00 2001 From: Nuno Goncalves Date: Mon, 20 Jun 2016 15:47:12 +0100 Subject: [PATCH 3/3] TSIP: support multi-constelation chip packets Signed-off-by: Nuno Goncalves --- driver_tsip.c | 116 +++++++++++++++++++++++++++++++++++++++++++++++++++++++++- packet.c | 32 ++++++++++++---- 2 files changed, 139 insertions(+), 9 deletions(-) diff --git a/driver_tsip.c b/driver_tsip.c index f397271..293f62b 100644 --- a/driver_tsip.c +++ b/driver_tsip.c @@ -34,7 +34,7 @@ void configuration_packets_accutime_gold(struct gps_device_t *session); void configuration_packets_generic(struct gps_device_t *session); #ifdef TSIP_ENABLE -#define TSIP_CHANNELS 12 +#define TSIP_CHANNELS 15 static int tsip_write(struct gps_device_t *session, unsigned int id, unsigned char *buf, size_t len) @@ -120,7 +120,7 @@ static gps_mask_t tsip_parse_input(struct gps_device_t *session) int i, j, len, count; gps_mask_t mask = 0; unsigned int id; - uint8_t u1, u2, u3, u4, u5; + uint8_t u1, u2, u3, u4, u5, u6, u7, u8, u9, u10; int16_t s1, s2, s3, s4; int32_t sl1, sl2, sl3; uint32_t ul1, ul2; @@ -513,8 +513,111 @@ static gps_mask_t tsip_parse_input(struct gps_device_t *session) session->gpsdata.satellites_visible = i; } break; + case 0x5d: /* GNSS Satellite Tracking Status (multi-GNSS operation) */ + if (len != 26) + break; + u1 = getub(buf, 0); /* PRN */ + u2 = getub(buf, 1); /* chan */ + u3 = getub(buf, 2); /* Acquisition flag */ + u4 = getub(buf, 3); /* SV used in Position or Time calculation*/ + f1 = getbef32((char *)buf, 4); /* Signal level */ + f2 = getbef32((char *)buf, 8); /* time of Last measurement */ + d1 = getbef32((char *)buf, 12) * RAD_2_DEG; /* Elevation */ + d2 = getbef32((char *)buf, 16) * RAD_2_DEG; /* Azimuth */ + u5 = getub(buf, 20); /* old measurement flag */ + u6 = getub(buf, 21); /* integer msec flag */ + u7 = getub(buf, 22); /* bad data flag */ + u8 = getub(buf, 23); /* data collection flag */ + u9 = getub(buf, 24); /* Used flags */ + u10 = getub(buf, 25); /* SV Type */ + + i = u2; /* channel number */ + gpsd_log(&session->context->errout, LOG_INF, + "Satellite Tracking Status: Ch %2d Con %d PRN %3d Acq %d Use %d SNR %4.1f LMT %.04f El %4.1f Az %5.1f Old %d Int %d Bad %d Col %d TPF %d SVT %d\n", + i, u10, u1, u3, u4, f1, f2, d1, d2, u5, u6, u7, u8, u9, u10); + if (i < TSIP_CHANNELS) { + if (d1 >= 0.0) { + session->gpsdata.skyview[i].PRN = (short)u1; + session->gpsdata.skyview[i].ss = (double)f1; + session->gpsdata.skyview[i].elevation = (short)round(d1); + session->gpsdata.skyview[i].azimuth = (short)round(d2); + session->gpsdata.skyview[i].used = (bool)u4; + } else { + session->gpsdata.skyview[i].PRN = (short)u1; + session->gpsdata.skyview[i].elevation = + session->gpsdata.skyview[i].azimuth = 0; + session->gpsdata.skyview[i].ss = 0.0; + session->gpsdata.skyview[i].used = false; + } + if (++i == session->gpsdata.satellites_visible) { + session->gpsdata.skyview_time = NAN; + mask |= SATELLITE_SET; /* last of the series */ + } + if (i > session->gpsdata.satellites_visible) + session->gpsdata.satellites_visible = i; + } + break; case 0x5e: /* Additional Fix Status Report */ break; + case 0x6c: /* Satellite Selection List */ + u1 = getub(buf, 0); /* nsvs/dimension */ + count = (int)getub(buf, 17); + if (len != (18 + count)) + break; + session->driver.tsip.last_6d = now; /* keep timestamp for request */ +#ifdef __UNUSED__ + /* + * This looks right, but it sets a spurious mode value when + * the satellite constellation looks good to the chip but no + * actual fix has yet been acquired. We should set the mode + * field (which controls gpsd's fix reporting) only from sentences + * that convey actual fix information, like 0x20, otherwise we + * get results like triggering their error modeler spuriously. + */ + switch (u1 & 7) { /* dimension */ + case 3: + //session->gpsdata.status = STATUS_FIX; + session->newdata.mode = MODE_2D; + break; + case 4: + //session->gpsdata.status = STATUS_FIX; + session->newdata.mode = MODE_3D; + break; + default: + //session->gpsdata.status = STATUS_NO_FIX; + session->newdata.mode = MODE_NO_FIX; + break; + } + mask |= MODE_SET; +#endif /* __UNUSED__ */ + session->gpsdata.satellites_used = count; + session->gpsdata.dop.pdop = getbef32((char *)buf, 1); + session->gpsdata.dop.hdop = getbef32((char *)buf, 5); + session->gpsdata.dop.vdop = getbef32((char *)buf, 9); + session->gpsdata.dop.tdop = getbef32((char *)buf, 13); + session->gpsdata.dop.gdop = + sqrt(pow(session->gpsdata.dop.pdop, 2) + + pow(session->gpsdata.dop.tdop, 2)); + + memset(session->driver.tsip.sats_used, 0, sizeof(session->driver.tsip.sats_used)); + buf2[0] = '\0'; + for (i = 0; i < count; i++) + str_appendf(buf2, sizeof(buf2), + " %d", session->driver.tsip.sats_used[i] = + (short)getub(buf, 18 + i)); + gpsd_log(&session->context->errout, LOG_DATA, + "AIVSS: 0x6d status=%d used=%d " + "pdop=%.1f hdop=%.1f vdop=%.1f tdop=%.1f gdup=%.1f\n", + session->gpsdata.status, + session->gpsdata.satellites_used, + session->gpsdata.dop.pdop, + session->gpsdata.dop.hdop, + session->gpsdata.dop.vdop, + session->gpsdata.dop.tdop, + session->gpsdata.dop.gdop); + mask |= DOP_SET | STATUS_SET | USED_IS; + break; + break; case 0x6d: /* All-In-View Satellite Selection */ u1 = getub(buf, 0); /* nsvs/dimension */ count = (int)((u1 >> 4) & 0x0f); @@ -797,6 +900,15 @@ static gps_mask_t tsip_parse_input(struct gps_device_t *session) session->newdata.mode, session->gpsdata.status); break; + case 0x4a: /* Set PPS Characteristics */ + break; + + case 0x4e: /* PPS Output */ + break; + + case 0xa2: /* UTC/GPS Timing */ + break; + case 0xab: /* Thunderbolt Timing Superpacket */ if (len != 17) { gpsd_log(&session->context->errout, 4, "pkt 0xab len=%d\n", len); diff --git a/packet.c b/packet.c index 4ab0b5e..0ab4bff 100644 --- a/packet.c +++ b/packet.c @@ -1888,6 +1888,7 @@ void packet_parse(struct gps_lexer_t *lexer) * 0x43, Velocity Fix, data length 20 * 0x45, Software Version Information, data length 10 * 0x46, Health of Receiver, data length 2 + * 0x47, Signal Level for All Satellites Tracked, data length 1+5*numSV * 0x48, GPS System Messages, data length 22 * 0x49, Almanac Health Page, data length 32 * 0x4a, LLA Position, data length 20 @@ -1900,8 +1901,10 @@ void packet_parse(struct gps_lexer_t *lexer) * 0x5a, Raw Measurements * 0x5b, Satellite Ephemeris Status, data length 16 * 0x5c, Satellite Tracking Status, data length 24 + * 0x5d, Satellite Tracking Status (multi-gnss), data length 26 * 0x5e, Additional Fix Status Report * 0x5f, 0x5F-01-0B: Reset Error Codes, data length 25 - not according to the length check below! + * 0x6c, Satellite Selection List, data length 18+numSV * 0x6d, All-In-View Satellite Selection, data length 17+numSV * 0x82, Differential Position Fix Mode, data length 1 * 0x83, Double Precision XYZ, data length 36 @@ -1924,10 +1927,16 @@ void packet_parse(struct gps_lexer_t *lexer) /* *INDENT-OFF* */ if (!((0x13 == pkt_id) || (0x1c == pkt_id) || + (0x38 == pkt_id) || + ((0x41 <= pkt_id) && (0x4c >= pkt_id)) || + ((0x54 <= pkt_id) && (0x57 >= pkt_id)) || + ((0x5a <= pkt_id) && (0x5f >= pkt_id)) || + (0x6c == pkt_id) || + (0x6d == pkt_id) || + ((0x82 <= pkt_id) && (0x84 >= pkt_id)) || + (0x8f == pkt_id) || (0xbb == pkt_id) || - (0xbc == pkt_id) || - (0x38 == pkt_id)) - && ((0x41 > pkt_id) || (0x8f < pkt_id))) { + (0xbc == pkt_id))) { gpsd_log(&lexer->errout, LOG_IO, "Packet ID 0x%02x out of range for TSIP\n", pkt_id); @@ -1952,6 +1961,9 @@ void packet_parse(struct gps_lexer_t *lexer) /* pass */ ; else if (TSIP_ID_AND_LENGTH(0x46, 2)) /* pass */ ; + /* 0x47 data length 1+5*numSV, total packetlen is 5+5*numSV */ + else if ((0x47 == pkt_id) && ((packetlen % 5) == 0)) + /* pass */ ; else if (TSIP_ID_AND_LENGTH(0x48, 22)) /* pass */ ; else if (TSIP_ID_AND_LENGTH(0x49, 32)) @@ -1962,7 +1974,7 @@ void packet_parse(struct gps_lexer_t *lexer) /* pass */ ; else if (TSIP_ID_AND_LENGTH(0x4c, 17)) /* pass */ ; - else if (TSIP_ID_AND_LENGTH(0x54, 12)) + else if (TSIP_ID_AND_LENGTH(0x54, 12)) //length 12 is not according to the packet list above /* pass */ ; else if (TSIP_ID_AND_LENGTH(0x55, 4)) /* pass */ ; @@ -1976,6 +1988,8 @@ void packet_parse(struct gps_lexer_t *lexer) /* pass */ ; else if (TSIP_ID_AND_LENGTH(0x5c, 24)) /* pass */ ; + else if (TSIP_ID_AND_LENGTH(0x5d, 26)) + /* pass */ ; else if (TSIP_ID_AND_LENGTH(0x5e, 2)) /* pass */ ; /* @@ -1984,9 +1998,13 @@ void packet_parse(struct gps_lexer_t *lexer) */ else if (TSIP_ID_AND_LENGTH(0x5f, 66)) /* pass */ ; - /* 0x6d is variable length depending on the sat picture */ + /* 0x6c data length 18+numSV, total packetlen is 22+numSV, numSV up to 224 */ + else if ((0x6c == pkt_id) + && ((22 <= packetlen) && (246 >= packetlen))) + /* pass */ ; + /* 0x6d data length 17+numSV, total packetlen is 21+numSV, numSV up to 32 */ else if ((0x6d == pkt_id) - && ((0x14 <= packetlen) && (0x20 >= packetlen))) + && ((21 <= packetlen) && (53 >= packetlen))) /* pass */ ; else if (TSIP_ID_AND_LENGTH(0x82, 1)) /* pass */ ; @@ -1995,7 +2013,7 @@ void packet_parse(struct gps_lexer_t *lexer) else if (TSIP_ID_AND_LENGTH(0x84, 36)) /* pass */ ; /* super packets, variable length */ - else if ((0x8e == pkt_id) || (0x8f == pkt_id)) + else if (0x8f == pkt_id) /* pass */ ; /* * This is according to [TSIP]. -- 2.7.4