gpsfs.c

这是一个同样来自贝尔实验室的和UNIX有着渊源的操作系统, 其简洁的设计和实现易于我们学习和理解

	lock(&fixlock);
	memmove(&f, &curfix, sizeof f);
	unlock(&fixlock);
	seprint(buf, buf + sizeof buf, "%*.0lud %*.0llud %*.0llud %c",
		Numsize-1, f.time,
		Vlnumsize-1, f.gpstime,
		Vlnumsize-1, f.localtime, f.valid + (gpsplayback?1:0));
	readstr(r, buf);
	return nil;
}

char*
readstats(Req *r)
{
	int i;
	char buf[1024], *p;

	p = buf;
	p = seprint(p, buf + sizeof buf, "%lld bytes read, %ld samples processed in %ld seconds\n",
		rawin, seconds, curfix.time - starttime);
	p = seprint(p, buf + sizeof buf, "%lud checksum errors\n", checksumerrors);
	p = seprint(p, buf + sizeof buf, "format errors:");
	for(i = 0; i < nelem(gpsmsg); i++){
		p = seprint(p, buf + sizeof buf, "[%s]: %ld, ",
			gpsmsg[i].name, gpsmsg[i].errors);
	}
	p = seprint(p, buf + sizeof buf, "\nhistogram of # bytes received per buffer:\n");
	for(i = 0; i < nelem(histo); i++){
		p = seprint(p, buf + sizeof buf, "[%d]: %ld ",
			i, histo[i]);
	}
	p = seprint(p, buf + sizeof buf, "\n");
	p = seprint(p, buf + sizeof buf, "bad/good/suspect lat: %lud/%lud/%lud\n",
		badlat, goodlat, suspectlat);
	p = seprint(p, buf + sizeof buf, "bad/good/suspect lon: %lud/%lud/%lud\n",
		badlon, goodlon, suspectlon);
	p = seprint(p, buf + sizeof buf, "good/suspect time: %lud/%lud\n", goodtime, suspecttime);
	USED(p);
	readstr(r, buf);
	return nil;
}

char*
readsats(Req *r)
{
	Fix f;
	int i;
	char buf[1024], *p;

	lock(&fixlock);
	memmove(&f, &curfix, sizeof f);
	unlock(&fixlock);
	p = seprint(buf, buf + sizeof buf, "%d	%d\n", gpsplayback|f.quality, f.satellites);
	for(i = 0; i < nelem(f.s); i++){
		if(f.s[i].prn == 0)
			continue;
		p = seprint(p, buf + sizeof buf, "%d	%d	%d	%d\n",
			f.s[i].prn, f.s[i].elevation, f.s[i].azimuth, f.s[i].snr);
	}
	readstr(r, buf);
	return nil;
}

char*
readraw(Req *r)
{
	int n;
	GPSfile *f;

	f = r->fid->file->aux;
	if(rawin - rawout > Rawbuf){
		rawout = rawin - Rawbuf;
		f->offset = rawout - r->ifcall.offset;
	}
	n = Rawbuf - (rawout&Rawmask);
	if(rawin - rawout < n)
		n = rawin - rawout;
	if(r->ifcall.count < n)
		n = r->ifcall.count;
	r->ofcall.count = n;
	if(n > 0){
		memmove(r->ofcall.data, raw + (rawout & Rawmask), n);
		rawout += n;
	}
	return nil;
}

void
rtcset(long t)
{
	static int fd;
	long r;
	int n;
	char buf[32];

	if(fd <= 0 && (fd = open("#r/rtc", ORDWR)) < 0){
		fprint(2, "Can't open #r/rtc: %r\n");
		return;
	}
	n = read(fd, buf, sizeof buf - 1);
	if(n <= 0){
		fprint(2, "Can't read #r/rtc: %r\n");
		return;
	}
	buf[n] = '\0';
	r = strtol(buf, nil, 0);
	if(r <= 0){
		fprint(2, "ridiculous #r/rtc: %ld\n", r);
		return;
	}
	if(r - t > 1 || t - r > 0){
		seek(fd, 0, 0);
		fprint(fd, "%ld", t);
		fprint(2, "correcting #r/rtc: %ld → %ld\n", r, t);
	}
	seek(fd, 0, 0);
}

int
gettime(Fix *f){
	/* Convert zulu time and date to Plan9 time(2) */
	Tm tm;
	int zulu;
	double d;
	long t;
	static int count;

	zulu = f->zulu;
	memset(&tm, 0, sizeof tm );
	tm.sec = zulu % 100;
	tm.min = (zulu/100) % 100;
	tm.hour = zulu / 10000;
	tm.year = f->date % 100 + 100;	/* This'll only work until 2099 */
	tm.mon = ((f->date/100) % 100) - 1;
	tm.mday = f->date / 10000;
	strcpy(tm.zone, "GMT");
	t = tm2sec(&tm);
	if(f->time && count < 3 && (t - f->time > 10 || t - f->time <= 0)){
		count++;
		suspecttime++;
		return -1;
	}
	goodtime++;
	f->time = t;
	count = 0;
	if(starttime == 0) starttime = t;
	f->gpstime = 1000000000LL * t + 1000000 * (int)modf(f->zulu, &d);
	if(setrtc){
		if(setrtc == 1 || (t % 300) == 0){
			rtcset(t);
			setrtc++;
		}
	}
	return 0;
}

int
getzulu(char *s, Fix *f){
	double d;

	if(*s == '\0') return 0;
	if(isdigit(*s)){
		d = strtod(s, nil);
		if(!isNaN(d))
			f->zulu = d;
		return 1;
	}
	return 0;
}

int
getdate(char *s, Fix *f){
	if(*s == 0) return 0;
	if(isdigit(*s)){
		f->date = strtol(s, nil, 10);
		return 1;
	}
	return 0;
}

int
getgs(char *s, Fix *f){
	double d;

	if(*s == 0) return 0;
	if(isdigit(*s)){
		d = strtod(s, nil);
		if(!isNaN(d))
			f->groundspeed = d;
		return 1;
	}
	return 0;
}

int
getkmh(char *s, Fix *f){
	double d;

	if(*s == 0) return 0;
	if(isdigit(*s)){
		d = strtod(s, nil);
		if(!isNaN(d))
			f->kmh = d;
		return 1;
	}
	return 0;
}

int
getcrs(char *s1, Fix *f){
	double d;

	if(*s1 == 0) return 0;
	if(isdigit(*s1)){
		d = strtod(s1, nil);
		if(!isNaN(d))
			f->course = d;
		return 1;
	}
	return 0;
}

int
gethdg(char *s1, Fix *f){
	double d;

	if(*s1 == 0) return 0;
	if(isdigit(*s1)){
		d = strtod(s1, nil);
		if(!isNaN(d))
			f->heading = d;
		return 1;
	}
	return 0;
}

int
getalt(char *s1, char *s2, Fix *f){
	double alt;

	if(*s1 == 0) return 0;
	if(isdigit(*s1)){
		alt = strtod(s1, nil);
		if(*s2 == 'M' && !isNaN(alt)){
			f->altitude = alt;
			return 1;
		}
		return 0;
	}
	return 0;
}

int
getsea(char *s1, char *s2, Fix *f){
	double alt;

	if(*s1 == 0) return 0;
	if(isdigit(*s1)){
		alt = strtod(s1, nil);
		if(*s2 == 'M'){
			f->sealevel = alt;
			return 1;
		}
		return 0;
	}
	return 0;
}

int
getlat(char *s1, char *s2, Fix *f){
	double lat;
	static count;

	if(*s1 == 0 || !isdigit(*s1) || strlen(s1) <= 5){
		badlat++;
		return -1;
	}
	lat = strtod(s1+2, nil);
	if(isNaN(lat)){
		badlat++;
		return -1;
	}
	lat /= 60.0;
	lat += 10*(s1[0] - '0') + s1[1] - '0';
	if(lat < 0 || lat > 90.0){
		badlat++;
		return -1;
	}
	switch(*s2){
	default:
		badlat++;
		return -1;
	case 'S':
		lat = -lat;
	case 'N':
		break;
	}
	if(f->lat <= 90.0 && count < 3 && fabs(f->lat - lat) > 10.0){
		count++;
		suspectlat++;
		return -1;
	}
	f->lat = lat;
	count = 0;
	goodlat++;
	return 0;
}

int
getlon(char *s1, char *s2, Fix *f){
	double lon;
	static count;

	if(*s1 == 0 || ! isdigit(*s1) || strlen(s1) <= 5){
		badlon++;
		return -1;
	}
	lon = strtod(s1+3, nil);
	if(isNaN(lon)){
		badlon++;
		return -1;
	}
	lon /= 60.0;
	lon += 100*(s1[0] - '0') + 10*(s1[1] - '0') + s1[2] - '0';
	if(lon < 0 || lon > 180.0){
		badlon++;
		return -1;
	}
	switch(*s2){
	default:
		badlon++;
		return -1;
	case 'W':
		lon = -lon;
	case 'E':
		break;
	}
	if(f->lon <= 180.0 && count < 3 && fabs(f->lon - lon) > 10.0){
		count++;
		suspectlon++;
		return -1;
	}
	f->lon = lon;
	goodlon++;
	count = 0;
	return 0;
}

int
getmagvar(char *s1, char *s2, Fix *f){
	double magvar;

	if(*s1 == 0) return 0;
	if(isdigit(*s1) && strlen(s1) > 5){
		magvar = strtod(s1+3, nil);
		if(isNaN(magvar))
			return 0;
		magvar /= 60.0;
		magvar += 100*(s1[0] - '0') + 10*(s1[1] - '0') + s1[2] - '0';
		if(*s2 == 'W'){
			f->magvar = -magvar;
			return 1;
		}
		if(*s2 == 'E'){
			f->magvar = magvar;
			return 1;
		}
		return 0;
	}
	return 0;
}

void
putline(char *s){
	write(ttyfd, s, strlen(s));
	write(ttyfd, "\r\n", 2);
}

int
type(char *s){
	int i;

	for(i = 0; i < nelem(gpsmsg); i++){
		if(strcmp(s, gpsmsg[i].name) == 0) return i;
	}
	return -1;
}

void
setline(void){
	char *serialctl;

	serialctl = smprint("%sctl", serial);
	if((ttyfd = open(serial, ORDWR)) < 0)
		sysfatal("%s: %r", serial);
	if((ctlfd = open(serialctl, OWRITE)) >= 0){
		if(fprint(ctlfd, baudstr, baud) < 0)
			sysfatal("%s: %r", serialctl);
	}else
		gpsplayback = 0x8;
	free(serialctl);
}

int getonechar(vlong *t){
	static char buf[32], *p;
	static int n;

	if(n == 0){
		n = read(ttyfd, buf, sizeof(buf));
		if(t) *t = nsec();
		if(n < 0)
			sysfatal("%s: %r", serial);
		if(n == 0)
			threadexits(nil);
		/*
		 * We received n characters, so the first must have been there
		 * at least n/(10*baud) seconds (10 is 1 start
		 * bit, one stop bit and 8 data bits per character)
		 */
		if(t) {
			*t -= n * nsecperchar;
			histo[n]++;
		}
		p = buf;
	}
	n--;
	return *p++;
}

void
getline(char *s, int size, vlong *t){
	uchar c;
	char *p;
	int n, cs;

tryagain:
	for(;;){
		p = s;
		n = 0;
		while((c = getonechar(t)) != '\n' && n < size){
			t = nil;
			if(c != '\r'){
				*p++ = c;
				n++;
			}
		}
		if(n < size)
			break;
		while(getonechar(t) != '\n' && n < 4096)
			n++;
		if(n == 4096)
			sysfatal("preposterous gps line, wrong baud rate?");
		fprint(2, "ridiculous gps line: %d bytes\n", n);
	}
	*p = 0;
	for(p = s; isdigit(*p); p++)
		;
	if(*p++ == '	')
		memmove(s, p, strlen(p)+1);
	if(s[0] == '$'){
		if(n > 4 && s[n-3] == '*'){
			s[n-3] = 0;
			p = s+1;
			cs = 0;
			while(*p) cs ^= *p++;
			n = strtol(&s[n-2], nil, 16);
			if(n != cs){
				if(debug)
					fprint(2, "Checksum error %s, 0x%x, 0x%x\n",
						s, n, cs);
				checksumerrors++;
				goto tryagain;
			}
		}
	}
	for(p = s; *p; rawin++)
		raw[rawin & Rawmask] = *p++;
	raw[rawin & Rawmask] = '\n';
	rawin++;
}