diff options
| -rw-r--r-- | driver_tsip.c | 125 | ||||
| -rw-r--r-- | packet.c | 40 |
2 files changed, 153 insertions, 12 deletions
diff --git a/driver_tsip.c b/driver_tsip.c index 8685297f..9271c476 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,114 @@ 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; case 0x6d: /* All-In-View Satellite Selection */ u1 = getub(buf, 0); /* nsvs/dimension */ count = (int)((u1 >> 4) & 0x0f); @@ -797,6 +903,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); @@ -1009,7 +1124,7 @@ static void tsip_event_hook(struct gps_device_t *session, event_t event) return; if (event == event_identified) { unsigned char buf[100]; - + /* * Set basic configuration, in case no hardware config response * comes back. @@ -1148,14 +1263,14 @@ void configuration_packets_generic(struct gps_device_t *session) /* configure generic Trimble TSIP device to a known state */ { unsigned char buf[100]; - + /* I/O Options */ putbyte(buf, 0, 0x1e); /* Position: DP, MSL, LLA */ putbyte(buf, 1, 0x02); /* Velocity: ENU */ putbyte(buf, 2, 0x00); /* Time: GPS */ putbyte(buf, 3, 0x08); /* Aux: dBHz */ (void)tsip_write(session, 0x35, buf, 4); - + /* Request Software Versions */ (void)tsip_write(session, 0x1f, NULL, 0); /* Request Current Time */ @@ -1889,7 +1889,7 @@ void packet_parse(struct gps_lexer_t *lexer) * 0x43, Velocity Fix XYZ, ECEF, data length 20 * 0x45, Software Version Information, data length 10 * 0x46, Health of Receiver, data length 2 - * 0x47, Signal Levels for all sats + * 0x47, Signal Level all Sats Tracked, data length 1+5*numSV * 0x48, GPS System Messages, data length 22 * 0x49, Almanac Health Page, data length 32 * 0x4a, Signle Precision Fix LLA, data length 20 @@ -1907,10 +1907,12 @@ 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, Severe Failure Notification * 0x5F-01-0B: Reset Error Codes * 0x5F-02: Ascii text message + * 0x6c, Satellite Selection List, data length 18+numSV * 0x6d, All-In-View Satellite Selection, data length 17+numSV * 0x6f, Synced Measurement Packet * 0x72, PV filter parameters @@ -1949,10 +1951,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); @@ -1977,6 +1985,12 @@ void packet_parse(struct gps_lexer_t *lexer) /* pass */ ; else if (TSIP_ID_AND_LENGTH(0x46, 2)) /* pass */ ; + else if ((0x47 == pkt_id) && ((packetlen % 5) == 0)) + /* + * 0x47 data length 1+5*numSV, packetlen is 5+5*numSV + * FIXME, should be a proper length calculation + */ + /* pass */ ; else if (TSIP_ID_AND_LENGTH(0x48, 22)) /* pass */ ; else if (TSIP_ID_AND_LENGTH(0x49, 32)) @@ -2001,6 +2015,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 */ ; /* @@ -2008,10 +2024,20 @@ void packet_parse(struct gps_lexer_t *lexer) * but we test for it so as to avoid setting packet not_tsip */ else if (TSIP_ID_AND_LENGTH(0x5f, 66)) + /* + * 0x6c data length 18+numSV, total packetlen is 22+numSV + * numSV up to 224 + */ + /* pass */ ; + else if ((0x6c == pkt_id) + && ((22 <= packetlen) && (246 >= packetlen))) + /* + * 0x6d data length 17+numSV, total packetlen is 21+numSV + * numSV up to 32 + */ /* pass */ ; - /* 0x6d is variable length depending on the sat picture */ else if ((0x6d == pkt_id) - && ((0x14 <= packetlen) && (0x20 >= packetlen))) + && ((21 <= packetlen) && (53 >= packetlen))) /* pass */ ; else if (TSIP_ID_AND_LENGTH(0x82, 1)) /* pass */ ; @@ -2020,7 +2046,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]. |