← Back to branch summary

~bilalakhtar/ubuntu/maverick/gpsdrive/gpsdrive-fix-325288

« back to all changes in this revision

Viewing changes to src/em.c

  • Committer: Bazaar Package Importer
  • Author(s): Frank Kirschner
  • Date: 2004-05-25 11:44:03 UTC
  • Revision ID: james.westby@ubuntu.com-20040525114403-j3rsu57cavfax6z8
Tags: upstream-2.09
Import upstream version 2.09

Show diffs side-by-side

added added

removed removed

Lines of Context:
src/em.c
 
1
#include "config.h"
 
2
#include <stdio.h>
 
3
#include <stdlib.h>
 
4
#include <math.h>
 
5
#include <sys/types.h>
 
6
#include <sys/stat.h>
 
7
#include <sys/socket.h>
 
8
#include <time.h>
 
9
#include <fcntl.h>
 
10
#include <netinet/in.h>
 
11
#include <syslog.h>
 
12
#include <unistd.h>
 
13
#include <string.h>
 
14
 
 
15
#include "gpsd.h"
 
16
#include "nmea.h"
 
17
#define BUFSIZE 4096
 
18
 
 
19
extern int debug;
 
20
extern char *latitude;
 
21
extern char *longitude;
 
22
extern char latd;
 
23
extern char lond;
 
24
 
 
25
 
 
26
#define PI 3.14159265358979323846
 
27
 
 
28
enum
 
29
{
 
30
  EM_HUNT_FF, EM_HUNT_81, EM_HUNT_ID, EM_HUNT_WC,
 
31
  EM_HUNT_FLAGS, EM_HUNT_CS, EM_HUNT_DATA, EM_HUNT_A
 
32
};
 
33
 
 
34
#define O(x) (x-6)
 
35
 
 
36
static unsigned short sn = 0;
 
37
static int eminit;
 
38
 
 
39
struct header
 
40
{
 
41
  unsigned short sync;
 
42
  unsigned short id;
 
43
  unsigned short ndata;
 
44
  unsigned short flags;
 
45
  unsigned short csum;
 
46
};
 
47
 
 
48
static void analyze (struct header *, unsigned short *, fd_set *, fd_set *);
 
49
 
 
50
unsigned short
 
51
em_checksum (unsigned short *w, int n)
 
52
{
 
53
  unsigned short csum = 0;
 
54
 
 
55
  while (n--)
 
56
    csum += *(w++);
 
57
  csum = -csum;
 
58
  return csum;
 
59
}
 
60
 
 
61
/* em_spew - Takes a message type, an array of data words, and a length
 
62
   for the array, and prepends a 5 word header (including checksum).
 
63
   The data words are expected to be checksummed */
 
64
 
 
65
static void
 
66
em_spew (int type, void *dat, int dlen)
 
67
{
 
68
  struct header h;
 
69
 
 
70
  h.flags = 0;
 
71
 
 
72
  h.sync = 0x81ff;
 
73
  h.id = type;
 
74
  h.ndata = dlen - 1;
 
75
  h.csum = em_checksum ((unsigned short *) &h, 4);
 
76
 
 
77
  write (gNMEAdata.fdout, &h, sizeof (h));
 
78
  write (gNMEAdata.fdout, dat, sizeof (unsigned short) * dlen);
 
79
}
 
80
 
 
81
static long
 
82
putlong (char *dm, int sign)
 
83
{
 
84
  double tmpl;
 
85
  long rad;
 
86
 
 
87
  tmpl = fabs (atof (dm));
 
88
 
 
89
  rad =
 
90
    (floor (tmpl / 100) + (fmod (tmpl, 100.0) / 60)) * 100000000 * PI / 180;
 
91
 
 
92
  if (sign)
 
93
    rad = -rad;
 
94
 
 
95
  return rad;
 
96
}
 
97
 
 
98
static void
 
99
em_init ()
 
100
{
 
101
  unsigned short data[22];
 
102
  time_t t;
 
103
  struct tm *tm;
 
104
 
 
105
  eminit = 0;
 
106
 
 
107
  if (latitude && longitude)
 
108
    {
 
109
      t = time (NULL);
 
110
      tm = gmtime (&t);
 
111
 
 
112
      if (sn++ > 32767)
 
113
        sn = 0;
 
114
 
 
115
      memset (data, 0, sizeof (data));
 
116
 
 
117
      data[0] = sn;             /* sequence number */
 
118
 
 
119
      data[1] = (1 << 2) | (1 << 3);
 
120
      data[2] = data[3] = data[4] = 0;
 
121
      data[5] = tm->tm_mday;
 
122
      data[6] = tm->tm_mon + 1;
 
123
      data[7] = tm->tm_year + 1900;
 
124
      data[8] = tm->tm_hour;
 
125
      data[9] = tm->tm_min;
 
126
      data[10] = tm->tm_sec;
 
127
      *(long *) (data + 11) = putlong (latitude, (latd == 'S') ? 1 : 0);
 
128
      *(long *) (data + 13) = putlong (longitude, (lond == 'W') ? 1 : 0);
 
129
      data[15] = data[16] = 0;
 
130
      data[17] = data[18] = data[19] = data[20] = 0;
 
131
      data[21] = em_checksum (data, 21);
 
132
 
 
133
      em_spew (1200, &data, 22);
 
134
    }
 
135
}
 
136
 
 
137
void
 
138
em_send_rtcm (unsigned short *rtcmbuf, int rtcmbytes)
 
139
{
 
140
  unsigned short data[34];
 
141
  int n = 1 + (rtcmbytes / 2 + rtcmbytes % 2);
 
142
 
 
143
  if (sn++ > 32767)
 
144
    sn = 0;
 
145
 
 
146
  memset (data, 0, sizeof (data));
 
147
 
 
148
  data[0] = sn;                 /* sequence number */
 
149
  memcpy (&data[1], rtcmbuf, rtcmbytes * (sizeof (char)));
 
150
  data[n] = em_checksum (data, n);
 
151
 
 
152
  em_spew (1351, &data, n + 1);
 
153
}
 
154
 
 
155
void
 
156
do_eminit ()
 
157
{
 
158
  /* Make sure these are zero before 1002 handler called */
 
159
  gNMEAdata.pdop = gNMEAdata.hdop = gNMEAdata.vdop = 0;
 
160
  eminit = 1;
 
161
}
 
162
 
 
163
static long
 
164
getlong (void *p)
 
165
{
 
166
  return *(long *) p;
 
167
}
 
168
 
 
169
 
 
170
static unsigned long
 
171
getulong (void *p)
 
172
{
 
173
  return *(unsigned long *) p;
 
174
}
 
175
 
 
176
 
 
177
static double
 
178
degtodm (double a)
 
179
{
 
180
  double d, m, t;
 
181
 
 
182
  d = floor (a);
 
183
  m = modf (a, &t);
 
184
  t = d * 100 + m * 60;
 
185
  return t;
 
186
}
 
187
 
 
188
static void
 
189
handle1000 (unsigned short *p)
 
190
{
 
191
#if 0
 
192
  fprintf (stderr, "date: %d %d %d  %d:%d:%d\n",
 
193
           p[O (19)], p[O (20)], p[O (21)], p[O (22)], p[O (23)], p[O (24)]);
 
194
 
 
195
  fprintf (stderr, "  solution invalid:\n");
 
196
  fprintf (stderr, "    altitude: %d\n", (p[O (10)] & 1) ? 1 : 0);
 
197
  fprintf (stderr, "    no diff gps: %d\n", (p[O (10)] & 2) ? 1 : 0);
 
198
  fprintf (stderr, "    not enough satellites: %d\n",
 
199
           (p[O (10)] & 4) ? 1 : 0);
 
200
  fprintf (stderr, "    exceed max EHPE: %d\n", (p[O (10)] & 8) ? 1 : 0);
 
201
  fprintf (stderr, "    exceed max EVPE: %d\n", (p[O (10)] & 16) ? 1 : 0);
 
202
  fprintf (stderr, "  solution type:\n");
 
203
  fprintf (stderr, "    propagated: %d\n", (p[O (11)] & 1) ? 1 : 0);
 
204
  fprintf (stderr, "    altitude: %d\n", (p[O (11)] & 2) ? 1 : 0);
 
205
  fprintf (stderr, "    differential: %d\n", (p[O (11)] & 4) ? 1 : 0);
 
206
  fprintf (stderr, "Number of measurements in solution: %d\n", p[O (12)]);
 
207
  fprintf (stderr, "Lat: %f\n",
 
208
           180.0 / (PI / ((double) getlong (p + O (27)) / 100000000)));
 
209
  fprintf (stderr, "Lon: %f\n",
 
210
           180.0 / (PI / ((double) getlong (p + O (29)) / 100000000)));
 
211
  fprintf (stderr, "Alt: %f\n", (double) getlong (p + O (31)) / 100.0);
 
212
  fprintf (stderr, "Speed: %f\n",
 
213
           (double) getlong (p + O (34)) / 100.0) * 1.94387;
 
214
  fprintf (stderr, "Map datum: %d\n", p[O (39)]);
 
215
  fprintf (stderr, "Magnetic variation: %f\n",
 
216
           p[O (37)] * 180 / (PI * 10000));
 
217
  fprintf (stderr, "Course: %f\n", (p[O (36)] * 180 / (PI * 1000)));
 
218
  fprintf (stderr, "Separation: %f\n", (p[O (33)] / 100));
 
219
#endif
 
220
 
 
221
  sprintf (gNMEAdata.utc, "%02d/%02d/%d %02d:%02d:%02d",
 
222
           p[O (19)], p[O (20)], p[O (21)], p[O (22)], p[O (23)], p[O (24)]);
 
223
 
 
224
  gNMEAdata.mag_var = p[O (37)] * 180 / (PI * 10000);   /* degrees */
 
225
 
 
226
  gNMEAdata.course = p[O (36)] * 180 / (PI * 1000);     /* degrees */
 
227
 
 
228
  gNMEAdata.satellites = p[O (12)];
 
229
 
 
230
  gNMEAdata.hours = p[O (22)];
 
231
 
 
232
  gNMEAdata.minutes = p[O (23)];
 
233
 
 
234
  gNMEAdata.seconds = p[O (24)];
 
235
 
 
236
  gNMEAdata.year = p[O (21)];
 
237
 
 
238
  gNMEAdata.month = p[O (20)];
 
239
 
 
240
  gNMEAdata.day = p[O (19)];
 
241
 
 
242
  gNMEAdata.latitude =
 
243
    180.0 / (PI / ((double) getlong (p + O (27)) / 100000000));
 
244
  gNMEAdata.longitude =
 
245
    180.0 / (PI / ((double) getlong (p + O (29)) / 100000000));
 
246
  gNMEAdata.speed = ((double) getulong (p + O (34)) / 100.0) * 1.94387;
 
247
  gNMEAdata.altitude = (double) getlong (p + O (31)) / 100.0;
 
248
 
 
249
  gNMEAdata.status = (p[O (10)] & 0x1c) ? 0 : 1;
 
250
 
 
251
  if (gNMEAdata.status)
 
252
    {
 
253
      gNMEAdata.mode = (p[O (10)] & 1) ? 2 : 3;
 
254
      if (p[O(11)] & 0x4) gNMEAdata.status = 2;
 
255
    }
 
256
  else
 
257
    {
 
258
      gNMEAdata.mode = 1;
 
259
    }
 
260
 
 
261
  gNMEAdata.separation = p[O (33)] / 100;       /* meters */
 
262
 
 
263
}
 
264
 
 
265
static void
 
266
handle1002 (unsigned short *p)
 
267
{
 
268
  int i, j;
 
269
 
 
270
  gNMEAdata.ZCHseen = 1;
 
271
  for (j = 0; j < 12; j++)
 
272
    {
 
273
      gNMEAdata.used[j] = 0;
 
274
    }
 
275
  for (i = 0; i < 12; i++)
 
276
    {
 
277
      gNMEAdata.Zs[i] = p[O (16 + (3 * i))];
 
278
      gNMEAdata.Zv[i] = (p[O (15 + (3 * i))] & 0xf);
 
279
#if 0
 
280
      fprintf (stderr, "Sat%02d:", i);
 
281
      fprintf (stderr, " used:%d", (p[O (15 + (3 * i))] & 1) ? 1 : 0);
 
282
      fprintf (stderr, " eph:%d", (p[O (15 + (3 * i))] & 2) ? 1 : 0);
 
283
      fprintf (stderr, " val:%d", (p[O (15 + (3 * i))] & 4) ? 1 : 0);
 
284
      fprintf (stderr, " dgps:%d", (p[O (15 + (3 * i))] & 8) ? 1 : 0);
 
285
      fprintf (stderr, " PRN:%d", p[O (16 + (3 * i))]);
 
286
      fprintf (stderr, " C/No:%d\n", p[O (17 + (3 * i))]);
 
287
#endif
 
288
      for (j = 0; j < 12; j++)
 
289
        {
 
290
          if (gNMEAdata.PRN[j] != p[O (16 + (3 * i))])
 
291
            continue;
 
292
          gNMEAdata.used[j] = (p[O (15 + (3 * i))] & 1);
 
293
          gNMEAdata.ss[j] = p[O (17 + (3 * i))];
 
294
          break;
 
295
        }
 
296
    }
 
297
}
 
298
 
 
299
static void
 
300
handle1003 (unsigned short *p)
 
301
{
 
302
  int  j;
 
303
 
 
304
  gNMEAdata.pdop = p[O (10)];
 
305
  gNMEAdata.hdop = p[O (11)];
 
306
  gNMEAdata.vdop = p[O (12)];
 
307
  gNMEAdata.in_view = p[O (14)];
 
308
 
 
309
  for (j = 0; j < 12; j++)
 
310
    {
 
311
      if (j < gNMEAdata.in_view)
 
312
        {
 
313
          gNMEAdata.PRN[j] = p[O (15 + (3 * j))];
 
314
          gNMEAdata.azimuth[j] = p[O (16 + (3 * j))] * 180 / (PI * 10000);
 
315
          gNMEAdata.elevation[j] = p[O (17 + (3 * j))] * 180 / (PI * 10000);
 
316
#if 0
 
317
          fprintf (stderr, "Sat%02d:", i);
 
318
          fprintf (stderr, " PRN:%d", p[O (15 + (3 * i))]);
 
319
          fprintf (stderr, " az:%d", p[O (16 + (3 * i))]);
 
320
          fprintf (stderr, " el:%d", p[O (17 + (3 * i))]);
 
321
          fprintf (stderr, "\n");
 
322
#endif
 
323
        }
 
324
      else
 
325
        {
 
326
          gNMEAdata.PRN[j] = 0;
 
327
          gNMEAdata.azimuth[j] = 0.0;
 
328
          gNMEAdata.elevation[j] = 0.0;
 
329
        }
 
330
    }
 
331
}
 
332
 
 
333
static void
 
334
handle1005 (unsigned short *p)
 
335
{
 
336
  int i;
 
337
  int numcorrections = p[O (12)];
 
338
#if 1
 
339
  fprintf (stderr, "Station bad: %d\n", (p[O (9)] & 1) ? 1 : 0);
 
340
  fprintf (stderr, "User disabled: %d\n", (p[O (9)] & 2) ? 1 : 0);
 
341
  fprintf (stderr, "Station ID: %d\n", p[O (10)]);
 
342
  fprintf (stderr, "Age of last correction in seconds: %d\n", p[O (11)]);
 
343
  fprintf (stderr, "Number of corrections: %d\n", p[O (12)]);
 
344
  for (i = 0; i < numcorrections; i++)
 
345
    {
 
346
      fprintf (stderr, "Sat%02d:", p[O (13 + i)] & 0x3f);
 
347
      fprintf (stderr, "ephemeris:%d", (p[O (13 + i)] & 64) ? 1 : 0);
 
348
      fprintf (stderr, "rtcm corrections:%d", (p[O (13 + i)] & 128) ? 1 : 0);
 
349
      fprintf (stderr, "rtcm udre:%d", (p[O (13 + i)] & 256) ? 1 : 0);
 
350
      fprintf (stderr, "sat health:%d", (p[O (13 + i)] & 512) ? 1 : 0);
 
351
      fprintf (stderr, "rtcm sat health:%d", (p[O (13 + i)] & 1024) ? 1 : 0);
 
352
      fprintf (stderr, "corrections state:%d",
 
353
               (p[O (13 + i)] & 2048) ? 1 : 0);
 
354
      fprintf (stderr, "iode mismatch:%d", (p[O (13 + i)] & 4096) ? 1 : 0);
 
355
    }
 
356
#endif
 
357
}
 
358
 
 
359
static void
 
360
analyze (struct header *h, unsigned short *p, fd_set * afds,
 
361
         fd_set * nmea_fds)
 
362
{
 
363
  unsigned char buf[BUFSIZE];
 
364
  char *bufp;
 
365
  char *bufp2;
 
366
  int i = 0, j = 0,  nmea = 0;
 
367
  int fd;
 
368
  extern int nfds;
 
369
 
 
370
  if (p[h->ndata] == em_checksum (p, h->ndata))
 
371
    {
 
372
      if (debug > 5)
 
373
        fprintf (stderr, "id %d\n", h->id);
 
374
      switch (h->id)
 
375
        {
 
376
        case 1000:
 
377
          handle1000 (p);
 
378
          bufp = buf;
 
379
          if (gNMEAdata.mode > 1)
 
380
            {
 
381
              sprintf (bufp,
 
382
                        "$GPGGA,%02d%02d%02d,%09.4lf,%c,%010.4lf,%c,%d,%02d,%.2f,%.1f,%c,%f,%c,%s,%s*",
 
383
                       gNMEAdata.hours, gNMEAdata.minutes, gNMEAdata.seconds,
 
384
                       degtodm (fabs (gNMEAdata.latitude)),
 
385
                       ((gNMEAdata.latitude > 0) ? 'N' : 'S'),
 
386
                       degtodm (fabs (gNMEAdata.longitude)),
 
387
                       ((gNMEAdata.longitude > 0) ? 'E' : 'W'),
 
388
                    gNMEAdata.status, gNMEAdata.satellites, gNMEAdata.hdop,
 
389
                       gNMEAdata.altitude, 'M', gNMEAdata.separation, 'M', "",
 
390
                       "");
 
391
              add_checksum (bufp + 1);
 
392
              bufp = bufp + strlen (bufp);
 
393
            }
 
394
          sprintf (bufp,
 
395
                    "$GPRMC,%02d%02d%02d,%c,%09.4lf,%c,%010.4lf,%c,%f,%f,%02d%02d%02d,%02f,%c*",
 
396
                   gNMEAdata.hours, gNMEAdata.minutes, gNMEAdata.seconds,
 
397
                   gNMEAdata.status ? 'A' : 'V',
 
398
                   degtodm (fabs (gNMEAdata.latitude)),
 
399
                   ((gNMEAdata.latitude > 0) ? 'N' : 'S'),
 
400
                   degtodm (fabs (gNMEAdata.longitude)),
 
401
                   ((gNMEAdata.longitude > 0) ? 'E' : 'W'), gNMEAdata.speed,
 
402
                   gNMEAdata.course, gNMEAdata.day, gNMEAdata.month,
 
403
                   (gNMEAdata.year % 100), gNMEAdata.mag_var,
 
404
                   (gNMEAdata.mag_var > 0) ? 'E' : 'W');
 
405
          add_checksum (bufp + 1);
 
406
          nmea = 1000;
 
407
          break;
 
408
        case 1002:
 
409
          handle1002 (p);
 
410
          bufp2 = bufp = buf;
 
411
          sprintf (bufp, "$GPGSA,%c,%d,", 'A', gNMEAdata.mode);
 
412
          j = 0;
 
413
          for (i = 0; i < 12; i++)
 
414
            {
 
415
              if (gNMEAdata.used[i])
 
416
                {
 
417
                  bufp = bufp + strlen (bufp);
 
418
                  sprintf (bufp, "%02d,", gNMEAdata.PRN[i]);
 
419
                  j++;
 
420
                }
 
421
            }
 
422
          for (i = j; i < 12; i++)
 
423
            {
 
424
              bufp = bufp + strlen (bufp);
 
425
              sprintf (bufp, ",");
 
426
            }
 
427
          bufp = bufp + strlen (bufp);
 
428
          sprintf (bufp, "%.2f,%.2f,%.2f*", gNMEAdata.pdop, gNMEAdata.hdop,
 
429
                   gNMEAdata.vdop);
 
430
          add_checksum (bufp2 + 1);
 
431
          bufp2 = bufp = bufp + strlen (bufp);
 
432
          sprintf (bufp, "$PRWIZCH");
 
433
          bufp = bufp + strlen (bufp);
 
434
          for (i = 0; i < 12; i++)
 
435
            {
 
436
              sprintf (bufp, ",%02d,%X", gNMEAdata.Zs[i], gNMEAdata.Zv[i]);
 
437
              bufp = bufp + strlen (bufp);
 
438
            }
 
439
          sprintf (bufp, "*");
 
440
          bufp = bufp + strlen (bufp);
 
441
          add_checksum (bufp2 + 1);
 
442
          nmea = 1002;
 
443
          break;
 
444
        case 1003:
 
445
          handle1003 (p);
 
446
          bufp2 = bufp = buf;
 
447
          j =
 
448
            (gNMEAdata.in_view / 4) + (((gNMEAdata.in_view % 4) > 0) ? 1 : 0);
 
449
          while (i < 12)
 
450
            {
 
451
              if (i % 4 == 0)
 
452
                sprintf (bufp, "$GPGSV,%d,%d,%02d", j, (i / 4) + 1,
 
453
                         gNMEAdata.in_view);
 
454
              bufp += strlen (bufp);
 
455
              if (i <= gNMEAdata.in_view && gNMEAdata.elevation[i])
 
456
                sprintf (bufp, ",%02d,%02d,%03d,%02d", gNMEAdata.PRN[i],
 
457
                         gNMEAdata.elevation[i], gNMEAdata.azimuth[i],
 
458
                         gNMEAdata.ss[i]);
 
459
              else
 
460
                sprintf (bufp, ",%02d,00,000,%02d,", gNMEAdata.PRN[i],
 
461
                         gNMEAdata.ss[i]);
 
462
              bufp += strlen (bufp);
 
463
              if (i % 4 == 3)
 
464
                {
 
465
                  sprintf (bufp, "*");
 
466
                  add_checksum (bufp2 + 1);
 
467
                  bufp += strlen (bufp);
 
468
                  bufp2 = bufp;
 
469
                }
 
470
              i++;
 
471
            }
 
472
          nmea = 1003;
 
473
          break;
 
474
        case 1005:
 
475
          handle1005 (p);
 
476
          break;
 
477
        }
 
478
    }
 
479
  if (nmea > 0)
 
480
    {
 
481
      if (debug > 4)
 
482
        fprintf (stderr, "%s", buf);
 
483
      for (fd = 0; fd < nfds; fd++)
 
484
        if (FD_ISSET (fd, nmea_fds))
 
485
          if (write (fd, buf, strlen (buf)) < 0)
 
486
            {
 
487
              FD_CLR (fd, afds);
 
488
              FD_CLR (fd, nmea_fds);
 
489
            }
 
490
    }
 
491
  if (eminit)
 
492
    em_init ();
 
493
}
 
494
 
 
495
 
 
496
static int
 
497
putword (unsigned short *p, unsigned char c, unsigned int n)
 
498
{
 
499
  *(((unsigned char *) p) + n) = c;
 
500
  if (n == 0)
 
501
    return 1;
 
502
  else
 
503
    return 0;
 
504
}
 
505
 
 
506
 
 
507
static void
 
508
em_eat (unsigned char c, fd_set * afds, fd_set * nmea_fds)
 
509
{
 
510
  static int state = EM_HUNT_FF;
 
511
  static struct header h;
 
512
 
 
513
  static unsigned int byte;
 
514
  static unsigned int words;
 
515
  static unsigned short *data;
 
516
 
 
517
  switch (state)
 
518
    {
 
519
 
 
520
    case EM_HUNT_FF:
 
521
      if (c == 0xff)
 
522
        state = EM_HUNT_81;
 
523
      if (c == 'E')
 
524
        state = EM_HUNT_A;
 
525
      break;
 
526
 
 
527
    case EM_HUNT_A:
 
528
      /* A better be right after E */
 
529
      if (c == 'A')
 
530
        write (gNMEAdata.fdout, "EARTHA\r\n", 8);
 
531
      state = EM_HUNT_FF;
 
532
      break;
 
533
 
 
534
    case EM_HUNT_81:
 
535
      if (c == 0x81)
 
536
        state = EM_HUNT_ID;
 
537
      h.sync = 0x81ff;
 
538
      byte = 0;
 
539
      break;
 
540
 
 
541
    case EM_HUNT_ID:
 
542
      if (!(byte = putword (&(h.id), c, byte)))
 
543
        state = EM_HUNT_WC;
 
544
      break;
 
545
 
 
546
    case EM_HUNT_WC:
 
547
      if (!(byte = putword (&(h.ndata), c, byte)))
 
548
        state = EM_HUNT_FLAGS;
 
549
      break;
 
550
 
 
551
    case EM_HUNT_FLAGS:
 
552
      if (!(byte = putword (&(h.flags), c, byte)))
 
553
        state = EM_HUNT_CS;
 
554
      break;
 
555
 
 
556
    case EM_HUNT_CS:
 
557
      if (!(byte = putword (&(h.csum), c, byte)))
 
558
        {
 
559
 
 
560
          if (h.csum == em_checksum ((unsigned short *) &h, 4))
 
561
            {
 
562
              state = EM_HUNT_DATA;
 
563
              data = (unsigned short *) malloc ((h.ndata + 1) * 2);
 
564
              words = 0;
 
565
            }
 
566
          else
 
567
            state = EM_HUNT_FF;
 
568
        }
 
569
      break;
 
570
 
 
571
    case EM_HUNT_DATA:
 
572
      if (!(byte = putword (data + words, c, byte)))
 
573
        words++;
 
574
      if (words == h.ndata + 1)
 
575
        {
 
576
          analyze (&h, data, afds, nmea_fds);
 
577
          free (data);
 
578
          state = EM_HUNT_FF;
 
579
        }
 
580
      break;
 
581
    }
 
582
}
 
583
 
 
584
int
 
585
handle_EMinput (int input, fd_set * afds, fd_set * nmea_fds)
 
586
{
 
587
  unsigned char c;
 
588
 
 
589
  if (read (input, &c, 1) != 1)
 
590
    return 1;
 
591
  em_eat (c, afds, nmea_fds);
 
592
  return 0;
 
593
}