summaryrefslogtreecommitdiff
diff options
context:
space:
mode:
Diffstat
-rw-r--r--bits.h4
-rw-r--r--gpsd.h4
-rw-r--r--tsip.c245
3 files changed, 131 insertions, 122 deletions
diff --git a/bits.h b/bits.h
index 18bcfebf..390a94f1 100644
--- a/bits.h
+++ b/bits.h
@@ -15,12 +15,12 @@
*/
union int_float {
- int i;
+ int32_t i;
float f;
};
union long_double {
- long long l;
+ int64_t l;
double d;
};
diff --git a/gpsd.h b/gpsd.h
index 353f921b..0c9286c6 100644
--- a/gpsd.h
+++ b/gpsd.h
@@ -115,6 +115,7 @@ struct gps_device_t {
unsigned long satcounter;
#endif /* SIRFII_ENABLE */
#ifdef TSIP_ENABLE
+ int16_t gps_week; /* Current GPS week number */
time_t last_request; /* Last time request packets were sent */
#endif /* TSIP_ENABLE */
#ifdef GARMIN_ENABLE /* private housekeeping stuff for the Garmin driver */
@@ -123,9 +124,6 @@ struct gps_device_t {
global context and save spave */
size_t GarminBufferLen; /* current GarminBuffer Length */
#endif /* GARMIN_ENABLE */
-#if defined(SIRFII_ENABLE) || defined(TSIP_ENABLE) || defined(GARMIN_ENABLE)
- unsigned int gps_week; /* Current GPS week number */
-#endif /*defined(SIRFII_ENABLE)||defined(TSIP_ENABLE)||defined(GARMIN_ENABLE)*/
#ifdef ZODIAC_ENABLE /* private housekeeping stuff for the Zodiac driver */
unsigned short sn; /* packet sequence number */
/*
diff --git a/tsip.c b/tsip.c
index 7a019466..4e5ae679 100644
--- a/tsip.c
+++ b/tsip.c
@@ -9,38 +9,10 @@
#define __USE_ISOC99 1 /* needed to get log2() from math.h */
#include <math.h>
#include "gpsd.h"
+#include "bits.h"
#ifdef TSIP_ENABLE
-union int_float {
- int i;
- float f;
-};
-
-union long_double {
- long long l;
- double d;
-};
-
-#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)(((unsigned long long)buf[off]<<56) \
- | ((unsigned long long)buf[off+1]<<48) \
- | ((unsigned long long)buf[off+2]<<40) \
- | ((unsigned long long)buf[off+3]<<32) \
- | ((unsigned long long)buf[off+4]<<24) \
- | ((unsigned long long)buf[off+5]<<16) \
- | ((unsigned long long)buf[off+6]<<8) \
- | (unsigned long long)buf[off+7]))
-#define getf(off) (i_f.i = getl(off), i_f.f)
-#define getd(off) (l_d.l = getL(off), l_d.d)
-
static int tsip_write(int fd, unsigned int id, unsigned char *buf, int len)
{
int i;
@@ -84,10 +56,10 @@ static void tsip_initializer(struct gps_device_t *session)
gpsd_set_speed(session, session->gpsdata.baudrate, 'O', 1);
/* I/O Options */
- putbyte(0,0x1e); /* Position: DP, MSL, LLA */
- putbyte(1,0x02); /* Velocity: ENU */
- putbyte(2,0x00); /* Time: GPS */
- putbyte(3,0x08); /* Aux: dBHz */
+ 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->gpsdata.gps_fd, 0x35, buf, 4);
/* Request Software Versions */
@@ -104,16 +76,16 @@ static bool tsip_speed_switch(struct gps_device_t *session, unsigned int speed)
{
unsigned char buf[100];
- 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 */
+ putbyte(buf,0,0xff); /* current port */
+ putbyte(buf,1,(round(log((double)speed/300)/M_LN2))+2); /* input baudrate */
+ putbyte(buf,2,getub(buf,1)); /* output baudrate */
+ putbyte(buf,3,8); /* character width (8 bits) */
+ putbyte(buf,4,1); /* parity (odd) */
+ putbyte(buf,5,0); /* stop bits (1 stopbit) */
+ putbyte(buf,6,0); /* flow control (none) */
+ putbyte(buf,7,0x02); /* input protocol (TSIP) */
+ putbyte(buf,8,0x02); /* output protocol (TSIP) */
+ putbyte(buf,9,0); /* reserved */
(void)tsip_write(session->gpsdata.gps_fd, 0xbc, buf, 10);
return true; /* it would be nice to error-check this */
@@ -121,10 +93,11 @@ static bool tsip_speed_switch(struct gps_device_t *session, unsigned int speed)
static gps_mask_t tsip_analyze(struct gps_device_t *session)
{
- int i, len;
+ int i, len, count;
gps_mask_t mask = 0;
- unsigned int id, u1;
- short s1,s2;
+ unsigned int id;
+ u_int8_t u1,u2,u3,u4;
+ int16_t s1;
float f1,f2,f3,f4,f5;
double d1,d2,d3,d4;
union int_float i_f;
@@ -165,20 +138,20 @@ static gps_mask_t 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",
- getbyte(0));
+ getub(buf,0));
break;
case 0x41: /* GPS Time */
if (len != 10)
break;
- f1 = getf(0); /* gpstime */
- s1 = getword(4); /* week */
- f2 = getf(6); /* leap seconds */
- if (f2 > 10.0) {
- session->gps_week = (unsigned)s1;
+ f1 = getf(buf,0); /* gpstime */
+ s1 = getsw(buf,4); /* week */
+ f2 = getf(buf,6); /* leap seconds */
+ if (f1 >= 0.0 && f2 > 10.0) {
+ session->gps_week = s1;
session->context->leap_seconds = (int)roundf(f2);
session->context->valid = LEAP_SECOND_VALID;
- session->gpsdata.sentence_time = gpstime_to_unix(s1, f1) - f2;
+ session->gpsdata.sentence_time = gpstime_to_unix((int)s1, f1) - f2;
#ifdef NTPSHM_ENABLE
(void)ntpshm_put(session, session->gpsdata.sentence_time + 0.075);
@@ -189,49 +162,50 @@ static gps_mask_t tsip_analyze(struct gps_device_t *session)
case 0x42: /* Single-Precision Position Fix, XYZ ECEF */
if (len != 16)
break;
- f1 = getf(0); /* X */
- f2 = getf(4); /* Y */
- f3 = getf(8); /* Z */
- f4 = getf(12); /* time-of-fix */
+ f1 = getf(buf,0); /* X */
+ f2 = getf(buf,4); /* Y */
+ f3 = getf(buf,8); /* Z */
+ f4 = getf(buf,12); /* time-of-fix */
gpsd_report(4, "GPS Position XYZ %f %f %f %f\n",f1,f2,f3,f4);
break;
case 0x43: /* Velocity Fix, XYZ ECEF */
if (len != 20)
break;
- f1 = getf(0); /* X velocity */
- f2 = getf(4); /* Y velocity */
- f3 = getf(8); /* Z velocity */
- f4 = getf(12); /* bias rate */
- f5 = getf(16); /* time-of-fix */
+ f1 = getf(buf,0); /* X velocity */
+ f2 = getf(buf,4); /* Y velocity */
+ f3 = getf(buf,8); /* Z velocity */
+ f4 = getf(buf,12); /* bias rate */
+ f5 = getf(buf,16); /* time-of-fix */
gpsd_report(4, "GPS Velocity XYZ %f %f %f %f %f\n",f1,f2,f3,f4,f5);
break;
case 0x45: /* Software Version Information */
if (len != 10)
break;
gpsd_report(4, "Software versions %d.%d %02d%02d%02d %d.%d %02d%02d%02d\n",
- (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));
+ getub(buf,0),getub(buf,1),getub(buf,4),getub(buf,2),getub(buf,3),
+ getub(buf,5),getub(buf,6),getub(buf,9),getub(buf,7),getub(buf,8));
break;
case 0x46: /* Health of Receiver */
if (len != 2)
break;
- gpsd_report(4, "Receiver health %02x %02x\n",getbyte(0),getbyte(1));
+ gpsd_report(4, "Receiver health %02x %02x\n",getub(buf,0),getub(buf,1));
break;
case 0x47: /* Signal Levels for all Satellites */
- s1 = (short)getbyte(0); /* count */
- if (len != (5*s1 + 1))
+ count = (int)getub(buf,0); /* satellite count */
+ if (len != (5*count + 1))
break;
- gpsd_zero_satellites(&session->gpsdata);
- session->gpsdata.satellites = s1;
+ session->gpsdata.satellites = count;
buf2[0] = '\0';
- for (i = 0; i < s1; i++) {
- session->gpsdata.PRN[i] = s2 = (short)getbyte(5*i + 1);
- f1 = getf(5*i + 2);
+ for (i = 0; i < count; i++) {
+ /*@ +charint @*/
+ session->gpsdata.PRN[i] = u1 = getub(buf,5*i + 1);
+ /*@ -charint @*/
+ f1 = getf(buf,5*i + 2);
session->gpsdata.ss[i] = (int)f1;
(void)snprintf(buf2+strlen(buf2), sizeof(buf2)-strlen(buf2),
- " %d=%1f",s2,f1);
+ " %d=%1f",(int)u1,f1);
}
- gpsd_report(4, "Signal Levels (%d):%s\n",s1,buf2);
+ gpsd_report(4, "Signal Levels (%d):%s\n",count,buf2);
mask |= SATELLITE_SET;
break;
case 0x48: /* GPS System Message */
@@ -241,11 +215,11 @@ static gps_mask_t tsip_analyze(struct gps_device_t *session)
case 0x4a: /* Single-Precision Position LLA */
if (len != 20)
break;
- session->gpsdata.fix.latitude = getf(0) * RAD_2_DEG;
- session->gpsdata.fix.longitude = getf(4) * RAD_2_DEG;
- session->gpsdata.fix.altitude = getf(8);
- f1 = getf(12); /* clock bias */
- f2 = getf(16); /* time-of-fix */
+ session->gpsdata.fix.latitude = getf(buf,0) * RAD_2_DEG;
+ session->gpsdata.fix.longitude = getf(buf,4) * RAD_2_DEG;
+ session->gpsdata.fix.altitude = getf(buf,8);
+ f1 = getf(buf,12); /* clock bias */
+ f2 = getf(buf,16); /* time-of-fix */
if (session->gps_week)
session->gpsdata.fix.time = session->gpsdata.sentence_time =
gpstime_to_unix((int)session->gps_week, f2) - session->context->leap_seconds;
@@ -258,22 +232,22 @@ static gps_mask_t 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",getbyte(0),getbyte(1),getbyte(2));
+ gpsd_report(4, "Machine ID %02x %02x %02x\n",getub(buf,0),getub(buf,1),getub(buf,2));
break;
case 0x55: /* IO Options */
if (len != 4)
break;
- gpsd_report(4, "IO Options %02x %02x %02x %02x\n",getbyte(0),getbyte(1),getbyte(2),getbyte(4));
+ gpsd_report(4, "IO Options %02x %02x %02x %02x\n",getub(buf,0),getub(buf,1),getub(buf,2),getub(buf,3));
break;
case 0x56: /* Velocity Fix, East-North-Up (ENU) */
if (len != 20)
break;
- f1 = getf(0); /* East velocity */
- f2 = getf(4); /* North velocity */
- f3 = getf(8); /* Up velocity */
- f4 = getf(12); /* clock bias rate */
- f5 = getf(16); /* time-of-fix */
- session->gpsdata.fix.climb = f1;
+ f1 = getf(buf,0); /* East velocity */
+ f2 = getf(buf,4); /* North velocity */
+ f3 = getf(buf,8); /* Up velocity */
+ f4 = getf(buf,12); /* clock bias rate */
+ f5 = getf(buf,16); /* time-of-fix */
+ session->gpsdata.fix.climb = f3;
/*@ -evalorder @*/
session->gpsdata.fix.speed = sqrt(pow(f2,2) + pow(f1,2));
/*@ +evalorder @*/
@@ -285,22 +259,47 @@ static gps_mask_t tsip_analyze(struct gps_device_t *session)
case 0x57: /* Information About Last Computed Fix */
if (len != 8)
break;
- f1 = getf(2); /* gps_time */
- s1 = getword(6); /* gps_weeks */
- if ((int)getbyte(0) != 0) /* good current fix? */
- session->gps_week = (unsigned)s1;
- gpsd_report(4, "Fix info %02x %02x %d %f\n",getbyte(0),getbyte(1),s1,f1);
+ f1 = getf(buf,2); /* gps_time */
+ s1 = getsw(buf,6); /* gps_week */
+ /*@ +charint @*/
+ if (getub(buf,0) == 0x01) /* good current fix? */
+ session->gps_week = s1;
+ /*@ -charint @*/
+ gpsd_report(4, "Fix info %02x %02x %d %f\n",getub(buf,0),getub(buf,1),s1,f1);
break;
case 0x58: /* Satellite System Data/Acknowledge from Receiver */
break;
case 0x59: /* Status of Satellite Disable or Ignore Health */
break;
case 0x5a: /* Raw Measurement Data */
+ if (len != 25)
+ break;
+ f1 = getf(buf,5); /* Signal Level */
+ f2 = getf(buf,9); /* Code phase */
+ f3 = getf(buf,13); /* Doppler */
+ d1 = getd(buf,17); /* Time of Measurement */
+ gpsd_report(4, "Raw Measurement Data %d %f %f %f %f\n",getub(buf,0),f1,f2,f3,d1);
break;
case 0x5c: /* Satellite Tracking Status */
+ if (len != 24)
+ break;
+ u1 = getub(buf,0); /* PRN */
+ u2 = getub(buf,1); /* chan */
+ u3 = getub(buf,2); /* Acquisition flag */
+ u4 = getub(buf,3); /* Ephemeris flag */
+ f1 = getf(buf,4); /* Signal level */
+ f2 = getf(buf,8); /* time of Last measurement */
+ d1 = getf(buf,12) * RAD_2_DEG; /* Elevation */
+ d2 = getf(buf,16) * RAD_2_DEG; /* Azimuth */
+ id = (unsigned)u2 >> 3; /* channel number */
+ session->gpsdata.PRN[id] = (int)u1;
+ session->gpsdata.ss[id] = (int)roundf(f1);
+ session->gpsdata.elevation[id] = (int)round(d1);
+ session->gpsdata.azimuth[id] = (int)round(d2);
+ gpsd_report(4, "Satellite Tracking Status %d: %d 0x%02x %d %d %f %f %f %f\n",id,u1,u2,u3,u4,f1,f2,d1,d2);
break;
case 0x6d: /* All-In-View Satellite Selection */
- u1 = (unsigned)getbyte(0);
+ u1 = getub(buf,0);
switch (u1 & 7) /* dimension */
{
case 3:
@@ -314,19 +313,23 @@ static gps_mask_t tsip_analyze(struct gps_device_t *session)
break;
}
session->gpsdata.satellites_used = (int)((u1 >> 4) & 0x0f);
- session->gpsdata.pdop = getf(1);
- session->gpsdata.hdop = getf(5);
- session->gpsdata.vdop = getf(9);
- session->gpsdata.tdop = getf(13);
+ session->gpsdata.pdop = getf(buf,1);
+ session->gpsdata.hdop = getf(buf,5);
+ session->gpsdata.vdop = getf(buf,9);
+ session->gpsdata.tdop = getf(buf,13);
/*@ -evalorder @*/
session->gpsdata.gdop = sqrt(pow(session->gpsdata.pdop,2)+pow(session->gpsdata.tdop,2));
/*@ +evalorder @*/
memset(session->gpsdata.used,0,sizeof(session->gpsdata.used));
+ buf2[0] = '\0';
+ /*@ +charint @*/
for (i = 0; i < session->gpsdata.satellites_used; i++)
- session->gpsdata.used[i] = (int)getbyte(16 + i);
+ (void)snprintf(buf2+strlen(buf2), sizeof(buf2)-strlen(buf2),
+ " %d",session->gpsdata.used[i] = getub(buf,17 + i));
+ /*@ -charint @*/
- gpsd_report(4, "Sat info: %d %d\n",session->gpsdata.fix.mode,session->gpsdata.satellites_used);
+ gpsd_report(4, "Sat info: %d %d:%s\n",session->gpsdata.fix.mode,session->gpsdata.satellites_used,buf2);
gpsd_binary_quality_dump(session, buf2, sizeof(buf2));
gpsd_report(3, "<= GPS: %s", buf2);
mask |= HDOP_SET | VDOP_SET | PDOP_SET | MODE_SET;
@@ -334,10 +337,12 @@ static gps_mask_t tsip_analyze(struct gps_device_t *session)
case 0x6e: /* Synchronized Measurements */
break;
case 0x6f: /* Synchronized Measurements Report */
- if (len < 20 || (int)getbyte(0) != 1 || (int)getbyte(1) != 2)
+ /*@ +charint @*/
+ if (len < 20 || getub(buf,0) != 1 || getub(buf,1) != 2)
break;
- s1 = getword(2); /* number of bytes */
- s2 = (short)getbyte(20); /* number of SVs */
+ /*@ -charint @*/
+ s1 = getsw(buf,2); /* number of bytes */
+ u1 = getub(buf,20); /* number of SVs */
break;
case 0x70: /* Filter Report */
break;
@@ -346,31 +351,33 @@ static gps_mask_t tsip_analyze(struct gps_device_t *session)
case 0x82: /* Differential Position Fix Mode */
if (len != 1)
break;
- if (session->gpsdata.status==STATUS_FIX && ((int)getbyte(0) & 0x01)!=0)
+ /*@ +charint @*/
+ if (session->gpsdata.status == STATUS_FIX && (getub(buf,0) & 0x01)!=0)
session->gpsdata.status = STATUS_DGPS_FIX;
- gpsd_report(4, "DGPS mode %d\n",getbyte(0));
+ /*@ -charint @*/
+ gpsd_report(4, "DGPS mode %d\n",getub(buf,0));
break;
case 0x83: /* Double-Precision XYZ Position Fix and Bias Information */
if (len != 36)
break;
- d1 = getd(0); /* X */
- d2 = getd(8); /* Y */
- d3 = getd(16); /* Z */
- d4 = getd(24); /* clock bias */
- f1 = getf(32); /* time-of-fix */
+ d1 = getd(buf,0); /* X */
+ d2 = getd(buf,8); /* Y */
+ d3 = getd(buf,16); /* Z */
+ d4 = getd(buf,24); /* clock bias */
+ f1 = getf(buf,32); /* time-of-fix */
gpsd_report(4, "GPS Position XYZ %f %f %f %f %f\n",d1,d2,d3,d4,f1);
break;
case 0x84: /* Double-Precision LLA Position Fix and Bias Information */
if (len != 36)
break;
- session->gpsdata.fix.latitude = getd(0) * RAD_2_DEG;
- session->gpsdata.fix.longitude = getd(8) * RAD_2_DEG;
- session->gpsdata.fix.altitude = getd(16);
- d1 = getd(24); /* clock bias */
- f2 = getf(32); /* time-of-fix */
+ session->gpsdata.fix.latitude = getd(buf,0) * RAD_2_DEG;
+ session->gpsdata.fix.longitude = getd(buf,8) * RAD_2_DEG;
+ session->gpsdata.fix.altitude = getd(buf,16);
+ d1 = getd(buf,24); /* clock bias */
+ f1 = getf(buf,32); /* time-of-fix */
if (session->gps_week)
session->gpsdata.fix.time = session->gpsdata.sentence_time =
- gpstime_to_unix((int)session->gps_week, f2) - session->context->leap_seconds;
+ gpstime_to_unix((int)session->gps_week, f1) - session->context->leap_seconds;
session->gpsdata.status = STATUS_FIX;
gpsd_report(4, "GPS DP LLA %f %f %f\n",session->gpsdata.fix.latitude,session->gpsdata.fix.longitude,session->gpsdata.fix.altitude);
gpsd_binary_fix_dump(session, buf2, sizeof(buf2));
@@ -378,7 +385,7 @@ static gps_mask_t tsip_analyze(struct gps_device_t *session)
mask |= LATLON_SET | ALTITUDE_SET;
break;
case 0x8f: /* Super Packet. Well... */
- switch (getbyte(0)) /* sub-packet ID */
+ switch (getub(buf,0)) /* sub-packet ID */
{
case 0x20: /* Last Fix with Extra Information (binary fixed point) */
if (len != 56)
@@ -389,7 +396,7 @@ static gps_mask_t tsip_analyze(struct gps_device_t *session)
break;
break;
default:
- gpsd_report(4,"Unhandled TSIP superpacket type 0x%02x\n",getbyte(0));
+ gpsd_report(4,"Unhandled TSIP superpacket type 0x%02x\n",getub(buf,0));
}
break;
default:
@@ -408,6 +415,10 @@ static gps_mask_t tsip_analyze(struct gps_device_t *session)
/* Request Signal Levels */
(void)tsip_write(session->gpsdata.gps_fd, 0x27, buf, 0);
+ /* Request Current Satellite Tracking Status */
+ putbyte(buf,0,0x00); /* All satellites */
+ (void)tsip_write(session->gpsdata.gps_fd, 0x3c, buf, 1);
+
session->last_request = t;
}
View Lorry Controller Status