diff -ur gps3d-1.20/Makefile gps3d-1.20-pere/Makefile --- gps3d-1.20/Makefile Fri Mar 29 17:15:14 2002 +++ gps3d-1.20-pere/Makefile Sat Jan 1 23:29:03 2005 @@ -1,6 +1,6 @@ LDFLAGS= -s -CFLAGS= -g0 -O99 -fomit-frame-pointer -I. -DNDEBUG -DX11 +CFLAGS= -g -O99 -fomit-frame-pointer -I. -DNDEBUG -DX11 #LDFLAGS= -g #CFLAGS= -g3 -O0 -Wall -I. -D_DEBUG @@ -56,7 +56,7 @@ gps \ gpsd \ viz - + all: ${ALL} @@ -67,7 +67,7 @@ ${CC} ${LDFLAGS} -o gpsd ${GPSD_OBJ} -lm viz:${VIZ_OBJ} - ${CC} ${LDFLAGS} -o viz ${VIZ_OBJ} -L/usr/X11R6/lib -lGL -lX11 -lXext -lm + ${CC} ${LDFLAGS} -o viz ${VIZ_OBJ} -L/usr/X11R6/lib -lGL -lX11 -lXext -lgps -lm clean: -rm -f *.o core ${ALL} diff -ur gps3d-1.20/viz_app.h gps3d-1.20-pere/viz_app.h --- gps3d-1.20/viz_app.h Fri Mar 29 17:15:14 2002 +++ gps3d-1.20-pere/viz_app.h Sun Jan 2 00:23:59 2005 @@ -8,7 +8,7 @@ void loop(); void newChannel(void*,int (*callback)(void*),void*); -int gpsInput(); +int gpsInput(void *data); void gpsCharInput(uint8_t); void *gpsSocket(); void gpsOutput(const char *); @@ -27,12 +27,15 @@ void paintSquareInfo(double,double,double,double,double,double); void newGGA(const uint8_t*,const uint8_t*,const uint8_t*); void newFix(const uint8_t*,const uint8_t*,const uint8_t*,const uint8_t*,const uint8_t*); +void newFixValue(double latitude, double longitude, double timestamp); void paintGreatcircle(double,double,double,double,int,double*,double*); void toggleAutomap(); void paintAuxInfo(); void paintSatellites(); void newSolution(const uint8_t [][81],int); +void newSolutionValue(const int prn_used[], int nFields); +void newSatelliteFixValue(int prn, double elevation, double azimuth); void newSatelliteFix( const uint8_t*, const uint8_t*, diff -ur gps3d-1.20/viz_brush.cpp gps3d-1.20-pere/viz_brush.cpp --- gps3d-1.20/viz_brush.cpp Fri Mar 29 17:15:14 2002 +++ gps3d-1.20-pere/viz_brush.cpp Sat Jan 1 22:39:56 2005 @@ -18,7 +18,7 @@ while(size--) { uint8_t c= *(buffer++); - gpsCharInput(c); + //gpsCharInput(c); } return 0; } diff -ur gps3d-1.20/viz_fix.cpp gps3d-1.20-pere/viz_fix.cpp --- gps3d-1.20/viz_fix.cpp Fri Mar 29 17:15:14 2002 +++ gps3d-1.20-pere/viz_fix.cpp Sat Jan 1 23:15:46 2005 @@ -125,32 +125,14 @@ trash(); } -void newFix( -const uint8_t *latitude, -const uint8_t *latitudeSign, -const uint8_t *longitude, -const uint8_t *longitudeSign, -const uint8_t *timeStamp -) +void newFixValue(double latitude, double longitude, double timestamp) { - double tmpLa; - double tmpLo; - if(parseNMEAAngle(latitude,latitudeSign,&tmpLa)<0) return; - if(parseNMEAAngle(longitude,longitudeSign,&tmpLo)<0) return; - if(tmpLa!=la || tmpLo!=lo) + if(latitude!=la || longitude!=lo) { - double hr= (timeStamp[0]-'0')*10 + (timeStamp[1]-'0'); - double mm= (timeStamp[2]-'0')*10 + (timeStamp[3]-'0'); - double ss= (timeStamp[4]-'0')*10 + (timeStamp[5]-'0'); - ss+= (timeStamp[6]-'0')/10.0 + (timeStamp[7]-'0')/100.0; - ss+= mm*60; - ss+= hr*3600; - - la= tmpLa; - lo= tmpLo; - stamp= ss; + la = latitude; + lo = longitude; + stamp = timestamp; trash(); - } if(autoMap) @@ -172,6 +154,30 @@ double dist= system::sqrt(pos[0] + pos[1] + pos[2]); getHTTPMap(0,la,lo,dist); + } +} + +void newFix( +const uint8_t *latitude, +const uint8_t *latitudeSign, +const uint8_t *longitude, +const uint8_t *longitudeSign, +const uint8_t *timeStamp +) +{ + double tmpLa; + double tmpLo; + if(parseNMEAAngle(latitude,latitudeSign,&tmpLa)<0) return; + if(parseNMEAAngle(longitude,longitudeSign,&tmpLo)<0) return; + + { + double hr= (timeStamp[0]-'0')*10 + (timeStamp[1]-'0'); + double mm= (timeStamp[2]-'0')*10 + (timeStamp[3]-'0'); + double ss= (timeStamp[4]-'0')*10 + (timeStamp[5]-'0'); + ss+= (timeStamp[6]-'0')/10.0 + (timeStamp[7]-'0')/100.0; + ss+= mm*60; + ss+= hr*3600; + newFixValue(tmpLa, tmpLo, ss); } } diff -ur gps3d-1.20/viz_gps.cpp gps3d-1.20-pere/viz_gps.cpp --- gps3d-1.20/viz_gps.cpp Fri Mar 29 17:15:14 2002 +++ gps3d-1.20-pere/viz_gps.cpp Sun Jan 2 00:25:59 2005 @@ -1,6 +1,7 @@ #include #include +#include static int checksum( const uint8_t *s @@ -9,19 +10,19 @@ uint8_t sum1= 0; while(1) { - uint8_t c= *(s++); - if(c==0 || c=='*') break; - sum1^= c; + uint8_t c= *(s++); + if(c==0 || c=='*') break; + sum1^= c; } uint32_t sum2= 0; while(1) { - int c= *(s++); - c-= (c>'9' ? 'A'-10 : '0'); - if(c<0 || c>15) break; - sum2<<= 4; - sum2+= c; + int c= *(s++); + c-= (c>'9' ? 'A'-10 : '0'); + if(c<0 || c>15) break; + sum2<<= 4; + sum2+= c; } return sum2!=(uint32_t)sum1; @@ -123,101 +124,89 @@ } } -void gpsCharInput( -uint8_t c -) +static int process_status(struct gps_data_t *status) { - static uint8_t *p; - static int state= 0; - static uint8_t buffer[81]; - switch(state) - { - case 0: - if(c=='\n') state= 1; - break; - case 1: - if(c=='$') state= 2; - else if(c=='\n') state= 1; - else state= 0; - p= buffer; - break; - case 2: - if(c=='\r') - { - p[0]= 0; - state= 0; - sentence(buffer); - } - else - { - *(p++)= c; - if(p-buffer>=80) state= 0; - } - break; +#if 0 /* Not supported yet */ + if (1) + newGGA(utc, quality, altitude); + newTrackFix(latitude,latitudesign, longitude, longitudesign); + newWaypoint(latitude, latitudeSign, longitude, longitudeSign, + ?, ?, name, ?, ?); +#endif + if (status->latlon_stamp.changed) + { + system::error(0,"info: Got new GPS pos\n"); + newFixValue(status->latitude, status->longitude, + status->latlon_stamp.last_refresh); + status->latlon_stamp.changed = 0; + } + + if (status->satellite_stamp.changed) + { + system::error(0,"info: Got %d GPS satellite fixes\n", + status->satellites); + for (int i = 0; i < status->satellites; ++i) + { + newSatelliteFixValue(status->PRN[i], status->elevation[i], + status->azimuth[i]); + } + status->satellite_stamp.changed = 0; + } + if (status->fix_quality_stamp.changed) + { + int prn_used[MAXCHANNELS]; + system::error(0,"info: Got GPS satellite solution (%d)\n", + status->satellites_used); + for (int i = 0; i < status->satellites; ++i) + { + prn_used[i] = status->PRN[i]; + } + newSolutionValue(status->used, status->satellites_used); + status->fix_quality_stamp.changed = 0; } + return 0; } -int gpsInput() +static struct gps_data_t *status = NULL; + +int gpsInput(void *data) { - while(1) + if (gps_poll(status)) { - uint8_t c; - int r= system::readSocket(gpsSocket(),&c,1); - if(r==1) gpsCharInput(c); - else break; + return process_status(status); } return 0; } -void *gpsSocket() + +void *gpsSocket(void) { static int init= 0; - static void *socket; if(init==0) { - int port; - const char *sport= getPort(); - const char *server= getServer(); - if(1!=system::sscanf(sport,"%d",&port)) port= 2222; - socket= system::makeSocket(server,port); - if(socket==(void*)-1) system::error(0,"warning: can't connect to GPS server %s:%d\n",server,port); - else system::error(0,"info: Connected to GPS server %s:%d\n",server,port); - init= 1; + char *port= (char*)getPort(); + char *server= (char*)getServer(); + status = gps_open(server, DEFAULT_GPSD_PORT); + if(status==NULL) + system::error(0,"warning: can't connect to GPS server %s:%s\n",server,port); + else + { + system::error(0,"info: Connected to GPS server %s:%s\n",server,port); + gps_query(status, "w+x\n"); + init= 1; + } } - return socket; + if (status) + return (void*)status->gps_fd; + else + return (void*)-1; } +/* gpsd do not accept input, and can't handle talkback. Disabled. */ void gpsOutput( const char *msg ) { - uint8_t buf[160]; - system::sprintf((char*)buf,"$PMGNCMD,%s*XX\r\n",msg); - - uint8_t sum= 0; - uint8_t *p= buf+1; - while(1) - { - uint8_t c= *(p++); - if(c=='*') break; - sum^= c; - } - - static const uint8_t *tohex= (const uint8_t *)"0123456789ABCDEF"; - p[0]= tohex[(sum>>4)&0xF]; - p[1]= tohex[(sum>>0)&0xF]; - - int n= 4+p-buf; - while(n) - { - int r= system::writeSocket(gpsSocket(),buf,n); - if(r>=0) n-= r; - else if(system::lastError()!=system::eAgain && system::lastError()!=system::eInterrupt) - { - system::error(0,"warning: can't write to gps socket\n"); - break; - } - } } void gpsLoadTrackFile( diff -ur gps3d-1.20/viz_satellite.cpp gps3d-1.20-pere/viz_satellite.cpp --- gps3d-1.20/viz_satellite.cpp Fri Nov 24 01:19:49 2000 +++ gps3d-1.20-pere/viz_satellite.cpp Sat Jan 1 23:49:05 2005 @@ -4,14 +4,15 @@ #include #define TRAJSIZE 10000 -static int trajSizes[50]; -static int trajColor[50]; -static double lastSatAzimuth[50]; -static double lastSatElevation[50]; -static double satX[50][TRAJSIZE]; -static double satY[50][TRAJSIZE]; -static double satZ[50][TRAJSIZE]; -static double satT[50][TRAJSIZE]; +#define MAXSATELLITES 50 +static int trajSizes[MAXSATELLITES]; +static int trajColor[MAXSATELLITES]; +static double lastSatAzimuth[MAXSATELLITES]; +static double lastSatElevation[MAXSATELLITES]; +static double satX[MAXSATELLITES][TRAJSIZE]; +static double satY[MAXSATELLITES][TRAJSIZE]; +static double satZ[MAXSATELLITES][TRAJSIZE]; +static double satT[MAXSATELLITES][TRAJSIZE]; // earthRadius = 6 378 136 meters // GPS satellites orbit at 20 180 000 meters altitude (from ground)= 26558136 from earth center @@ -52,14 +53,30 @@ return valid; } +void newSolutionValue(const int prn_used[], int nFields) +{ + int tj[MAXSATELLITES]; + system::memset(tj,0,sizeof(tj)); + if(nFields>15) nFields=15; + for(int j=3; j15) nFields=15; for(int j=3; j90.0) return; - if(a>360.0) return; - if(num>=50) return; + if(elevation>90.0) return; + if(azimuth>360.0) return; + if(prn>=MAXSATELLITES) return; - int pnum= trajSizes[num]; + int pnum= trajSizes[prn]; if( pnum==0 || - lastSatAzimuth[num]!=a || - lastSatElevation[num]!=e + lastSatAzimuth[prn]!=azimuth || + lastSatElevation[prn]!=elevation ) { double sat[3]; @@ -138,23 +143,40 @@ int correct= ae2xyz( la, lo, - a, - e, + azimuth, + elevation, altitudeSquared, sat ); if(correct) { int mod= pnum%TRAJSIZE; - satX[num][mod]= sat[0]; - satY[num][mod]= sat[1]; - satZ[num][mod]= sat[2]; - satT[num][mod]= lastStamp(); - trajSizes[num]= ++pnum; + satX[prn][mod]= sat[0]; + satY[prn][mod]= sat[1]; + satZ[prn][mod]= sat[2]; + satT[prn][mod]= lastStamp(); + trajSizes[prn]= ++pnum; trash(); } } } +void newSatelliteFix( +const uint8_t *prn, +const uint8_t *elevation, +const uint8_t *azimuth, +const uint8_t * +) +{ + int num; + double e; + double a; + if(lastStamp()<0) return; + if(1!=system::sscanf((const char*)prn,"%d",&num)) return; + if(1!=system::sscanf((const char*)elevation,"%lf",&e)) return; + if(1!=system::sscanf((const char*)azimuth,"%lf",&a)) return; + + newSatelliteFixValue(num, e, a); +} static inline void satPos( int i, @@ -169,7 +191,7 @@ void paintSatellites() { - for(int i=0;i<50;++i) + for(int i=0;i