WebSVN – planix.SVN – Blame – /os/branches/feature_tlsv12/sys/src/cmd/aux/gps/gpsfs.c

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

Subversion Repositories planix.SVN

(root)/os/branches/feature_tlsv12/sys/src/cmd/aux/gps/gpsfs.c – Rev 22