This shows you the differences between two versions of the page.
Both sides previous revision Previous revision | |||
sdk:scripts:gps-udp-client [2019/07/08 12:47] fachet |
sdk:scripts:gps-udp-client [2022/09/05 12:04] schmitt |
||
---|---|---|---|
Line 6: | Line 6: | ||
*/ | */ | ||
- | void usage() | + | void usage() { |
+ | printf("usage: gps-udp-client.are <server> <port>\n"); | ||
+ | exit(1); | ||
+ | } | ||
+ | |||
+ | int gpsd_restart () | ||
{ | { | ||
- | nb_syslog("Usage: gps-udp-client.are <server> <port>"); | + | nb_syslog("Restarting GPS daemon"); |
- | exit(1); | + | |
+ | nb_config_set("gpsd.0.status=0"); | ||
+ | sleep(3); | ||
+ | nb_config_set("gpsd.0.status=1"); | ||
+ | sleep(5); | ||
+ | |||
+ | return 0; | ||
} | } | ||
- | int connect_gpsd () | + | int gpsd_connect () |
{ | { | ||
- | for (attempt = 0; attempt < 60; attempt++) { | + | nb_syslog("Connecting to GPS daemon"); |
- | sleep(1); | + | |
- | sock = socket(AF_INET, SOCK_STREAM, IPPROTO_TCP); | + | gpsd = socket(AF_INET, SOCK_STREAM, IPPROTO_TCP); |
- | if (sock < 0) { | + | if (gpsd < 0) { |
- | nb_syslog("Unable to create socket"); | + | nb_syslog("ERROR: Unable to open socket"); |
- | continue; | + | exit(-1); |
- | } | + | } |
- | nb_syslog("Connecting to GPS daemon"); | + | for (attempt = 0; attempt <= 5; attempt++) { |
- | if (connect(sock, "127.0.0.1", 2947) < 0){ | + | sleep(3); |
- | nb_syslog("Could not connect to GPS daemon"); | + | |
- | close(sock); | + | |
- | continue; | + | |
- | } | + | |
- | nb_syslog("Requesting raw NMEA stream"); | + | |
- | if (send(sock, "R\n") < 0) { | + | |
- | nb_syslog("Unable to request raw NMEA stream"); | + | |
- | close(sock); | + | |
- | continue; | + | |
- | } | + | |
- | nb_syslog("Successfully connected to GPS daemon"); | + | if (connect(gpsd, "127.0.0.1", 2947) < 0) { |
- | return sock; | + | nb_syslog("Could not connect to daemon"); |
+ | |||
+ | if (attempt == 3) { | ||
+ | nb_syslog("Unable to connect, restarting daemon"); | ||
+ | gpsd_restart(); | ||
+ | } | ||
+ | } else { | ||
+ | break; | ||
+ | } | ||
} | } | ||
+ | nb_syslog("Connected to daemon, requesting NMEA"); | ||
+ | send(gpsd,"R=1\n"); | ||
+ | |||
+ | nb_syslog("Processing NMEA"); | ||
- | return -1; | + | return gpsd; |
} | } | ||
if (argc < 2) { | if (argc < 2) { | ||
- | usage(); | + | usage(); |
} | } | ||
SERVER = trim((string) argv[1]); | SERVER = trim((string) argv[1]); | ||
PORT = (int) argv[2]; | PORT = (int) argv[2]; | ||
- | + | gpsd = -1; | |
- | gpsd = connect_gpsd(); | + | |
- | if (gpsd < 0) { | + | |
- | exit(1); | + | |
- | } | + | |
/* open UDP server socket */ | /* open UDP server socket */ | ||
server = socket(AF_INET, SOCK_DGRAM, IPPROTO_UDP); | server = socket(AF_INET, SOCK_DGRAM, IPPROTO_UDP); | ||
if (server < 0) { | if (server < 0) { | ||
- | nb_syslog("Unable to open server socket"); | + | nb_syslog("Unable to open socket"); |
- | close(gpsd); | + | exit(-1); |
- | exit(1); | + | |
} | } | ||
/* process NMEA stream */ | /* process NMEA stream */ | ||
nb_syslog("Processing NMEA data"); | nb_syslog("Processing NMEA data"); | ||
- | |||
while (1) { | while (1) { | ||
- | /* wait for socket data */ | + | /* connect to gpsd */ |
- | rv = select(gpsd, 3); | + | if (gpsd < 0) { |
- | if (rv > 0) { | + | gpsd = gpsd_connect(gpsd); |
- | data = recv(gpsd); | + | |
- | len = strlen(data); | + | |
- | } else if (rv < 0) { | + | |
- | nb_syslog("select failed"); | + | |
- | len = -1; | + | |
- | } else { | + | |
- | /* nothing received */ | + | |
- | continue; | + | |
- | } | + | |
- | if (len <= 0) { | + | |
- | nb_syslog("Receive failed, re-connecting to GPS daemon"); | + | |
- | close(gpsd); | + | |
- | gpsd = connect_gpsd(); | + | |
if (gpsd < 0) { | if (gpsd < 0) { | ||
- | exit(1); | + | nb_syslog("ERROR: Unable to connect to daemon"); |
- | } | + | sleep(5); |
- | } else { | + | continue; |
- | + | ||
- | /* Convert message $GN to $GP */ | + | |
- | /* if (strstr(data,"$GNGGA")==0) { | + | |
- | * a_arr = explode(data); | + | |
- | * a_arr[2]="P"; | + | |
- | * data = implode(a_arr); | + | |
- | * } | + | |
- | */ | + | |
- | /* skip all messages different from GPGGA */ | + | |
- | /* if (strstr(data,"$GPGGA")==()) | + | |
- | * continue; | + | |
- | */ | + | |
- | sent = sendto(server, data, SERVER, PORT); | + | |
- | if (sent != len) { | + | |
- | nb_syslog("Unable to send message to %s:%d", SERVER, PORT); | + | |
} | } | ||
+ | } | ||
+ | |||
+ | /* wait for client socket data */ | ||
+ | rv = select(gpsd, 3); | ||
+ | |||
+ | if (rv == -1) { | ||
+ | nb_syslog("ERROR: select failed, re-connecting"); | ||
+ | close(gpsd); | ||
+ | gpsd = -1; | ||
+ | continue; | ||
+ | } else if (rv == 0) { | ||
+ | /* nothing received */ | ||
+ | continue; | ||
+ | } | ||
+ | data = recv(gpsd); | ||
+ | len = strlen(data); | ||
+ | |||
+ | if (len == 0) { | ||
+ | nb_syslog("ERROR: no data, re-connecting"); | ||
+ | close(gpsd); | ||
+ | gpsd = -1; | ||
+ | continue; | ||
+ | } | ||
+ | |||
+ | sent = sendto(server, data, SERVER, PORT); | ||
+ | if (sent != len) { | ||
+ | nb_syslog("ERROR: Unable to send %d bytes to %s:%d", len, SERVER, PORT); | ||
} | } | ||
} | } | ||
- | close(gpsd); | + | if (gpsd > -1) close(gpsd); |
- | close(server); | + | if (server > -1) close(server); |
+ | |||
+ | exit(-1); | ||
- | exit(0); | ||
</code> | </code> | ||