gpsfs.c 18 KB

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