diff options
Diffstat (limited to 'tsip.c')
| -rw-r--r-- | tsip.c | 105 |
1 files changed, 55 insertions, 50 deletions
@@ -22,14 +22,14 @@ union long_double { double d; }; -#define putb(off,b) { buf[off] = (unsigned char)(b); } -#define putw(off,w) { putb(off,(w) >> 8); putb(off+1,w); } -#define putl(off,l) { putw(off,(l) >> 16); putw(off+2,l); } - -#define getb(off) (buf[off]) -#define getw(off) ((short)((getb(off) << 8) | getb(off+1))) -#define getl(off) ((int)((getb(off) << 24) | (getb(off+1) << 16) \ - | (getb(off+3) << 8) | getb(off+4))) +#define putbyte(off,b) { buf[off] = (unsigned char)(b); } +#define putword(off,w) { putbyte(off,(w) >> 8); putbyte(off+1,w); } +#define putlong(off,l) { putword(off,(l) >> 16); putword(off+2,l); } + +#define getbyte(off) (buf[off]) +#define getword(off) ((short)((getbyte(off) << 8) | getbyte(off+1))) +#define getl(off) ((int)((getbyte(off) << 24) | (getbyte(off+1) << 16) \ + | (getbyte(off+3) << 8) | getbyte(off+4))) #define getL(off) ((long long)(((long long)getl(off) << 32) | ((long long)getl(off+4) & 0xffffffffL))) #define getf(off) (i_f.i = getl(off), i_f.f) #define getd(off) (l_d.l = getL(off), l_d.d) @@ -37,16 +37,18 @@ union long_double { static int tsip_write(int fd, unsigned int id, unsigned char *buf, int len) { int i; - unsigned char buf2[BUFSIZ]; + char buf2[BUFSIZ]; buf2[0] = '\0'; for (i = 0; i < len; i++) - (void)snprintf(buf2+strlen(buf2), sizeof(buf)-strlen(buf), - "%02x", buf[i]); + (void)snprintf(buf2+strlen(buf2), + sizeof(buf2)-strlen(buf2), + "%02x", (unsigned)buf[i]); gpsd_report(5, "Sent TSIP packet id 0x%02x: %s\n",id,buf2); + /*@ +charint @*/ buf2[0] = '\x10'; - buf2[1] = (unsigned char)id; + buf2[1] = (char)id; if (write(fd,buf2,2) != 2) return -1; @@ -60,6 +62,7 @@ static int tsip_write(int fd, unsigned int id, unsigned char *buf, int len) } buf2[1] = '\x03'; + /*@ -charint @*/ if (write(fd,buf2,2) != 2) return -1; @@ -74,10 +77,10 @@ static void tsip_initializer(struct gps_device_t *session) gpsd_set_speed(session, session->gpsdata.baudrate, 'O', 1); /* I/O Options */ - putb(0,0x1e); /* Position: DP, MSL, LLA */ - putb(1,0x02); /* Velocity: ENU */ - putb(2,0x00); /* Time: GPS */ - putb(3,0x08); /* Aux: dBHz */ + putbyte(0,0x1e); /* Position: DP, MSL, LLA */ + putbyte(1,0x02); /* Velocity: ENU */ + putbyte(2,0x00); /* Time: GPS */ + putbyte(3,0x08); /* Aux: dBHz */ (void)tsip_write(session->gpsdata.gps_fd, 0x35, buf, 4); /* Request Software Versions */ @@ -94,16 +97,16 @@ static bool tsip_speed_switch(struct gps_device_t *session, unsigned int speed) { unsigned char buf[100]; - putb(0,0xff); /* current port */ - putb(1,(round(log(speed/300)/M_LN2))+2); /* input baudrate */ - putb(2,buf[1]); /* output baudrate */ - putb(3,8); /* character width (8 bits) */ - putb(4,1); /* parity (odd) */ - putb(5,0); /* stop bits (1 stopbit) */ - putb(6,0); /* flow control (none) */ - putb(7,0x02); /* input protocol (TSIP) */ - putb(8,0x02); /* input protocol (TSIP) */ - putb(9,0); /* reserved */ + putbyte(0,0xff); /* current port */ + putbyte(1,(round(log((double)speed/300)/M_LN2))+2); /* input baudrate */ + putbyte(2,buf[1]); /* output baudrate */ + putbyte(3,8); /* character width (8 bits) */ + putbyte(4,1); /* parity (odd) */ + putbyte(5,0); /* stop bits (1 stopbit) */ + putbyte(6,0); /* flow control (none) */ + putbyte(7,0x02); /* input protocol (TSIP) */ + putbyte(8,0x02); /* input protocol (TSIP) */ + putbyte(9,0); /* reserved */ (void)tsip_write(session->gpsdata.gps_fd, 0xbc, buf, 10); return true; /* it would be nice to error-check this */ @@ -127,13 +130,14 @@ static int tsip_analyze(struct gps_device_t *session) return 0; } + /*@ +charint @*/ if (session->outbuflen < 4 || session->outbuffer[0] != 0x10) return 0; /* remove DLE stuffing and put data part of message in buf */ buf2[0] = '\0'; - for (i = 2; i < session->outbuflen; i++) { + for (i = 2; i < (int)session->outbuflen; i++) { if (session->outbuffer[i] == 0x10) if (session->outbuffer[++i] == 0x03) break; @@ -142,6 +146,7 @@ static int tsip_analyze(struct gps_device_t *session) sizeof(buf)-strlen(buf), "%02x", buf[len++] = session->outbuffer[i]); } + /*@ -charint @*/ (void)snprintf(session->gpsdata.tag, sizeof(session->gpsdata.tag), "ID%02x", id = session->outbuffer[1]); @@ -151,13 +156,13 @@ static int tsip_analyze(struct gps_device_t *session) switch (id) { case 0x13: /* Packet Received */ gpsd_report(4, "Received packet of type %02x cannot be parsed\n", - getb(0)); + getbyte(0)); break; case 0x41: /* GPS Time */ if (len != 10) break; f1 = getf(0); /* gpstime */ - s1 = getw(4); /* week */ + s1 = getword(4); /* week */ f2 = getf(6); /* leap seconds */ if (f2 > 10.0) { session->gps_week = s1; @@ -167,7 +172,7 @@ static int tsip_analyze(struct gps_device_t *session) session->gpsdata.sentence_time = gpstime_to_unix(s1, f1) - f2; #ifdef NTPSHM_ENABLE - ntpshm_put(session, session->gpsdata.sentence_time + 0.075); + (void)ntpshm_put(session, session->gpsdata.sentence_time + 0.075); #endif mask |= TIME_SET; } @@ -195,23 +200,23 @@ static int tsip_analyze(struct gps_device_t *session) if (len != 10) break; gpsd_report(4, "Software versions %d.%d %02d%02d%02d %d.%d %02d%02d%02d\n", - (int)getb(0),(int)getb(1),(int)getb(4),(int)getb(2),(int)getb(3), - (int)getb(5),(int)getb(6),(int)getb(9),(int)getb(7),(int)getb(8)); + (int)getbyte(0),(int)getbyte(1),(int)getbyte(4),(int)getbyte(2),(int)getbyte(3), + (int)getbyte(5),(int)getbyte(6),(int)getbyte(9),(int)getbyte(7),(int)getbyte(8)); break; case 0x46: /* Health of Receiver */ if (len != 2) break; - gpsd_report(4, "Receiver health %02x %02x\n",getb(0),getb(1)); + gpsd_report(4, "Receiver health %02x %02x\n",getbyte(0),getbyte(1)); break; case 0x47: /* Signal Levels for all Satellites */ - s1 = getb(0); /* count */ + s1 = getbyte(0); /* count */ if (len != (5*s1 + 1)) break; gpsd_zero_satellites(&session->gpsdata); session->gpsdata.satellites = s1; buf2[0] = '\0'; for (i = 0; i < s1; i++) { - session->gpsdata.PRN[i] = s2 = getb(5*i + 1); + session->gpsdata.PRN[i] = s2 = getbyte(5*i + 1); session->gpsdata.ss[i] = f1 = getf(5*i + 2); (void)snprintf(buf2+strlen(buf2), sizeof(buf2)-strlen(buf2), " %d=%.1f",s2,f1); @@ -243,12 +248,12 @@ static int tsip_analyze(struct gps_device_t *session) case 0x4b: /* Machine/Code ID and Additional Status */ if (len != 3) break; - gpsd_report(4, "Machine ID %02x %02x %02x\n",getb(0),getb(1),getb(2)); + gpsd_report(4, "Machine ID %02x %02x %02x\n",getbyte(0),getbyte(1),getbyte(2)); break; case 0x55: /* IO Options */ if (len != 4) break; - gpsd_report(4, "IO Options %02x %02x %02x %02x\n",getb(0),getb(1),getb(2),getb(4)); + gpsd_report(4, "IO Options %02x %02x %02x %02x\n",getbyte(0),getbyte(1),getbyte(2),getbyte(4)); break; case 0x56: /* Velocity Fix, East-North-Up (ENU) */ if (len != 20) @@ -271,10 +276,10 @@ static int tsip_analyze(struct gps_device_t *session) if (len != 8) break; f1 = getf(2); /* gps_time */ - s1 = getw(6); /* gps_weeks */ - if (getb(0) != 0) /* good current fix? */ + s1 = getword(6); /* gps_weeks */ + if (getbyte(0) != 0) /* good current fix? */ session->gps_week = s1; - gpsd_report(4, "Fix info %02x %02x %d %f\n",getb(0),getb(1),s1,f1); + gpsd_report(4, "Fix info %02x %02x %d %f\n",getbyte(0),getbyte(1),s1,f1); break; case 0x58: /* Satellite System Data/Acknowledge from Receiver */ break; @@ -285,7 +290,7 @@ static int tsip_analyze(struct gps_device_t *session) case 0x5c: /* Satellite Tracking Status */ break; case 0x6d: /* All-In-View Satellite Selection */ - s1 = getb(0); + s1 = getbyte(0); switch (s1 & 7) /* dimension */ { case 3: @@ -307,7 +312,7 @@ static int tsip_analyze(struct gps_device_t *session) memset(session->gpsdata.used,0,sizeof(session->gpsdata.used)); for (i = 0; i < session->gpsdata.satellites_used; i++) - session->gpsdata.used[i] = getb(16 + i); + session->gpsdata.used[i] = getbyte(16 + i); gpsd_report(4, "Sat info: %d %d\n",session->gpsdata.fix.mode,session->gpsdata.satellites_used); gpsd_binary_quality_dump(session, buf2, sizeof(buf2)); @@ -317,10 +322,10 @@ static int tsip_analyze(struct gps_device_t *session) case 0x6e: /* Synchronized Measurements */ break; case 0x6f: /* Synchronized Measurements Report */ - if (len < 20 || getb(0) != 1 || getb(1) != 2) + if (len < 20 || getbyte(0) != 1 || getbyte(1) != 2) break; - s1 = getw(2); /* number of bytes */ - s2 = getb(20); /* number of SVs */ + s1 = getword(2); /* number of bytes */ + s2 = getbyte(20); /* number of SVs */ break; case 0x70: /* Filter Report */ break; @@ -329,9 +334,9 @@ static int tsip_analyze(struct gps_device_t *session) case 0x82: /* Differential Position Fix Mode */ if (len != 1) break; - if (session->gpsdata.status == STATUS_FIX && (getb(0) & 0x01)) + if (session->gpsdata.status == STATUS_FIX && (getbyte(0) & 0x01)) session->gpsdata.status = STATUS_DGPS_FIX; - gpsd_report(4, "DGPS mode %d\n",getb(0)); + gpsd_report(4, "DGPS mode %d\n",getbyte(0)); break; case 0x83: /* Double-Precision XYZ Position Fix and Bias Information */ if (len != 36) @@ -361,7 +366,7 @@ static int tsip_analyze(struct gps_device_t *session) mask |= LATLON_SET | ALTITUDE_SET; break; case 0x8f: /* Super Packet. Well... */ - switch (getb(0)) /* sub-packet ID */ + switch (getbyte(0)) /* sub-packet ID */ { case 0x20: /* Last Fix with Extra Information (binary fixed point) */ if (len != 56) @@ -372,7 +377,7 @@ static int tsip_analyze(struct gps_device_t *session) break; break; default: - gpsd_report(4,"Unhandled TSIP superpacket type 0x%02x\n",getb(0)); + gpsd_report(4,"Unhandled TSIP superpacket type 0x%02x\n",getbyte(0)); } break; default: @@ -383,7 +388,7 @@ static int tsip_analyze(struct gps_device_t *session) /* see if it is time to send some request packets for reports that */ /* the receiver won't send at fixed intervals */ - time(&t); + (void)time(&t); if ((t - session->last_request) >= 5) { /* Request GPS Receiver Position Fix Mode */ (void)tsip_write(session->gpsdata.gps_fd, 0x24, buf, 0); |