gpsfs.c 18 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299300301302303304305306307308309310311312313314315316317318319320321322323324325326327328329330331332333334335336337338339340341342343344345346347348349350351352353354355356357358359360361362363364365366367368369370371372373374375376377378379380381382383384385386387388389390391392393394395396397398399400401402403404405406407408409410411412413414415416417418419420421422423424425426427428429430431432433434435436437438439440441442443444445446447448449450451452453454455456457458459460461462463464465466467468469470471472473474475476477478479480481482483484485486487488489490491492493494495496497498499500501502503504505506507508509510511512513514515516517518519520521522523524525526527528529530531532533534535536537538539540541542543544545546547548549550551552553554555556557558559560561562563564565566567568569570571572573574575576577578579580581582583584585586587588589590591592593594595596597598599600601602603604605606607608609610611612613614615616617618619620621622623624625626627628629630631632633634635636637638639640641642643644645646647648649650651652653654655656657658659660661662663664665666667668669670671672673674675676677678679680681682683684685686687688689690691692693694695696697698699700701702703704705706707708709710711712713714715716717718719720721722723724725726727728729730731732733734735736737738739740741742743744745746747748749750751752753754755756757758759760761762763764765766767768769770771772773774775776777778779780781782783784785786787788789790791792793794795796797798799800801802803804805806807808809810811812813814815816817818819820821822823824825826827828829830831832833834835836837838839840841842843844845846847848849850851852853854855856857858859860861862863864865866867868869870871872873874875876877878879880881882883884885886887888889890891892893894895896897898899900901902903904905906907908909910911912913914915916917918919920921922923924925926927928929930931932933934935936937938939940941942943944945946947948949950951952953954955956957958959960961962963964965966967968969970971972973974
  1. /*
  2. * This file is part of the UCB release of Plan 9. It is subject to the license
  3. * terms in the LICENSE file found in the top-level directory of this
  4. * distribution and at http://akaros.cs.berkeley.edu/files/Plan9License. No
  5. * part of the UCB release of Plan 9, including this file, may be copied,
  6. * modified, propagated, or distributed except according to the terms contained
  7. * in the LICENSE file.
  8. */
  9. #include <u.h>
  10. #include <libc.h>
  11. #include <fcall.h>
  12. #include <thread.h>
  13. #include <ctype.h>
  14. #include <9p.h>
  15. #include "dat.h"
  16. enum
  17. {
  18. Numsize= 12,
  19. Vlnumsize= 22,
  20. Rawbuf= 0x10000,
  21. Rawmask= Rawbuf-1,
  22. };
  23. #define nsecperchar ((int)(1000000000.0 * 10.0 / baud))
  24. typedef struct Fix Fix;
  25. typedef struct Satellite Satellite;
  26. typedef struct GPSfile GPSfile;
  27. typedef struct Gpsmsg Gpsmsg;
  28. struct Satellite {
  29. int prn;
  30. int elevation;
  31. int azimuth;
  32. int snr;
  33. };
  34. struct Fix {
  35. int messages; /* bitmap of types seen */
  36. Place;
  37. /*
  38. * The following are in Plan 9 time format:
  39. * seconds or nanoseconds since the epoch.
  40. */
  41. int64_t localtime; /* nsec() value when first byte was read */
  42. int64_t gpstime; /* nsec() value from GPS */
  43. long time; /* time() value from GPS */
  44. double zulu;
  45. int date;
  46. char valid;
  47. unsigned char quality;
  48. uint16_t satellites;
  49. double pdop;
  50. double hdop;
  51. double vdop;
  52. double altitude;
  53. double sealevel;
  54. double groundspeed;
  55. double kmh;
  56. double course;
  57. double heading;
  58. double magvar;
  59. Satellite s[12];
  60. };
  61. struct GPSfile {
  62. char *name;
  63. char* (*rread)(Req*);
  64. int mode;
  65. int64_t offset; /* for raw: rawout - read-offset */
  66. };
  67. enum {
  68. ASTRAL,
  69. GPGGA,
  70. GPGLL,
  71. GPGSA,
  72. GPGSV,
  73. GPRMC,
  74. GPVTG,
  75. PRWIRID,
  76. PRWIZCH
  77. };
  78. struct Gpsmsg {
  79. char *name;
  80. int tokens;
  81. uint32_t errors;
  82. };
  83. char raw[Rawbuf];
  84. int64_t rawin;
  85. int64_t rawout;
  86. uint32_t badlat, goodlat, suspectlat;
  87. uint32_t badlon, goodlon, suspectlon;
  88. uint32_t suspecttime, goodtime;
  89. uint32_t histo[32];
  90. char *serial = "/dev/eia0";
  91. Gpsmsg gpsmsg[] = {
  92. [ASTRAL] = { "ASTRAL", 0, 0},
  93. [GPGGA] = { "$GPGGA", 15, 0},
  94. /* NMEA 2.3 permits optional 8th field, mode */
  95. [GPGLL] = { "$GPGLL", 7, 0},
  96. [GPGSA] = { "$GPGSA", 18, 0},
  97. [GPGSV] = { "$GPGSV", 0, 0},
  98. [GPRMC] = { "$GPRMC", 0, 0},
  99. [GPVTG] = { "$GPVTG", 0, 0},
  100. [PRWIRID] = { "$PRWIRID", 0, 0},
  101. [PRWIZCH] = { "$PRWIZCH", 0, 0},
  102. };
  103. int ttyfd, ctlfd, debug;
  104. int setrtc;
  105. int baud = Baud;
  106. char *baudstr = "b%dd1r1pns1l8i9";
  107. uint32_t seconds;
  108. uint32_t starttime;
  109. uint32_t checksumerrors;
  110. int gpsplayback; /* If set, return times and positions with `invalid' marker set */
  111. Place where = {-(74.0 + 23.9191/60.0), 40.0 + 41.1346/60.0};
  112. Fix curfix;
  113. Lock fixlock;
  114. int type(char*);
  115. void setline(void);
  116. int getonechar(int64_t*);
  117. void getline(char*, int, int64_t*);
  118. void putline(char*);
  119. int gettime(Fix*);
  120. int getzulu(char *, Fix*);
  121. int getalt(char*, char*, Fix*);
  122. int getsea(char*, char*, Fix*);
  123. int getlat(char*, char*, Fix*);
  124. int getlon(char*, char*, Fix*);
  125. int getgs(char*, Fix *);
  126. int getkmh(char*, Fix*);
  127. int getcrs(char*, Fix*);
  128. int gethdg(char*, Fix*);
  129. int getdate(char*, Fix*);
  130. int getmagvar(char*, char*, Fix*);
  131. void printfix(int, Fix*);
  132. void ropen(Req *r);
  133. void rread(Req *r);
  134. void rend(Srv *s);
  135. void gpsinit(void);
  136. char* readposn(Req*);
  137. char* readtime(Req*);
  138. char* readsats(Req*);
  139. char* readstats(Req*);
  140. char* readraw(Req*);
  141. GPSfile files[] = {
  142. { "time", readtime, 0444, 0 },
  143. { "position", readposn, 0444, 0 },
  144. { "satellites", readsats, 0444, 0 },
  145. { "stats", readstats, 0444, 0 },
  146. { "raw", readraw, DMEXCL|0444, 0 },
  147. };
  148. Srv s = {
  149. .open = ropen,
  150. .read = rread,
  151. .end = rend,
  152. };
  153. File *root;
  154. File *gpsdir;
  155. void
  156. rend(Srv *s)
  157. {
  158. sysfatal("gpsfs demised");
  159. }
  160. void
  161. ropen(Req *r)
  162. {
  163. respond(r, nil);
  164. }
  165. void
  166. rread(Req *r)
  167. {
  168. GPSfile *f;
  169. r->ofcall.count = 0;
  170. f = r->fid->file->aux;
  171. respond(r, f->rread(r));
  172. }
  173. void
  174. fsinit(void)
  175. {
  176. char* user;
  177. int i;
  178. user = getuser();
  179. s.tree = alloctree(user, user, 0555, nil);
  180. if(s.tree == nil)
  181. sysfatal("fsinit: alloctree: %r");
  182. root = s.tree->root;
  183. if((gpsdir = createfile(root, "gps", user, DMDIR|0555, nil)) == nil)
  184. sysfatal("fsinit: createfile: gps: %r");
  185. for(i = 0; i < nelem(files); i++)
  186. if(createfile(gpsdir, files[i].name, user, files[i].mode, files + i) == nil)
  187. sysfatal("fsinit: createfile: %s: %r", files[i].name);
  188. }
  189. void
  190. threadmain(int argc, char*argv[])
  191. {
  192. char *srvname, *mntpt;
  193. srvname = "gps";
  194. mntpt = "/mnt";
  195. ARGBEGIN {
  196. default:
  197. fprint(2, "usage: %s [-b baud] [-d device] [-l logfile] [-m mntpt] [-r] [-s postname]\n", argv0);
  198. exits("usage");
  199. case 'D':
  200. debug++;
  201. break;
  202. case 'b':
  203. baud = strtol(ARGF(), nil, 0);
  204. break;
  205. case 'd':
  206. serial = ARGF();
  207. break;
  208. case 'r':
  209. setrtc = 1;
  210. break;
  211. case 's':
  212. srvname = ARGF();
  213. break;
  214. case 'm':
  215. mntpt = ARGF();
  216. break;
  217. } ARGEND
  218. fmtinstall('L', placeconv);
  219. rfork(RFNOTEG);
  220. fsinit();
  221. gpsinit();
  222. threadpostmountsrv(&s, srvname, mntpt, MBEFORE);
  223. threadexits(nil);
  224. }
  225. static void
  226. gpstrack(void *v)
  227. {
  228. Fix fix;
  229. static char buf[256], *t[32];
  230. int n, i, k, tp;
  231. int64_t localtime;
  232. double d;
  233. setline();
  234. fix.messages = 0;
  235. fix.lon = 181.0;
  236. fix.lat = 91.0;
  237. fix.zulu = 0;
  238. fix.date = 0;
  239. fix.valid = 0;
  240. fix.quality = 0;
  241. fix.satellites = 0;
  242. fix.pdop = 0.0;
  243. fix.hdop = 0.0;
  244. fix.vdop = 0.0;
  245. fix.altitude = 0.0;
  246. fix.sealevel = 0.0;
  247. fix.groundspeed = 0.0;
  248. fix.kmh = 0.0;
  249. fix.course = 0.0;
  250. fix.heading = 0.0;
  251. fix.magvar = 0.0;
  252. for(;;){
  253. getline(buf, sizeof buf, &localtime);
  254. n = getfields(buf, t, nelem(t), 0,",\r\n");
  255. if(n == 0)
  256. continue;
  257. tp = type(t[0]);
  258. if(tp >= 0 && tp < nelem(gpsmsg) && gpsmsg[tp].tokens &&
  259. gpsmsg[tp].tokens > n){
  260. gpsmsg[tp].errors++;
  261. if(debug)
  262. fprint(2, "%s: Expect %d tokens, got %d\n",
  263. gpsmsg[tp].name, gpsmsg[tp].tokens, n);
  264. continue;
  265. }
  266. switch(tp){
  267. case ASTRAL:
  268. putline("$IIGPQ,ASTRAL*73");
  269. putline("$PRWIILOG,GGA,A,T,10,0");
  270. putline("$PRWIILOG,RMC,A,T,10,0");
  271. putline("$PRWIILOG,GSA,A,T,10,0");
  272. putline("$PRWIILOG,GSV,V,,,");
  273. fprint(2, "Reply: %s\n", "$IIGPQ,ASTRAL*73");
  274. break;
  275. case PRWIRID:
  276. case PRWIZCH:
  277. for(i = 0; i < n; i++) fprint(2, "%s,", t[i]);
  278. fprint(2, "(%d tokens)\n", n);
  279. break;
  280. case GPGGA:
  281. if(getlat(t[2], t[3], &fix))
  282. break;
  283. if(getlon(t[4], t[5], &fix))
  284. break;
  285. getzulu(t[1], &fix);
  286. if(fix.date && gettime(&fix))
  287. break;
  288. if(isdigit(*t[7]))
  289. fix.satellites = strtol(t[7], nil, 10);
  290. if(isdigit(*t[8])){
  291. d = strtod(t[8], nil);
  292. if(!isNaN(d))
  293. fix.hdop = d;
  294. }
  295. getalt(t[9], t[10], &fix);
  296. getsea(t[11], t[12], &fix);
  297. fix.localtime = localtime;
  298. fix.quality = strtol(t[6], nil, 10);
  299. fix.messages |= 1 << tp;
  300. break;
  301. case GPRMC:
  302. fix.valid = *t[2];
  303. getgs(t[7], &fix);
  304. getcrs(t[8], &fix);
  305. getdate(t[9], &fix);
  306. getmagvar(t[10], t[11], &fix);
  307. if((fix.messages & (1 << GPGGA)) == 0){
  308. if(getlat(t[3], t[4], &fix))
  309. break;
  310. if(getlon(t[5], t[6], &fix))
  311. break;
  312. fix.localtime = localtime;
  313. getzulu(t[1], &fix);
  314. if(fix.date)
  315. gettime(&fix);
  316. }
  317. fix.messages |= 1 << tp;
  318. break;
  319. case GPGSA:
  320. if(*t[15]){
  321. d = strtod(t[15], nil);
  322. if(!isNaN(d))
  323. fix.pdop = d;
  324. }
  325. if(*t[16]){
  326. d = strtod(t[16], nil);
  327. if(!isNaN(d))
  328. fix.hdop = d;
  329. }
  330. if(*t[17]){
  331. d = strtod(t[17], nil);
  332. if(!isNaN(d))
  333. fix.vdop = d;
  334. }
  335. fix.messages |= 1 << tp;
  336. break;
  337. case GPGLL:
  338. if(getlat(t[1], t[2], &fix))
  339. break;
  340. if(getlon(t[3], t[4], &fix))
  341. break;
  342. getzulu(t[5], &fix);
  343. fix.messages |= 1 << tp;
  344. break;
  345. case GPGSV:
  346. if(n < 8){
  347. gpsmsg[tp].errors++;
  348. if(debug)
  349. fprint(2, "%s: Expect at least 8 tokens, got %d\n",
  350. gpsmsg[tp].name, n);
  351. break;
  352. }
  353. i = 4*(strtol(t[2], nil, 10)-1); /* starting entry in satellite table */
  354. fix.satellites = strtol(t[3], nil, 10);
  355. k = 4;
  356. while(i < nelem(fix.s) && k + 3 < n){
  357. fix.s[i].prn = strtol(t[k++], nil, 10);
  358. fix.s[i].elevation = strtol(t[k++], nil, 10);
  359. fix.s[i].azimuth = strtol(t[k++], nil, 10);
  360. fix.s[i].snr = strtol(t[k++], nil, 10);
  361. k += 4;
  362. i++;
  363. }
  364. fix.messages |= 1 << tp;
  365. break;
  366. case GPVTG:
  367. if(n < 8){
  368. gpsmsg[tp].errors++;
  369. if(debug)
  370. fprint(2, "%s: Expect at least 8 tokens, got %d\n",
  371. gpsmsg[tp].name, n);
  372. break;
  373. }
  374. getcrs(t[2], &fix);
  375. gethdg(t[4], &fix);
  376. getgs(t[6], &fix);
  377. if(n > 8)
  378. getkmh(t[8], &fix);
  379. fix.messages |= 1 << tp;
  380. break;
  381. default:
  382. if(debug && fix.date)
  383. fprint(2, "Don't know %s\n", t[0]);
  384. break;
  385. }
  386. if(fix.valid){
  387. seconds++;
  388. lock(&fixlock);
  389. memmove(&curfix, &fix, sizeof fix);
  390. unlock(&fixlock);
  391. if(debug)
  392. printfix(2, &fix);
  393. fix.valid = 0;
  394. fix.messages = 0;
  395. for(i = 0; i < nelem(fix.s); i++)
  396. fix.s[i].prn = 0;
  397. if(gpsplayback)
  398. sleep(100);
  399. }
  400. }
  401. }
  402. void
  403. gpsinit(void)
  404. {
  405. proccreate(gpstrack, nil, 4096);
  406. }
  407. void
  408. printfix(int f, Fix *fix){
  409. int i;
  410. fprint(f, "%L, ", fix->Place);
  411. fprint(f, "%g, ", fix->magvar);
  412. fprint(f, "%gm - %gm = %gm, ", fix->altitude, fix->sealevel, fix->altitude - fix->sealevel);
  413. fprint(f, "%06dZ(%g)-", (int)fix->zulu, fix->zulu);
  414. fprint(f, "%06d\n", fix->date);
  415. if(fix->lat >= 0)
  416. fprint(f, "%11.8fN, ", fix->lat);
  417. else
  418. fprint(f, "%11.8fS, ", -fix->lat);
  419. if(fix->lon >= 0)
  420. fprint(f, "%12.8fE, ", fix->lon);
  421. else
  422. fprint(f, "%12.8fW, ", -fix->lon);
  423. fprint(f, "%g@%g, ", fix->course, fix->groundspeed);
  424. fprint(f, "(%c, %ds)\n", fix->valid, fix->satellites);
  425. for(i = 0; i < nelem(fix->s); i++){
  426. if(fix->s[i].prn == 0)
  427. continue;
  428. fprint(f, "[%d, %d°, %d°, %d]\n",
  429. fix->s[i].prn, fix->s[i].elevation, fix->s[i].azimuth, fix->s[i].snr);
  430. }
  431. }
  432. char*
  433. readposn(Req *r)
  434. {
  435. Fix f;
  436. char buf[256];
  437. lock(&fixlock);
  438. memmove(&f, &curfix, sizeof f);
  439. unlock(&fixlock);
  440. snprint(buf, sizeof buf, "%x %06dZ %lud %g %g %g %g %g %g",
  441. gpsplayback|f.quality, (int)f.zulu, f.time, f.lon, f.lat, f.altitude - f.sealevel,
  442. f.course, f.groundspeed, f.magvar);
  443. readstr(r, buf);
  444. return nil;
  445. }
  446. char*
  447. readtime(Req *r)
  448. {
  449. Fix f;
  450. char buf[Numsize+Vlnumsize+Vlnumsize+8];
  451. lock(&fixlock);
  452. memmove(&f, &curfix, sizeof f);
  453. unlock(&fixlock);
  454. seprint(buf, buf + sizeof buf, "%*.0lud %*.0llud %*.0llud %c",
  455. Numsize-1, f.time,
  456. Vlnumsize-1, f.gpstime,
  457. Vlnumsize-1, f.localtime, f.valid + (gpsplayback?1:0));
  458. readstr(r, buf);
  459. return nil;
  460. }
  461. char*
  462. readstats(Req *r)
  463. {
  464. int i;
  465. char buf[1024], *p;
  466. p = buf;
  467. p = seprint(p, buf + sizeof buf, "%lld bytes read, %ld samples processed in %ld seconds\n",
  468. rawin, seconds, curfix.time - starttime);
  469. p = seprint(p, buf + sizeof buf, "%lud checksum errors\n", checksumerrors);
  470. p = seprint(p, buf + sizeof buf, "format errors:");
  471. for(i = 0; i < nelem(gpsmsg); i++){
  472. p = seprint(p, buf + sizeof buf, "[%s]: %ld, ",
  473. gpsmsg[i].name, gpsmsg[i].errors);
  474. }
  475. p = seprint(p, buf + sizeof buf, "\nhistogram of # bytes received per buffer:\n");
  476. for(i = 0; i < nelem(histo); i++){
  477. p = seprint(p, buf + sizeof buf, "[%d]: %ld ",
  478. i, histo[i]);
  479. }
  480. p = seprint(p, buf + sizeof buf, "\n");
  481. p = seprint(p, buf + sizeof buf, "bad/good/suspect lat: %lud/%lud/%lud\n",
  482. badlat, goodlat, suspectlat);
  483. p = seprint(p, buf + sizeof buf, "bad/good/suspect lon: %lud/%lud/%lud\n",
  484. badlon, goodlon, suspectlon);
  485. p = seprint(p, buf + sizeof buf, "good/suspect time: %lud/%lud\n", goodtime, suspecttime);
  486. USED(p);
  487. readstr(r, buf);
  488. return nil;
  489. }
  490. char*
  491. readsats(Req *r)
  492. {
  493. Fix f;
  494. int i;
  495. char buf[1024], *p;
  496. lock(&fixlock);
  497. memmove(&f, &curfix, sizeof f);
  498. unlock(&fixlock);
  499. p = seprint(buf, buf + sizeof buf, "%d %d\n", gpsplayback|f.quality, f.satellites);
  500. for(i = 0; i < nelem(f.s); i++){
  501. if(f.s[i].prn == 0)
  502. continue;
  503. p = seprint(p, buf + sizeof buf, "%d %d %d %d\n",
  504. f.s[i].prn, f.s[i].elevation, f.s[i].azimuth, f.s[i].snr);
  505. }
  506. readstr(r, buf);
  507. return nil;
  508. }
  509. char*
  510. readraw(Req *r)
  511. {
  512. int n;
  513. GPSfile *f;
  514. f = r->fid->file->aux;
  515. if(rawin - rawout > Rawbuf){
  516. rawout = rawin - Rawbuf;
  517. f->offset = rawout - r->ifcall.offset;
  518. }
  519. n = Rawbuf - (rawout&Rawmask);
  520. if(rawin - rawout < n)
  521. n = rawin - rawout;
  522. if(r->ifcall.count < n)
  523. n = r->ifcall.count;
  524. r->ofcall.count = n;
  525. if(n > 0){
  526. memmove(r->ofcall.data, raw + (rawout & Rawmask), n);
  527. rawout += n;
  528. }
  529. return nil;
  530. }
  531. void
  532. rtcset(int32_t t)
  533. {
  534. static int fd;
  535. int32_t r;
  536. int n;
  537. char buf[32];
  538. if(fd <= 0 && (fd = open("#r/rtc", ORDWR)) < 0){
  539. fprint(2, "Can't open #r/rtc: %r\n");
  540. return;
  541. }
  542. n = read(fd, buf, sizeof buf - 1);
  543. if(n <= 0){
  544. fprint(2, "Can't read #r/rtc: %r\n");
  545. return;
  546. }
  547. buf[n] = '\0';
  548. r = strtol(buf, nil, 0);
  549. if(r <= 0){
  550. fprint(2, "ridiculous #r/rtc: %ld\n", r);
  551. return;
  552. }
  553. if(r - t > 1 || t - r > 0){
  554. seek(fd, 0, 0);
  555. fprint(fd, "%ld", t);
  556. fprint(2, "correcting #r/rtc: %ld → %ld\n", r, t);
  557. }
  558. seek(fd, 0, 0);
  559. }
  560. int
  561. gettime(Fix *f){
  562. /* Convert zulu time and date to Plan9 time(2) */
  563. Tm tm;
  564. int zulu;
  565. double d;
  566. int32_t t;
  567. static int count;
  568. zulu = f->zulu;
  569. memset(&tm, 0, sizeof tm );
  570. tm.sec = zulu % 100;
  571. tm.min = (zulu/100) % 100;
  572. tm.hour = zulu / 10000;
  573. tm.year = f->date % 100 + 100; /* This'll only work until 2099 */
  574. tm.mon = ((f->date/100) % 100) - 1;
  575. tm.mday = f->date / 10000;
  576. strcpy(tm.zone, "GMT");
  577. t = tm2sec(&tm);
  578. if(f->time && count < 3 && (t - f->time > 10 || t - f->time <= 0)){
  579. count++;
  580. suspecttime++;
  581. return -1;
  582. }
  583. goodtime++;
  584. f->time = t;
  585. count = 0;
  586. if(starttime == 0) starttime = t;
  587. f->gpstime = 1000000000LL * t + 1000000 * (int)modf(f->zulu, &d);
  588. if(setrtc){
  589. if(setrtc == 1 || (t % 300) == 0){
  590. rtcset(t);
  591. setrtc++;
  592. }
  593. }
  594. return 0;
  595. }
  596. int
  597. getzulu(char *s, Fix *f){
  598. double d;
  599. if(*s == '\0') return 0;
  600. if(isdigit(*s)){
  601. d = strtod(s, nil);
  602. if(!isNaN(d))
  603. f->zulu = d;
  604. return 1;
  605. }
  606. return 0;
  607. }
  608. int
  609. getdate(char *s, Fix *f){
  610. if(*s == 0) return 0;
  611. if(isdigit(*s)){
  612. f->date = strtol(s, nil, 10);
  613. return 1;
  614. }
  615. return 0;
  616. }
  617. int
  618. getgs(char *s, Fix *f){
  619. double d;
  620. if(*s == 0) return 0;
  621. if(isdigit(*s)){
  622. d = strtod(s, nil);
  623. if(!isNaN(d))
  624. f->groundspeed = d;
  625. return 1;
  626. }
  627. return 0;
  628. }
  629. int
  630. getkmh(char *s, Fix *f){
  631. double d;
  632. if(*s == 0) return 0;
  633. if(isdigit(*s)){
  634. d = strtod(s, nil);
  635. if(!isNaN(d))
  636. f->kmh = d;
  637. return 1;
  638. }
  639. return 0;
  640. }
  641. int
  642. getcrs(char *s1, Fix *f){
  643. double d;
  644. if(*s1 == 0) return 0;
  645. if(isdigit(*s1)){
  646. d = strtod(s1, nil);
  647. if(!isNaN(d))
  648. f->course = d;
  649. return 1;
  650. }
  651. return 0;
  652. }
  653. int
  654. gethdg(char *s1, Fix *f){
  655. double d;
  656. if(*s1 == 0) return 0;
  657. if(isdigit(*s1)){
  658. d = strtod(s1, nil);
  659. if(!isNaN(d))
  660. f->heading = d;
  661. return 1;
  662. }
  663. return 0;
  664. }
  665. int
  666. getalt(char *s1, char *s2, Fix *f){
  667. double alt;
  668. if(*s1 == 0) return 0;
  669. if(isdigit(*s1)){
  670. alt = strtod(s1, nil);
  671. if(*s2 == 'M' && !isNaN(alt)){
  672. f->altitude = alt;
  673. return 1;
  674. }
  675. return 0;
  676. }
  677. return 0;
  678. }
  679. int
  680. getsea(char *s1, char *s2, Fix *f){
  681. double alt;
  682. if(*s1 == 0) return 0;
  683. if(isdigit(*s1)){
  684. alt = strtod(s1, nil);
  685. if(*s2 == 'M'){
  686. f->sealevel = alt;
  687. return 1;
  688. }
  689. return 0;
  690. }
  691. return 0;
  692. }
  693. int
  694. getlat(char *s1, char *s2, Fix *f){
  695. double lat;
  696. static int count;
  697. if(*s1 == 0 || !isdigit(*s1) || strlen(s1) <= 5){
  698. badlat++;
  699. return -1;
  700. }
  701. lat = strtod(s1+2, nil);
  702. if(isNaN(lat)){
  703. badlat++;
  704. return -1;
  705. }
  706. lat /= 60.0;
  707. lat += 10*(s1[0] - '0') + s1[1] - '0';
  708. if(lat < 0 || lat > 90.0){
  709. badlat++;
  710. return -1;
  711. }
  712. switch(*s2){
  713. default:
  714. badlat++;
  715. return -1;
  716. case 'S':
  717. lat = -lat;
  718. case 'N':
  719. break;
  720. }
  721. if(f->lat <= 90.0 && count < 3 && fabs(f->lat - lat) > 10.0){
  722. count++;
  723. suspectlat++;
  724. return -1;
  725. }
  726. f->lat = lat;
  727. count = 0;
  728. goodlat++;
  729. return 0;
  730. }
  731. int
  732. getlon(char *s1, char *s2, Fix *f){
  733. double lon;
  734. static int count;
  735. if(*s1 == 0 || ! isdigit(*s1) || strlen(s1) <= 5){
  736. badlon++;
  737. return -1;
  738. }
  739. lon = strtod(s1+3, nil);
  740. if(isNaN(lon)){
  741. badlon++;
  742. return -1;
  743. }
  744. lon /= 60.0;
  745. lon += 100*(s1[0] - '0') + 10*(s1[1] - '0') + s1[2] - '0';
  746. if(lon < 0 || lon > 180.0){
  747. badlon++;
  748. return -1;
  749. }
  750. switch(*s2){
  751. default:
  752. badlon++;
  753. return -1;
  754. case 'W':
  755. lon = -lon;
  756. case 'E':
  757. break;
  758. }
  759. if(f->lon <= 180.0 && count < 3 && fabs(f->lon - lon) > 10.0){
  760. count++;
  761. suspectlon++;
  762. return -1;
  763. }
  764. f->lon = lon;
  765. goodlon++;
  766. count = 0;
  767. return 0;
  768. }
  769. int
  770. getmagvar(char *s1, char *s2, Fix *f){
  771. double magvar;
  772. if(*s1 == 0) return 0;
  773. if(isdigit(*s1) && strlen(s1) > 5){
  774. magvar = strtod(s1+3, nil);
  775. if(isNaN(magvar))
  776. return 0;
  777. magvar /= 60.0;
  778. magvar += 100*(s1[0] - '0') + 10*(s1[1] - '0') + s1[2] - '0';
  779. if(*s2 == 'W'){
  780. f->magvar = -magvar;
  781. return 1;
  782. }
  783. if(*s2 == 'E'){
  784. f->magvar = magvar;
  785. return 1;
  786. }
  787. return 0;
  788. }
  789. return 0;
  790. }
  791. void
  792. putline(char *s){
  793. write(ttyfd, s, strlen(s));
  794. write(ttyfd, "\r\n", 2);
  795. }
  796. int
  797. type(char *s){
  798. int i;
  799. for(i = 0; i < nelem(gpsmsg); i++){
  800. if(strcmp(s, gpsmsg[i].name) == 0) return i;
  801. }
  802. return -1;
  803. }
  804. void
  805. setline(void){
  806. char *serialctl;
  807. serialctl = smprint("%sctl", serial);
  808. if((ttyfd = open(serial, ORDWR)) < 0)
  809. sysfatal("%s: %r", serial);
  810. if((ctlfd = open(serialctl, OWRITE)) >= 0){
  811. if(fprint(ctlfd, baudstr, baud) < 0)
  812. sysfatal("%s: %r", serialctl);
  813. }else
  814. gpsplayback = 0x8;
  815. free(serialctl);
  816. }
  817. int getonechar(int64_t *t){
  818. static char buf[32], *p;
  819. static int n;
  820. if(n == 0){
  821. n = read(ttyfd, buf, sizeof(buf));
  822. if(t) *t = nsec();
  823. if(n < 0)
  824. sysfatal("%s: %r", serial);
  825. if(n == 0)
  826. threadexits(nil);
  827. /*
  828. * We received n characters, so the first must have been there
  829. * at least n/(10*baud) seconds (10 is 1 start
  830. * bit, one stop bit and 8 data bits per character)
  831. */
  832. if(t) {
  833. *t -= n * nsecperchar;
  834. histo[n]++;
  835. }
  836. p = buf;
  837. }
  838. n--;
  839. return *p++;
  840. }
  841. void
  842. getline(char *s, int size, int64_t *t){
  843. uint8_t c;
  844. char *p;
  845. int n, cs;
  846. tryagain:
  847. for(;;){
  848. p = s;
  849. n = 0;
  850. while((c = getonechar(t)) != '\n' && n < size){
  851. t = nil;
  852. if(c != '\r'){
  853. *p++ = c;
  854. n++;
  855. }
  856. }
  857. if(n < size)
  858. break;
  859. while(getonechar(t) != '\n' && n < 4096)
  860. n++;
  861. if(n == 4096)
  862. sysfatal("preposterous gps line, wrong baud rate?");
  863. fprint(2, "ridiculous gps line: %d bytes\n", n);
  864. }
  865. *p = 0;
  866. for(p = s; isdigit(*p); p++)
  867. ;
  868. if(*p++ == ' ')
  869. memmove(s, p, strlen(p)+1);
  870. if(s[0] == '$'){
  871. if(n > 4 && s[n-3] == '*'){
  872. s[n-3] = 0;
  873. p = s+1;
  874. cs = 0;
  875. while(*p) cs ^= *p++;
  876. n = strtol(&s[n-2], nil, 16);
  877. if(n != cs){
  878. if(debug)
  879. fprint(2, "Checksum error %s, 0x%x, 0x%x\n",
  880. s, n, cs);
  881. checksumerrors++;
  882. goto tryagain;
  883. }
  884. }
  885. }
  886. for(p = s; *p; rawin++)
  887. raw[rawin & Rawmask] = *p++;
  888. raw[rawin & Rawmask] = '\n';
  889. rawin++;
  890. }