mirror of
https://github.com/eddyem/eddys_snippets.git
synced 2025-12-06 02:35:12 +03:00
added some settings changes
This commit is contained in:
eddyem
parent
451da2be50
commit
5db2728d5e
@ -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
|
||||
|
||||
@ -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);
|
||||
|
||||
442
GPS/main.c
442
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(){
|
||||
static char buf[1025];
|
||||
char *ptr = buf;
|
||||
uint8_t *get_portdata(){
|
||||
static uint8_t buf[1025];
|
||||
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){
|
||||
char *eol;
|
||||
char chs[2];
|
||||
unsigned char checksum = 0;
|
||||
if(*buf != '$' || !(eol = strchr(buf, '*'))){
|
||||
int write_with_checksum(uint8_t *buf){
|
||||
static char CS[3];
|
||||
//uint8_t *ptr = buf;
|
||||
uint8_t checksum = 0;
|
||||
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);
|
||||
snprintf(chs, 2, "%X", checksum);
|
||||
if(!strncmp(chs, ++buf, 2)) return 0;
|
||||
checksum ^= *buf;
|
||||
snprintf(chs, 3, "%X", checksum);
|
||||
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);
|
||||
buf = strchr(buf, ',')+1;
|
||||
sscanf((char*)buf, "%2d%2d%f", &H, &M, &S);
|
||||
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;
|
||||
sscanf(buf, "%2d%f", &LA, &lattitude);
|
||||
buf = strchr(buf, ',')+1;
|
||||
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);
|
||||
}
|
||||
buf = strchr(buf, ',')+1;
|
||||
sscanf(buf, "%3d%f", &LO, &longitude);
|
||||
buf = strchr(buf, ',')+1;
|
||||
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);
|
||||
}
|
||||
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;
|
||||
if(sscanf(buf, "%2d%2d%2d", &d, &m, &y) == 3)
|
||||
NEXT();
|
||||
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;
|
||||
sscanf(buf, "%f,%c*", &mag, &mageast);
|
||||
NEXT();
|
||||
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){
|
||||
//DBG("txt: %s\n", buf);
|
||||
void txt(_U_ uint8_t *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){
|
||||
char *eol;
|
||||
while(buf && (eol = strchr(buf, '\r'))){
|
||||
void parce_data(uint8_t *buf){
|
||||
uint8_t *eol;
|
||||
//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)){
|
||||
DBG("Bad string: %s\n", buf);
|
||||
if(strncmp((char*)buf, "$GP", 3)){
|
||||
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();
|
||||
char *str = NULL;
|
||||
while(dtime() - T0 < 10.){
|
||||
if((str = get_portdata()))
|
||||
int i;
|
||||
for(i = 0; i < GPMAXMSG; ++i){
|
||||
int block = 0;
|
||||
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;
|
||||
|
||||
@ -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;
|
||||
}
|
||||
|
||||
@ -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__
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user