From e19519fa0aa4e5edff8ad008043aee6f14fa317f Mon Sep 17 00:00:00 2001 From: "Gary E. Miller" Date: Tue, 29 Mar 2016 16:26:34 -0700 Subject: Parse Skytraq packets to inbuffer. No packet decoder yet. --- gpsd.h-tail | 1 + packet.c | 84 ++++++++++++++++++++++++++++++++++++++++++++++++++++++--- packet_states.h | 14 +++++----- 3 files changed, 90 insertions(+), 9 deletions(-) diff --git a/gpsd.h-tail b/gpsd.h-tail index b7b0db3d..012ca7f4 100644 --- a/gpsd.h-tail +++ b/gpsd.h-tail @@ -149,6 +149,7 @@ struct gps_lexer_t { #define RTCM3_PACKET 17 #define JSON_PACKET 18 #define PACKET_TYPES 19 /* increment this as necessary */ +#define SKY_PACKET 20 #define TEXTUAL_PACKET_TYPE(n) ((((n)>=NMEA_PACKET) && ((n)<=MAX_TEXTUAL_TYPE)) || (n)==JSON_PACKET) #define GPS_PACKET_TYPE(n) (((n)>=NMEA_PACKET) && ((n)<=MAX_GPSPACKET_TYPE)) #define LOSSLESS_PACKET_TYPE(n) (((n)>=RTCM2_PACKET) && ((n)<=RTCM3_PACKET)) diff --git a/packet.c b/packet.c index bb820642..11be8455 100644 --- a/packet.c +++ b/packet.c @@ -250,12 +250,12 @@ static bool nextstate(struct gps_lexer_t *lexer, unsigned char c) break; } #endif -#ifdef SIRF_ENABLE +#if defined(SIRF_ENABLE) || defined(SKYTRAQ_ENABLE) if (c == 0xa0) { lexer->state = SIRF_LEADER_1; break; } -#endif /* SIRF_ENABLE */ +#endif /* SIRF_ENABLE || SKYTRAQ_ENABLE */ #ifdef SUPERSTAR2_ENABLE if (c == SOH) { lexer->state = SUPERSTAR2_LEADER; @@ -796,13 +796,24 @@ static bool nextstate(struct gps_lexer_t *lexer, unsigned char c) return character_pushback(lexer, GROUND_STATE); break; #endif /* NMEA0183_ENABLE */ -#ifdef SIRF_ENABLE +#if defined(SIRF_ENABLE) || defined(SKYTRAQ_ENABLE) case SIRF_LEADER_1: +# ifdef SIRF_ENABLE + /* SIRF leads with 0xA0,0xA2 */ if (c == 0xa2) lexer->state = SIRF_LEADER_2; else +# endif /* SIRF_ENABLE */ +# ifdef SKYTRAQ_ENABLE + /* Skytraq leads with 0xA0,0xA1 */ + if (c == 0xa1) + lexer->state = SKY_LEADER_2; + else +# endif /* SKYTRAQ_ENABLE */ return character_pushback(lexer, GROUND_STATE); break; +#endif /* SIRF_ENABLE || SKYTRAQ_ENABLE */ +#ifdef SIRF_ENABLE case SIRF_LEADER_2: lexer->length = (size_t) (c << 8); lexer->state = SIRF_LENGTH_1; @@ -837,6 +848,66 @@ static bool nextstate(struct gps_lexer_t *lexer, unsigned char c) return character_pushback(lexer, GROUND_STATE); break; #endif /* SIRF_ENABLE */ +#ifdef SKYTRAQ_ENABLE + case SKY_LEADER_2: + /* MSB of length is first */ + lexer->length = (size_t) (c << 8); + lexer->state = SKY_LENGTH_1; + break; + case SKY_LENGTH_1: + /* Skytraq length can be any 16 bit number, except 0 */ + lexer->length += c; + if ( 0 == lexer->length ) + return character_pushback(lexer, GROUND_STATE); + if (lexer->length > MAX_PACKET_LENGTH) + return character_pushback(lexer, GROUND_STATE); + lexer->state = SKY_PAYLOAD; + break; + case SKY_PAYLOAD: + if ( 00 == --lexer->length) + lexer->state = SKY_DELIVERED; + break; + case SKY_DELIVERED: + if ( lexer->errout.debug >= LOG_RAW+1) { + char scratchbuf[MAX_PACKET_LENGTH*2+1]; + gpsd_log(&lexer->errout, LOG_RAW+1, + "Skytraq = %s\n", + gpsd_packetdump(scratchbuf, sizeof(scratchbuf), + (char *)lexer->inbuffer, + lexer->inbufptr - (unsigned char *)lexer->inbuffer)); + } + { + unsigned char csum = 0; + for (n = 4; + (unsigned char *)(lexer->inbuffer + n) < lexer->inbufptr - 1; + n++) + csum ^= lexer->inbuffer[n]; + if (csum != c) { + gpsd_log(&lexer->errout, LOG_IO, + "Skytraq bad checksum 0x%hhx, expecting 0x%x\n", + csum, c); + lexer->state = GROUND_STATE; + break; + } + } + lexer->state = SKY_CSUM; + break; + case SKY_CSUM: + if ( 0x0d != c) + return character_pushback(lexer, GROUND_STATE); + lexer->state = SKY_TRAILER_1; + break; + case SKY_TRAILER_1: + if ( 0x0a != c) + return character_pushback(lexer, GROUND_STATE); + lexer->state = SKY_RECOGNIZED; + break; + case SKY_RECOGNIZED: + if ( 0xa0 != c) + return character_pushback(lexer, GROUND_STATE); + lexer->state = SIRF_LEADER_1; + break; +#endif /* SKYTRAQ */ #ifdef SUPERSTAR2_ENABLE case SUPERSTAR2_LEADER: ctmp = c; @@ -1676,6 +1747,13 @@ void packet_parse(struct gps_lexer_t *lexer) break; } #endif /* SIRF_ENABLE */ +#ifdef SKYTRAQ_ENABLE + else if (lexer->state == SKY_RECOGNIZED) { + // packet_accept(lexer, SKY_PACKET); + packet_discard(lexer); + break; + } +#endif /* SKYTRAQ_ENABLE */ #ifdef SUPERSTAR2_ENABLE else if (lexer->state == SUPERSTAR2_RECOGNIZED) { unsigned a = 0, b; diff --git a/packet_states.h b/packet_states.h index 2254f412..2f54d5a8 100644 --- a/packet_states.h +++ b/packet_states.h @@ -57,8 +57,10 @@ EARTHA_5, /* EARTHA leader H */ #endif /* EARTHMATE_ENABLE */ +#if defined(SIRF_ENABLE) || defined(SKYTRAQ_ENABLE) + SIRF_LEADER_1, /* seen first character of SiRF/Skytraq leader <0x0A> */ +#endif /* SIRF_ENABLE || SKYTRAQ_ENABLE */ #ifdef SIRF_ENABLE - SIRF_LEADER_1, /* we've seen first character of SiRF leader */ SIRF_LEADER_2, /* seen second character of SiRF leader */ SIRF_LENGTH_1, /* seen first byte of SiRF length */ SIRF_PAYLOAD, /* we're in a SiRF payload part */ @@ -69,15 +71,15 @@ #ifdef SKYTRAQ_ENABLE /* <0xA0,0xA1><0x0D,0x0A> */ - /* Len is of Message ID and Message Body */ - SKY_EXPECTED, /* expecting Skytraq packet */ - SKY_LEADER_1, /* saw leading 0xA0 */ - SKY_LEADER_2, /* saw leading 0xA1 */ + /* Len is two bytes, is the length of Message ID and Message Body */ + /* Skytraq leader 1 same as SIRF_LEADER_1 */ + SKY_LEADER_2, /* saw leader 2 <0xA1> */ SKY_LENGTH_1, /* saw first byte of packet length */ SKY_LENGTH_2, /* saw second byte of packet length */ - SKY_MID, /* saw message one byte ID */ SKY_PAYLOAD, /* we're in a Skytraq payload */ + SKY_DELIVERED, /* saw last byte of Skytraq payload */ SKY_CSUM, /* saw Skytraq checksum */ + SKY_TRAILER_1, /* saw first byte of Skytraq trailer <0x0D> */ SKY_RECOGNIZED, /* found end of the Skytraq packet */ #endif /* SKYTRAQ_ENABLE */ -- cgit v1.2.1