linux/gps3d-1.20-gpsd.diff

   1 diff -ur gps3d-1.20/Makefile gps3d-1.20-pere/Makefile
   2 --- gps3d-1.20/Makefile Fri Mar 29 17:15:14 2002
   3 +++ gps3d-1.20-pere/Makefile    Sat Jan  1 23:29:03 2005
   4 @@ -1,6 +1,6 @@
   5
   6  LDFLAGS= -s
   7 -CFLAGS= -g0 -O99 -fomit-frame-pointer -I. -DNDEBUG -DX11
   8 +CFLAGS= -g -O99 -fomit-frame-pointer -I. -DNDEBUG -DX11
   9
  10  #LDFLAGS= -g
  11  #CFLAGS= -g3 -O0 -Wall -I. -D_DEBUG
  12 @@ -56,7 +56,7 @@
  13         gps                     \
  14         gpsd                    \
  15         viz
  16 -
  17 +
  18
  19  all: ${ALL}
  20
  21 @@ -67,7 +67,7 @@
  22         ${CC} ${LDFLAGS} -o gpsd ${GPSD_OBJ} -lm
  23
  24  viz:${VIZ_OBJ}
  25 -       ${CC} ${LDFLAGS} -o viz ${VIZ_OBJ} -L/usr/X11R6/lib -lGL -lX11 -lXext -lm
  26 +       ${CC} ${LDFLAGS} -o viz ${VIZ_OBJ} -L/usr/X11R6/lib -lGL -lX11 -lXext -lgps -lm
  27
  28  clean:
  29         -rm -f *.o core ${ALL}
  30 diff -ur gps3d-1.20/viz_app.h gps3d-1.20-pere/viz_app.h
  31 --- gps3d-1.20/viz_app.h        Fri Mar 29 17:15:14 2002
  32 +++ gps3d-1.20-pere/viz_app.h   Sun Jan  2 00:23:59 2005
  33 @@ -8,7 +8,7 @@
  34  void   loop();
  35  void   newChannel(void*,int (*callback)(void*),void*);
  36
  37 -int    gpsInput();
  38 +int    gpsInput(void *data);
  39  void   gpsCharInput(uint8_t);
  40  void   *gpsSocket();
  41  void   gpsOutput(const char *);
  42 @@ -27,12 +27,15 @@
  43  void   paintSquareInfo(double,double,double,double,double,double);
  44  void   newGGA(const uint8_t*,const uint8_t*,const uint8_t*);
  45  void   newFix(const uint8_t*,const uint8_t*,const uint8_t*,const uint8_t*,const uint8_t*);
  46 +void   newFixValue(double latitude, double longitude, double timestamp);
  47  void   paintGreatcircle(double,double,double,double,int,double*,double*);
  48  void   toggleAutomap();
  49  void   paintAuxInfo();
  50
  51  void   paintSatellites();
  52  void   newSolution(const uint8_t [][81],int);
  53 +void   newSolutionValue(const int prn_used[], int nFields);
  54 +void   newSatelliteFixValue(int prn, double elevation, double azimuth);
  55  void   newSatelliteFix(
  56             const uint8_t*,
  57             const uint8_t*,
  58 diff -ur gps3d-1.20/viz_brush.cpp gps3d-1.20-pere/viz_brush.cpp
  59 --- gps3d-1.20/viz_brush.cpp    Fri Mar 29 17:15:14 2002
  60 +++ gps3d-1.20-pere/viz_brush.cpp       Sat Jan  1 22:39:56 2005
  61 @@ -18,7 +18,7 @@
  62      while(size--)
  63      {
  64         uint8_t c= *(buffer++);
  65 -       gpsCharInput(c);
  66 +       //gpsCharInput(c);
  67      }
  68      return 0;
  69  }
  70 diff -ur gps3d-1.20/viz_fix.cpp gps3d-1.20-pere/viz_fix.cpp
  71 --- gps3d-1.20/viz_fix.cpp      Fri Mar 29 17:15:14 2002
  72 +++ gps3d-1.20-pere/viz_fix.cpp Sat Jan  1 23:15:46 2005
  73 @@ -125,32 +125,14 @@
  74      trash();
  75  }
  76
  77 -void newFix(
  78 -const uint8_t *latitude,
  79 -const uint8_t *latitudeSign,
  80 -const uint8_t *longitude,
  81 -const uint8_t *longitudeSign,
  82 -const uint8_t *timeStamp
  83 -)
  84 +void newFixValue(double latitude, double longitude, double timestamp)
  85  {
  86 -    double tmpLa;
  87 -    double tmpLo;
  88 -    if(parseNMEAAngle(latitude,latitudeSign,&tmpLa)<0) return;
  89 -    if(parseNMEAAngle(longitude,longitudeSign,&tmpLo)<0) return;
  90 -    if(tmpLa!=la || tmpLo!=lo)
  91 +    if(latitude!=la || longitude!=lo)
  92      {
  93 -       double hr= (timeStamp[0]-'0')*10 + (timeStamp[1]-'0');
  94 -       double mm= (timeStamp[2]-'0')*10 + (timeStamp[3]-'0');
  95 -       double ss= (timeStamp[4]-'0')*10 + (timeStamp[5]-'0');
  96 -       ss+= (timeStamp[6]-'0')/10.0 + (timeStamp[7]-'0')/100.0;
  97 -       ss+= mm*60;
  98 -       ss+= hr*3600;
  99 -
 100 -       la= tmpLa;
 101 -       lo= tmpLo;
 102 -       stamp= ss;
 103 +        la = latitude;
 104 +       lo = longitude;
 105 +       stamp = timestamp;
 106         trash();
 107 -
 108      }
 109
 110      if(autoMap)
 111 @@ -172,6 +154,30 @@
 112
 113         double dist= system::sqrt(pos[0] + pos[1] + pos[2]);
 114         getHTTPMap(0,la,lo,dist);
 115 +    }
 116 +}
 117 +
 118 +void newFix(
 119 +const uint8_t *latitude,
 120 +const uint8_t *latitudeSign,
 121 +const uint8_t *longitude,
 122 +const uint8_t *longitudeSign,
 123 +const uint8_t *timeStamp
 124 +)
 125 +{
 126 +    double tmpLa;
 127 +    double tmpLo;
 128 +    if(parseNMEAAngle(latitude,latitudeSign,&tmpLa)<0) return;
 129 +    if(parseNMEAAngle(longitude,longitudeSign,&tmpLo)<0) return;
 130 +
 131 +    {
 132 +        double hr= (timeStamp[0]-'0')*10 + (timeStamp[1]-'0');
 133 +       double mm= (timeStamp[2]-'0')*10 + (timeStamp[3]-'0');
 134 +       double ss= (timeStamp[4]-'0')*10 + (timeStamp[5]-'0');
 135 +       ss+= (timeStamp[6]-'0')/10.0 + (timeStamp[7]-'0')/100.0;
 136 +       ss+= mm*60;
 137 +       ss+= hr*3600;
 138 +       newFixValue(tmpLa, tmpLo, ss);
 139      }
 140  }
 141
 142 diff -ur gps3d-1.20/viz_gps.cpp gps3d-1.20-pere/viz_gps.cpp
 143 --- gps3d-1.20/viz_gps.cpp      Fri Mar 29 17:15:14 2002
 144 +++ gps3d-1.20-pere/viz_gps.cpp Sun Jan  2 00:25:59 2005
 145 @@ -1,6 +1,7 @@
 146
 147  #include <viz_app.h>
 148  #include <viz_system.h>
 149 +#include <gps.h>
 150
 151  static int checksum(
 152  const uint8_t *s
 153 @@ -9,19 +10,19 @@
 154      uint8_t sum1= 0;
 155      while(1)
 156      {
 157 -       uint8_t c= *(s++);
 158 -       if(c==0 || c=='*') break;
 159 -       sum1^= c;
 160 +       uint8_t c= *(s++);
 161 +       if(c==0 || c=='*') break;
 162 +       sum1^= c;
 163      }
 164
 165      uint32_t sum2= 0;
 166      while(1)
 167      {
 168 -       int c= *(s++);
 169 -       c-= (c>'9' ? 'A'-10 : '0');
 170 -       if(c<0 || c>15) break;
 171 -       sum2<<= 4;
 172 -       sum2+= c;
 173 +       int c= *(s++);
 174 +       c-= (c>'9' ? 'A'-10 : '0');
 175 +       if(c<0 || c>15) break;
 176 +       sum2<<= 4;
 177 +       sum2+= c;
 178      }
 179
 180      return sum2!=(uint32_t)sum1;
 181 @@ -123,101 +124,89 @@
 182      }
 183  }
 184
 185 -void gpsCharInput(
 186 -uint8_t c
 187 -)
 188 +static int process_status(struct gps_data_t *status)
 189  {
 190 -    static uint8_t *p;
 191 -    static int state= 0;
 192 -    static uint8_t buffer[81];
 193 -    switch(state)
 194 -    {
 195 -       case 0:
 196 -           if(c=='\n')         state= 1;
 197 -           break;
 198 -       case 1:
 199 -           if(c=='$')          state= 2;
 200 -           else if(c=='\n')    state= 1;
 201 -           else                state= 0;
 202 -           p= buffer;
 203 -           break;
 204 -       case 2:
 205 -           if(c=='\r')
 206 -           {
 207 -               p[0]= 0;
 208 -               state= 0;
 209 -               sentence(buffer);
 210 -           }
 211 -           else
 212 -           {
 213 -               *(p++)= c;
 214 -               if(p-buffer>=80) state= 0;
 215 -           }
 216 -           break;
 217 +#if 0 /* Not supported yet */
 218 +    if (1)
 219 +        newGGA(utc, quality, altitude);
 220 +    newTrackFix(latitude,latitudesign, longitude, longitudesign);
 221 +    newWaypoint(latitude, latitudeSign, longitude, longitudeSign,
 222 +                ?, ?, name, ?, ?);
 223 +#endif
 224 +    if (status->latlon_stamp.changed)
 225 +    {
 226 +        system::error(0,"info: Got new GPS pos\n");
 227 +        newFixValue(status->latitude, status->longitude,
 228 +                    status->latlon_stamp.last_refresh);
 229 +        status->latlon_stamp.changed = 0;
 230 +    }
 231 +
 232 +    if (status->satellite_stamp.changed)
 233 +    {
 234 +        system::error(0,"info: Got %d GPS satellite fixes\n",
 235 +                      status->satellites);
 236 +        for (int i = 0; i < status->satellites; ++i)
 237 +        {
 238 +            newSatelliteFixValue(status->PRN[i], status->elevation[i],
 239 +                                 status->azimuth[i]);
 240 +        }
 241 +        status->satellite_stamp.changed = 0;
 242 +    }
 243 +    if (status->fix_quality_stamp.changed)
 244 +    {
 245 +        int prn_used[MAXCHANNELS];
 246 +        system::error(0,"info: Got GPS satellite solution (%d)\n",
 247 +                      status->satellites_used);
 248 +        for (int i = 0; i < status->satellites; ++i)
 249 +        {
 250 +            prn_used[i] = status->PRN[i];
 251 +        }
 252 +        newSolutionValue(status->used, status->satellites_used);
 253 +        status->fix_quality_stamp.changed = 0;
 254      }
 255 +    return 0;
 256  }
 257
 258 -int gpsInput()
 259 +static struct gps_data_t *status = NULL;
 260 +
 261 +int gpsInput(void *data)
 262  {
 263 -    while(1)
 264 +    if (gps_poll(status))
 265      {
 266 -       uint8_t c;
 267 -       int r= system::readSocket(gpsSocket(),&c,1);
 268 -       if(r==1) gpsCharInput(c);
 269 -       else break;
 270 +        return process_status(status);
 271      }
 272      return 0;
 273  }
 274
 275 -void *gpsSocket()
 276 +
 277 +void *gpsSocket(void)
 278  {
 279      static int init= 0;
 280 -    static void *socket;
 281      if(init==0)
 282      {
 283 -       int port;
 284 -       const char *sport= getPort();
 285 -       const char *server= getServer();
 286 -       if(1!=system::sscanf(sport,"%d",&port)) port= 2222;
 287 -       socket= system::makeSocket(server,port);
 288 -       if(socket==(void*)-1)   system::error(0,"warning: can't connect to GPS server %s:%d\n",server,port);
 289 -       else                    system::error(0,"info: Connected to GPS server %s:%d\n",server,port);
 290 -       init= 1;
 291 +       char *port= (char*)getPort();
 292 +       char *server= (char*)getServer();
 293 +       status = gps_open(server, DEFAULT_GPSD_PORT);
 294 +       if(status==NULL)
 295 +           system::error(0,"warning: can't connect to GPS server %s:%s\n",server,port);
 296 +       else
 297 +       {
 298 +           system::error(0,"info: Connected to GPS server %s:%s\n",server,port);
 299 +           gps_query(status, "w+x\n");
 300 +           init= 1;
 301 +       }
 302      }
 303 -    return socket;
 304 +    if (status)
 305 +        return (void*)status->gps_fd;
 306 +    else
 307 +        return (void*)-1;
 308  }
 309
 310 +/* gpsd do not accept input, and can't handle talkback.  Disabled. */
 311  void gpsOutput(
 312  const char *msg
 313  )
 314  {
 315 -    uint8_t buf[160];
 316 -    system::sprintf((char*)buf,"$PMGNCMD,%s*XX\r\n",msg);
 317 -
 318 -    uint8_t sum= 0;
 319 -    uint8_t *p= buf+1;
 320 -    while(1)
 321 -    {
 322 -       uint8_t c= *(p++);
 323 -       if(c=='*') break;
 324 -       sum^= c;
 325 -    }
 326 -
 327 -    static const uint8_t *tohex= (const uint8_t *)"0123456789ABCDEF";
 328 -    p[0]= tohex[(sum>>4)&0xF];
 329 -    p[1]= tohex[(sum>>0)&0xF];
 330 -
 331 -    int n= 4+p-buf;
 332 -    while(n)
 333 -    {
 334 -       int r= system::writeSocket(gpsSocket(),buf,n);
 335 -       if(r>=0) n-= r;
 336 -       else if(system::lastError()!=system::eAgain && system::lastError()!=system::eInterrupt)
 337 -       {
 338 -           system::error(0,"warning: can't write to gps socket\n");
 339 -           break;
 340 -       }
 341 -    }
 342  }
 343
 344  void gpsLoadTrackFile(
 345 diff -ur gps3d-1.20/viz_satellite.cpp gps3d-1.20-pere/viz_satellite.cpp
 346 --- gps3d-1.20/viz_satellite.cpp        Fri Nov 24 01:19:49 2000
 347 +++ gps3d-1.20-pere/viz_satellite.cpp   Sat Jan  1 23:49:05 2005
 348 @@ -4,14 +4,15 @@
 349  #include <viz_system.h>
 350
 351  #define TRAJSIZE 10000
 352 -static int trajSizes[50];
 353 -static int trajColor[50];
 354 -static double lastSatAzimuth[50];
 355 -static double lastSatElevation[50];
 356 -static double satX[50][TRAJSIZE];
 357 -static double satY[50][TRAJSIZE];
 358 -static double satZ[50][TRAJSIZE];
 359 -static double satT[50][TRAJSIZE];
 360 +#define MAXSATELLITES 50
 361 +static int trajSizes[MAXSATELLITES];
 362 +static int trajColor[MAXSATELLITES];
 363 +static double lastSatAzimuth[MAXSATELLITES];
 364 +static double lastSatElevation[MAXSATELLITES];
 365 +static double satX[MAXSATELLITES][TRAJSIZE];
 366 +static double satY[MAXSATELLITES][TRAJSIZE];
 367 +static double satZ[MAXSATELLITES][TRAJSIZE];
 368 +static double satT[MAXSATELLITES][TRAJSIZE];
 369
 370  // earthRadius = 6 378 136 meters
 371  // GPS satellites orbit at 20 180 000 meters altitude (from ground)= 26558136 from earth center
 372 @@ -52,14 +53,30 @@
 373      return valid;
 374  }
 375
 376 +void newSolutionValue(const int prn_used[], int nFields)
 377 +{
 378 +    int tj[MAXSATELLITES];
 379 +    system::memset(tj,0,sizeof(tj));
 380 +    if(nFields>15) nFields=15;
 381 +    for(int j=3; j<nFields; ++j)
 382 +    {
 383 +        tj[prn_used[j]] = 1;
 384 +    }
 385 +
 386 +    if(system::memcmp(trajColor,tj,sizeof(tj)))
 387 +    {
 388 +       system::memcpy(trajColor,tj,sizeof(tj));
 389 +       trash();
 390 +    }
 391 +}
 392 +
 393  void newSolution(
 394  const uint8_t  sol[][81],
 395  int            nFields
 396  )
 397  {
 398 -    int tj[50];
 399 -    int size= 50*sizeof(int);
 400 -    system::memset(tj,0,size);
 401 +    int tj[MAXSATELLITES];
 402 +    system::memset(tj,0,sizeof(tj));
 403      if(nFields>15) nFields=15;
 404      for(int j=3; j<nFields; ++j)
 405      {
 406 @@ -70,9 +87,9 @@
 407         }
 408      }
 409
 410 -    if(system::memcmp(trajColor,tj,size))
 411 +    if(system::memcmp(trajColor,tj,sizeof(tj)))
 412      {
 413 -       system::memcpy(trajColor,tj,size);
 414 +       system::memcpy(trajColor,tj,sizeof(tj));
 415         trash();
 416      }
 417  }
 418 @@ -106,30 +123,18 @@
 419      return sphereIntersect(result,0,aSquared,r,d);
 420  }
 421
 422 -void newSatelliteFix(
 423 -const uint8_t  *prn,
 424 -const uint8_t  *elevation,
 425 -const uint8_t  *azimuth,
 426 -const uint8_t  *
 427 -)
 428 +void newSatelliteFixValue(int prn, double elevation, double azimuth)
 429  {
 430 -    int num;
 431 -    double e;
 432 -    double a;
 433 -    if(lastStamp()<0) return;
 434 -    if(1!=system::sscanf((const char*)prn,"%d",&num)) return;
 435 -    if(1!=system::sscanf((const char*)elevation,"%lf",&e)) return;
 436 -    if(1!=system::sscanf((const char*)azimuth,"%lf",&a)) return;
 437
 438 -    if(e>90.0) return;
 439 -    if(a>360.0) return;
 440 -    if(num>=50) return;
 441 +    if(elevation>90.0) return;
 442 +    if(azimuth>360.0) return;
 443 +    if(prn>=MAXSATELLITES) return;
 444
 445 -    int pnum= trajSizes[num];
 446 +    int pnum= trajSizes[prn];
 447      if(
 448         pnum==0                         ||
 449 -       lastSatAzimuth[num]!=a          ||
 450 -       lastSatElevation[num]!=e
 451 +       lastSatAzimuth[prn]!=azimuth            ||
 452 +       lastSatElevation[prn]!=elevation
 453      )
 454      {
 455         double sat[3];
 456 @@ -138,23 +143,40 @@
 457         int correct= ae2xyz(
 458             la,
 459             lo,
 460 -           a,
 461 -           e,
 462 +           azimuth,
 463 +           elevation,
 464             altitudeSquared,
 465             sat
 466         );
 467         if(correct)
 468         {
 469             int mod= pnum%TRAJSIZE;
 470 -           satX[num][mod]= sat[0];
 471 -           satY[num][mod]= sat[1];
 472 -           satZ[num][mod]= sat[2];
 473 -           satT[num][mod]= lastStamp();
 474 -           trajSizes[num]= ++pnum;
 475 +           satX[prn][mod]= sat[0];
 476 +           satY[prn][mod]= sat[1];
 477 +           satZ[prn][mod]= sat[2];
 478 +           satT[prn][mod]= lastStamp();
 479 +           trajSizes[prn]= ++pnum;
 480             trash();
 481         }
 482      }
 483  }
 484 +void newSatelliteFix(
 485 +const uint8_t  *prn,
 486 +const uint8_t  *elevation,
 487 +const uint8_t  *azimuth,
 488 +const uint8_t  *
 489 +)
 490 +{
 491 +    int num;
 492 +    double e;
 493 +    double a;
 494 +    if(lastStamp()<0) return;
 495 +    if(1!=system::sscanf((const char*)prn,"%d",&num)) return;
 496 +    if(1!=system::sscanf((const char*)elevation,"%lf",&e)) return;
 497 +    if(1!=system::sscanf((const char*)azimuth,"%lf",&a)) return;
 498 +
 499 +    newSatelliteFixValue(num, e, a);
 500 +}
 501
 502  static inline void satPos(
 503  int    i,
 504 @@ -169,7 +191,7 @@
 505
 506  void paintSatellites()
 507  {
 508 -    for(int i=0;i<50;++i)
 509 +    for(int i=0;i<MAXSATELLITES;++i)
 510      {
 511         int start= 0;
 512         int nPoints= trajSizes[i];