added some settings changes

GPS/cmdlnopts.c

@@ -28,7 +28,12 @@ glob_pars G;  // internal global parameters structure
 int help = 0; // whether to show help string
 
 glob_pars Gdefault = {
-	.devpath        = "/dev/ttyUSB0",
+	 .devpath        = "/dev/ttyUSB0"
+	,.pollubx        = 0
+	,.pollinterval   = 1.
+	,.block_msg      = {0,1,0,0,0,0}   // all exept RMC are blocked by default
+	,.polltmout      = 10.
+	,.stationary     = 0
 };
 
 /*
@@ -40,11 +45,20 @@ myoption cmdlnopts[] = {
 	{"help",	0,	NULL,	'h',	arg_int,	APTR(&help),		N_("show this help")},
 	/// "ÐÕÔØ Ë ÕÓÔÒÏÊÓÔ×Õ"
 	{"device",1,	NULL,	'd',	arg_string,	APTR(&G.devpath),	N_("device path")},
+	{"poll-udx", 0,	NULL,	'p',	arg_none,	APTR(&G.pollubx),	N_("poll UDX,00")},
+	{"pollinterval",1,NULL,	'i',	arg_double,	APTR(&G.pollinterval),N_("polling interval")},
+	{"no-rmc",0,&G.block_msg[GPRMC],	0,	arg_none,	NULL,	N_("block RMC message")},
+	{"gsv",	0,	&G.block_msg[GPGSV],	1,	arg_none,	NULL,	N_("process GSV message")},
+	{"gsa",	0,	&G.block_msg[GPGSA],	1,	arg_none,	NULL,	N_("process GSA message")},
+	{"gga",	0,	&G.block_msg[GPGGA],	1,	arg_none,	NULL,	N_("process GGA message")},
+	{"gll",	0,	&G.block_msg[GPGLL],	1,	arg_none,	NULL,	N_("process GLL message")},
+	{"vtg",	0,	&G.block_msg[GPVTG],	1,	arg_none,	NULL,	N_("process VTG message")},
+	{"timeout", 1,	NULL,	't',	arg_double,	APTR(&G.polltmout), N_("polling timeout")},
+	{"stationary",0, NULL,	's',	arg_int,	APTR(&G.stationary),N_("configure as stationary")},
 	// ...
 	end_option
 };
 
-
 /**
  * Parce command line options and return dynamically allocated structure
  * 		to global parameters

GPS/cmdlnopts.h

@@ -25,12 +25,28 @@
 
 #include "parceargs.h"
 
+// GSV, RMC, GSA, GGA, GLL, VTG, TXT
+extern char *GPmsgs[];
+typedef enum{
+	GPGSV = 0,
+	GPRMC,
+	GPGSA,
+	GPGGA,
+	GPGLL,
+	GPVTG,
+	GPMAXMSG
+}GPmsg_type;
+
 /*
  * here are some typedef's for global data
  */
-
 typedef struct{
-	char *devpath;         // device path
+	char *devpath;               // device path
+	int pollubx;                 // flag of polling ubx00
+	double pollinterval;         // ubx00 polling interval (in seconds)
+	int block_msg[GPMAXMSG];     // array of messages: 1-show, 0-block
+	double polltmout;            // polling timeout (program ends after this interval)
+	int stationary;              // configure as stationary
 }glob_pars;
 
 glob_pars *parce_args(int argc, char **argv);

GPS/main.c

@@ -32,15 +32,18 @@
 
 glob_pars *GP = NULL;
 
+// Messages for blocking: GSV, RMC, GSA, GGA, GLL, VTG
+char *GPmsgs[] = {"GSV", "RMC", "GSA", "GGA", "GLL", "VTG"};
+
 static void signals(int sig){
 	DBG("Get signal %d, quit.\n", sig);
 	restore_tty();
 	exit(sig);
 }
 
-char *get_portdata(){
+uint8_t *get_portdata(){
-	static char buf[1025];
+	static uint8_t buf[1025];
-	char *ptr = buf;
+	uint8_t *ptr = buf;
 	size_t L = 0, rest = 1024;
 	while(rest && (L = read_tty(ptr, rest))){
 		rest -= L;
@@ -54,25 +57,190 @@ char *get_portdata(){
 }
 
 /**
- * Checksum: return 0 if fails
+ * Calculate checksum & write message to port
+ * @param  buf - command to write (with leading $ and trailing *)
+ * return 0 if fails
  */
-int checksum(char *buf){
+int write_with_checksum(uint8_t *buf){
-	char *eol;
+	static char CS[3];
-	char chs[2];
+	//uint8_t *ptr = buf;
-	unsigned char checksum = 0;
+	uint8_t checksum = 0;
-	if(*buf != '$' || !(eol = strchr(buf, '*'))){
+	if(*buf != '$') return 0;
+	if(write_tty(buf, strlen((char*)buf))) return 0;
+	++buf; // skip leaders
+	do{
+		checksum ^= *buf++;
+	}while(*buf && *buf != '*');
+	snprintf(CS, 3, "%X", checksum);
+	if(write_tty((uint8_t*)CS, 2)) return 0;
+	if(write_tty((uint8_t*)"\r\n", 2)) return 0;
+	//DBG("Write: %s%c%c", ptr, CS[0], CS[1]);
+	return 1;
+}
+
+// Check checksum
+int checksum(uint8_t *buf){
+	uint8_t *eol;
+	char chs[3];
+	uint8_t checksum = 0;
+	if(*buf != '$' || !(eol = (uint8_t*)strchr((char*)buf, '*'))){
 		DBG("Wrong data: %s\n", buf);
 		return 0;
 	}
 	while(++buf != eol)
-		checksum ^= (unsigned char) (*buf);
+		checksum ^= *buf;
-	snprintf(chs, 2, "%X", checksum);
+	snprintf(chs, 3, "%X", checksum);
-	if(!strncmp(chs, ++buf, 2)) return 0;
+	if(strncmp(chs, (char*)++buf, 2)) return 0;
 	return 1;
 }
 
-void gsv(_U_ char *buf){
+uint8_t *nextpos(uint8_t **buf, int pos){
+	int i;
+	if(pos < 1) pos = 1;
+	for(i = 0; i < pos; ++i){
+		*buf = (uint8_t*)strchr((char*)*buf, ',');
+		if(!*buf) break;
+		++(*buf);
+	}
+	return *buf;
+}
+#define NEXT()      do{if(!nextpos(&buf, 1)) goto ret;}while(0)
+#define SKIP(NPOS)  do{if(!nextpos(&buf, NPOS)) goto ret;}while(0)
+
+/*
+ * Satellites in View
+ * $GPGSV,NoMsg,MsgNo,NoSv,{,sv,elv,az,cno}*cs
+ * 1    = total number of GPGSV messages being output
+ * 2    = Number of this message
+ * 3    = Satellites in View
+ *    4-7 will be repeated 1..4 times
+ * sv   = Satellite ID
+ * elv  = Elevation, range 0..90deg
+ * az   = Azimuth, range 0..359deg
+ * cno  = SNR, range 0.99dB
+ * cs   = control sum
+ * 3,1,11,01,68,278,,03,39,292,08,04,66,190,30,11,55,231,34*79
+ * 3,2,11,14,30,050,10,17,11,322,,19,09,202,,22,14,096,*74
+ * 3,3,11,23,20,232,30,31,43,118,33,32,76,352,*43
+*/
+void gsv(uint8_t *buf){
 	//DBG("gsv: %s\n", buf);
+	int Nrec = -1, Ncur, inview = 0;
+	static int sat_scanned = 0;
+	if(sscanf((char*)buf, "%d,%d,", &Nrec, &Ncur) != 2) goto ret;
+	SKIP(2);
+	if(sscanf((char*)buf, "%d,", &inview) != 1) goto ret;
+	if(inview < 1) goto ret;
+	NEXT();
+	if(Ncur == 1)
+		printf("%d satellites in view field: (No: ID, elevation, azimuth, SNR)\n", inview);
+	do{
+		int id, el, az, snr;
+		// there could be no "SNR" field if we can't find this satellite on sky
+		if(sscanf((char*)buf, "%d,%d,%d,%d,", &id, &el, &az, &snr) < 3) break;
+		printf(" (%d: %d, %d, %d, %d)", ++sat_scanned, id, el, az, snr);
+		SKIP(4);
+	}while(1);
+ret:
+	if(inview < 1) printf("There's no GPS satellites in viewfield\n");
+	if(Nrec > 0 && Nrec == Ncur){
+		sat_scanned = 0;
+		printf("\n");
+	}
+}
+
+/**
+ * acquire last command
+ * @return 0 if failed
+ */
+int get_ACK(uint8_t *conf) {
+	int i;
+	uint8_t ack[10] = {0xb5, 0x62, // header
+		0x05, 0x01, // ACK-ACK
+		2, 0}; // 2 bytes, little-endian
+	// send ACK to recent CONF changes
+	ack[6] = conf[2];
+	ack[7] = conf[3];
+	double T0 = dtime();
+	// checksum
+	for (i = 2; i < 8; ++i){
+		ack[8] += ack[i];
+		ack[9] += ack[8];
+	}
+/*
+printf("ack: ");
+for(i = 0; i < 10; ++i)
+printf("%X ", ack[i]);
+printf("\n");
+*/
+	uint8_t buf[10], *ptr = buf, *cmp = ack;
+	size_t remain = 10;
+	// wait not more than 3 seconds
+	while(dtime() - T0 < 3. && remain){
+		size_t L = read_tty(ptr, remain);
+		if(L){
+			remain -= L;
+			for(i = 0; i < (int)L; ++i){
+				//DBG("got: %X (%c)", *ptr, *ptr);
+				if(*ptr++ != *cmp++){
+					//DBG("NEQ: %X != %X", ptr[-1], cmp[-1]);
+					ptr = buf; cmp = ack; remain = 10;
+					break;
+				}
+			}
+		}
+	}
+	if(!remain) return 1;
+	return 0;
+}
+
+void cfg_stationary(){
+	int i;
+	uint8_t stat[44] = {0xb5, 0x62, // header
+		0x06, 0x24, // CFG-NAV5
+		36, 0, // 36 bytes, little-endian
+		1, 0,  // mask: only dynamic model
+		2};    // stationary model
+//	stat[44] = '\r';
+//	stat[45] = '\n';
+/*
+	uint8_t stat[] = {
+	0xB5, 0x62, 0x06, 0x24, 0x24, 0x00, 0xFF, 0xFF, 0x02, 0x03,
+	0x00, 0x00, 0x00, 0x00, 0x10, 0x27, 0x00, 0x00, 0x05, 0x00,
+	0xFA, 0x00, 0xFA, 0x00, 0x64, 0x00, 0x2C, 0x01, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00, '\r', '\n'};
+	*/
+	for(i = 2; i < 42; ++i){
+		stat[42] += stat[i];
+		stat[43] += stat[42];
+	}
+/*
+printf("conf: ");
+for(i = 0; i < 44; ++i)
+printf("%X ", conf[i]);
+printf("\n");
+*/
+	i = 0;
+	do{
+		write_tty(stat, 44);
+		DBG("Written, aquire");
+	}while(!get_ACK(stat) && ++i < 11);
+	// precise point position
+	uint8_t prec[48] = {0xb5, 0x62, // header
+		0x06, 0x23, // CFG-NAVX5
+		40, 0, // 40 bytes, little-endian
+		0, 0, 0x20}; // mask for PPP
+	prec[32] = 1; // usePPP = TRUE (field 26)
+	for(i = 2; i < 46; ++i){
+		prec[46] += prec[i];
+		prec[47] += prec[46];
+	}
+	i = 0;
+	do{
+		write_tty(prec, 48);
+		DBG("Written, aquire");
+	}while(!get_ACK(prec) && ++i < 11);
 }
 
 /*
@@ -92,96 +260,230 @@ void gsv(_U_ char *buf){
  * 12   = Checksum
  * 213457.00,A,4340.59415,N,04127.47560,E,2.494,,290615,,,A*7B
  */
-void rmc(char *buf){
+void rmc(uint8_t *buf){
+	//DBG("rmc: %s\n", buf);
 	int H, M, LO, LA, d, m, y;
 	float S;
 	float longitude, lattitude, speed, track, mag;
 	char varn = 'V', north = '0', east = '0', mageast = '0';
-	sscanf(buf, "%2d%2d%f", &H, &M, &S);
+	sscanf((char*)buf, "%2d%2d%f", &H, &M, &S);
-	buf = strchr(buf, ',')+1;
+	NEXT();
 	if(*buf != ',') varn = *buf;
 	if(varn != 'A')
 		printf("(data could be wrong)");
 	else
 		printf("(data valid)");
 	printf(" time: %02d:%02d:%05.2f", H, M, S);
-	buf = strchr(buf, ',')+1;
+	NEXT();
-	sscanf(buf, "%2d%f", &LA, &lattitude);
+	sscanf((char*)buf, "%2d%f", &LA, &lattitude);
-	buf = strchr(buf, ',')+1;
+	NEXT();
 	if(*buf != ','){
 		north = *buf;
 		lattitude = (float)LA + lattitude / 60.;
 		if(north == 'S') lattitude = -lattitude;
 		printf(" latt: %f", lattitude);
 	}
-	buf = strchr(buf, ',')+1;
+	NEXT();
-	sscanf(buf, "%3d%f", &LO, &longitude);
+	sscanf((char*)buf, "%3d%f", &LO, &longitude);
-	buf = strchr(buf, ',')+1;
+	NEXT();
 	if(*buf != ','){
 		east = *buf;
 		longitude = (float)LO + longitude / 60.;
 		if(east == 'W') longitude = -longitude;
 		printf(" long: %f", longitude);
 	}
-	buf = strchr(buf, ',')+1;
+	NEXT();
 	if(*buf != ','){
-		sscanf(buf, "%f", &speed);
+		sscanf((char*)buf, "%f", &speed);
 		printf(" speed: %fknots", speed);
 	}
-	buf = strchr(buf, ',')+1;
+	NEXT();
 	if(*buf != ','){
-		sscanf(buf, "%f", &track);
+		sscanf((char*)buf, "%f", &track);
 		printf(" track: %fdeg,True", track);
 	}
-	buf = strchr(buf, ',')+1;
+	NEXT();
-	if(sscanf(buf, "%2d%2d%2d", &d, &m, &y) == 3)
+	if(sscanf((char*)buf, "%2d%2d%2d", &d, &m, &y) == 3)
 		printf(" date(dd/mm/yy): %02d/%02d/%02d", d, m, y);
-	buf = strchr(buf, ',')+1;
+	NEXT();
-	sscanf(buf, "%f,%c*", &mag, &mageast);
+	sscanf((char*)buf, "%f,%c*", &mag, &mageast);
 	if(mageast == 'E' || mageast == 'W'){
 		if(mageast == 'W') mag = -mag;
 		printf(" magnetic var: %f", mag);
 	}
+ret:
 	printf("\n");
 }
 
-// A,2,04,31,32,14,19,,,,,,,,2.77,2.58,1.00*05
+/*
-void gsa(_U_ char *buf){
+ * Overall Satellite data
+ * $GPGSA,Smode,FS{,sv},PDOP,HDOP,VDOP*cs
+ * 1     = mode: 'M' - manual, 'A' - auto
+ * 2     = fix status: 1 - not available, 2 - 2D, 3 - 3D
+ * 3..14 = used satellite number
+ * 15    = position dilution
+ * 16    = horizontal dilution
+ * 17    = vertical dilution
+ * A,2,04,31,32,14,19,,,,,,,,2.77,2.58,1.00*05
+ */
+void gsa(uint8_t *buf){
 	//DBG("gsa: %s\n", buf);
+	int used[12];
+	int i, Nused = 0;
+	if(*buf == 'M') printf("Mode: manual; ");
+	else if(*buf == 'A') printf("Mode: auto; ");
+	else return; // wrong data
+	NEXT();
+	switch(*buf){
+		case '1':
+			printf("no fix; ");
+		break;
+		case '2':
+			printf("2D fix; ");
+		break;
+		case '3':
+			printf("3D fix; ");
+		break;
+		default:
+			goto ret;
+	}
+	NEXT();
+	for(i = 0; i < 12; ++i){
+		int N;
+		if(sscanf((char*)buf, "%d,", &N) == 1)
+			used[Nused++] = N;
+		NEXT();
+	}
+	if(Nused){
+		printf("%d satellites used: ", Nused);
+		for(i = 0; i < Nused; ++i)
+			printf("%d, ", used[i]);
+	}
+	float pos, hor, vert;
+	if(sscanf((char*)buf, "%f,%f,%f", &pos, &hor, &vert) == 3){
+		printf("DILUTION: pos=%.2f, hor=%.2f, vert=%.2f", pos, hor, vert);
+	}
+ret:
+	printf("\n");
 }
 
 // 213457.00,4340.59415,N,04127.47560,E,1,05,2.58,1275.8,M,17.0,M,,*60
-void gga(_U_ char *buf){
+void gga(_U_ uint8_t *buf){
 	//DBG("gga: %s\n", buf);
 }
 
 //4340.59415,N,04127.47560,E,213457.00,A,A*60
-void gll(_U_ char *buf){
+void gll(_U_ uint8_t *buf){
 	//DBG("gll: %s\n", buf);
 }
 
 // ,T,,M,2.494,N,4.618,K,A*23
-void vtg(_U_ char *buf){
+void vtg(_U_ uint8_t *buf){
 	//DBG("vtg: %s\n", buf);
 }
 
 
-void txt(_U_ char *buf){
+void txt(_U_ uint8_t *buf){
-	//DBG("txt: %s\n", buf);
+	DBG("txt: %s\n", buf);
+}
+
+/**
+ * PUBX,00
+ * $PUBX,00,hhmmss.ss,Latitude,N,Longitude,E,AltRef,NavStat,Hacc,Vacc,SOG,COG,Vvel,ageC,HDOP,VDOP,TDOP,GU,RU,DR,*cs
+ * here buf starts from hhmmss.ss == 1
+ * 1  = UTC time
+ * 2  = lattitude
+ * 3  = N/S
+ * 4  = longitude
+ * 5  = E/W
+ * 6  = altitude
+ * 7  = nav.stat: NF/DR/G2/G3/D2/D3/RK/TT
+ * 8  = horizontal accuracy
+ * 9  = vertical accuracy
+ * 10 = speed over ground (km/h)
+ * 11 = cource over ground (deg)
+ * 12 = vertical velocy ("+" -- down, "-" -- up)
+ * 13 = age of most recent DGPS correction
+ * 14 = hor. dilution
+ * 15 = vert. dilution
+ * 16 = time dilution
+ * 17 = number of GPS satell. used
+ * 18 = number of GLONASS sat. used
+ * 19 = DR used
+ * $PUBX,00,113123.00,4340.61823,N,04127.45581,E,1295.919,G2,30,6.9,1.875,38.17,0.000,,2.77,1.00,1.41,3,0,0*4C
+ */
+void pubx(uint8_t *buf){
+	//DBG("pubx_00: %s\n", buf);
+	int H, M, LO, LA, gps, glo, dr;
+	float S, longitude, lattitude, altitude, hacc, vacc, speed, track, vertspd,
+		age, hdop, vdop, tdop;
+	char north = '0', east = '0';
+	sscanf((char*)buf, "%2d%2d%f", &H, &M, &S);
+	printf("time: %02d:%02d:%05.2f", H, M, S);
+	NEXT();
+	sscanf((char*)buf, "%2d%f", &LA, &lattitude);
+	NEXT();
+	if(*buf != ','){
+		north = *buf;
+		lattitude = (float)LA + lattitude / 60.;
+		if(north == 'S') lattitude = -lattitude;
+		printf(" latt: %f", lattitude);
+	}
+	NEXT();
+	sscanf((char*)buf, "%3d%f", &LO, &longitude);
+	NEXT();
+	if(*buf != ','){
+		east = *buf;
+		longitude = (float)LO + longitude / 60.;
+		if(east == 'W') longitude = -longitude;
+		printf(" long: %f", longitude);
+	}
+	NEXT();
+#define FSCAN(par, nam) do{if(*buf != ','){sscanf((char*)buf, "%f", &par); \
+		printf(" " nam ": %f", par);} NEXT();}while(0)
+	FSCAN(altitude, "altitude");
+	if(*buf != ','){
+		printf(" nav. status: %c%c", buf[0], buf[1]);
+	}
+	NEXT();
+	FSCAN(hacc,"hor.accuracy");
+	FSCAN(vacc, "vert.accuracy");
+	FSCAN(speed,"speed");
+	FSCAN(track,"cource");
+	FSCAN(vertspd,"vertical speed ('+'-down)");
+	FSCAN(age,"DGPS age");
+	FSCAN(hdop, "hor. dilution");
+	FSCAN(vdop, "vert. dilution");
+	FSCAN(tdop, "time dilution");
+#undef FSCAN
+#define ISCAN(par, nam) do{if(*buf != ','){sscanf((char*)buf, "%d", &par); \
+		printf(" " nam ": %d", par);} NEXT();}while(0)
+	ISCAN(gps, "GPS used");
+	ISCAN(glo, "GLONASS used");
+	ISCAN(dr, "DR used");
+#undef ISCAN
+ret:
+	printf("\n");
 }
 
 /**
  * Parce content of buffer with GPS data
  * WARNING! This function changes data content
  */
-void parce_data(char *buf){
+void parce_data(uint8_t *buf){
-	char *eol;
+	uint8_t *eol;
-	while(buf && (eol = strchr(buf, '\r'))){
+	//DBG("GOT: %s", buf);
+	while(*buf && (eol = (uint8_t*)strchr((char*)buf, '\r'))){
 		*eol = 0;
 		// now make checksum checking:
 		if(!checksum(buf)) goto cont;
-		if(strncmp(buf, "$GP", 3)){
+		if(strncmp((char*)buf, "$GP", 3)){
-			DBG("Bad string: %s\n", buf);
+			if(strncmp((char*)buf, "$PUBX,00,", 9) == 0){
+				buf += 9;
+				pubx(buf);
+			}else{
+				DBG("Bad string: %s\n", buf);
+			}
 			goto cont;
 		}
 		buf += 3;
@@ -190,13 +492,13 @@ void parce_data(char *buf){
 		switch(*buf){
 			case 'G': // GSV, GSA, GGA, GLL
 				++buf;
-				if(strncmp(buf, "SV", 2) == 0){
+				if(strncmp((char*)buf, "SV", 2) == 0){
 					gsv(buf+3);
-				}else if(strncmp(buf, "SA", 2) == 0){
+				}else if(strncmp((char*)buf, "SA", 2) == 0){
 					gsa(buf+3);
-				}else if(strncmp(buf, "GA", 2) == 0){
+				}else if(strncmp((char*)buf, "GA", 2) == 0){
 					gga(buf+3);
-				}else if(strncmp(buf, "LL", 2) == 0){
+				}else if(strncmp((char*)buf, "LL", 2) == 0){
 					gll(buf+3);
 				}else{
 					DBG("Unknown: $GPG%s", buf);
@@ -205,7 +507,7 @@ void parce_data(char *buf){
 			break;
 			case 'R': // RMC
 				++buf;
-				if(strncmp(buf, "MC", 2) == 0){
+				if(strncmp((char*)buf, "MC", 2) == 0){
 					rmc(buf+3);
 				}else{
 					DBG("Unknown: $GPR%s", buf);
@@ -214,7 +516,7 @@ void parce_data(char *buf){
 			break;
 			case 'V': // VTG
 				++buf;
-				if(strncmp(buf, "TG", 2) == 0){
+				if(strncmp((char*)buf, "TG", 2) == 0){
 					vtg(buf+3);
 				}else{
 					DBG("Unknown: $GPV%s", buf);
@@ -223,7 +525,7 @@ void parce_data(char *buf){
 			break;
 			case 'T': // TXT
 				++buf;
-				if(strncmp(buf, "XT", 2) == 0){
+				if(strncmp((char*)buf, "XT", 2) == 0){
 					txt(buf+3);
 				}else{
 					DBG("Unknown: $GPT%s", buf);
@@ -234,13 +536,26 @@ void parce_data(char *buf){
 				DBG("Unknown: $GP%s", buf);
 				goto cont;
 		}
-	//	printf("GOT: %s\n", buf);
 	cont:
 		if(eol[1] != '\n') break;
 		buf = eol + 2;
 	}
 }
 
+/**
+ * set rate for given NMEA field
+ * @param field - name of NMEA field
+ * @param rate  - rate in seconds (0 disables field)
+ * @return -1 if fails, rate if OK
+ */
+int nmea_fieldrate(uint8_t *field, int rate){
+	uint8_t buf[256];
+	if(rate < 0) return -1;
+	snprintf((char*)buf, 255, "$PUBX,40,%s,0,%d,0,0*", field, rate);
+	if(write_with_checksum(buf)) return rate;
+	else return -1;
+}
+
 int main(int argc, char **argv){
 	check4running(argv, PIDFILE, NULL);
 	initial_setup();
@@ -253,11 +568,30 @@ int main(int argc, char **argv){
 	signal(SIGINT, signals);  // ctrl+C - quit
 	signal(SIGQUIT, signals); // ctrl+\ - quit
 	signal(SIGTSTP, SIG_IGN); // ignore ctrl+Z
-	double T0 = dtime();
+	int i;
-	char *str = NULL;
+	for(i = 0; i < GPMAXMSG; ++i){
-	while(dtime() - T0 < 10.){
+		int block = 0;
-		if((str = get_portdata()))
+		if(GP->block_msg[i])
+			block = 1; // unblock message
+		if(nmea_fieldrate((uint8_t*)GPmsgs[i], block) != block)
+			WARN("Can't %sblock %s!", block?"un":"", GPmsgs[i]);
+		else
+			printf("%sblock %s\n", block?"un":"", GPmsgs[i]);
+	}
+	if(GP->stationary){
+		printf("stationary config\n");
+		cfg_stationary();
+	}
+	double T, T0 = dtime(), Tpoll = 0., tmout = GP->polltmout;
+	uint8_t *str = NULL;
+	while((T=dtime()) - T0 < tmout){
+		if(GP->pollubx && T-Tpoll > GP->pollinterval){
+			Tpoll = T;
+			write_tty((uint8_t*)"$PUBX,00*33\r\n", 13);
+		}
+		if((str = get_portdata())){
 			parce_data(str);
+		}
 	}
 	signals(0);
 	return 0;

GPS/usefull_macros.c

@@ -294,7 +294,7 @@ void tty_init(char *comdev){
  * @param length   - buffer len
  * @return amount of readed bytes
  */
-size_t read_tty(char *buff, size_t length){
+size_t read_tty(uint8_t *buff, size_t length){
 	ssize_t L = 0;
 	fd_set rfds;
 	struct timeval tv;
@@ -309,3 +309,12 @@ size_t read_tty(char *buff, size_t length){
 	}
 	return (size_t)L;
 }
+
+int write_tty(uint8_t *buff, size_t length){
+	ssize_t L = write(comfd, buff, length);
+	if((size_t)L != length){
+		WARN("Write error!");
+		return 1;
+	}
+	return 0;
+}

GPS/usefull_macros.h

@@ -117,6 +117,7 @@ int mygetchar();
 
 void restore_tty();
 void tty_init(char *comdev);
-size_t read_tty(char *buff, size_t length);
+size_t read_tty(uint8_t *buff, size_t length);
+int write_tty(uint8_t *buff, size_t length);
 
 #endif // __USEFULL_MACROS_H__