This shows you the differences between two versions of the page.
Both sides previous revision Previous revision Next revision | Previous revision | ||
sdk:scripts:gps-udp-client [2017/10/11 16:03] fachet |
sdk:scripts:gps-udp-client [2022/09/05 12:04] (current) 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); | + | if (connect(gpsd, "127.0.0.1", 2947) < 0) { |
- | continue; | + | nb_syslog("Could not connect to daemon"); |
- | } | + | |
- | nb_syslog("Requesting raw NMEA stream"); | + | if (attempt == 3) { |
- | if (send(sock, "R\n") < 0) { | + | nb_syslog("Unable to connect, restarting daemon"); |
- | nb_syslog("Unable to request raw NMEA stream"); | + | gpsd_restart(); |
- | close(sock); | + | } |
- | continue; | + | } else { |
+ | break; | ||
} | } | ||
+ | } | ||
+ | nb_syslog("Connected to daemon, requesting NMEA"); | ||
+ | send(gpsd,"R=1\n"); | ||
- | nb_syslog("Successfully connected to GPS daemon"); | + | nb_syslog("Processing NMEA"); |
- | return sock; | + | |
- | } | + | |
- | 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; |
- | sent = sendto(server, data, SERVER, PORT); | + | |
- | if (sent != len) { | + | |
- | nb_syslog("Unable to send message to %s:%d", SERVER, PORT); | + | |
} | } | ||
} | } | ||
- | } | ||
- | close(gpsd); | + | /* wait for client socket data */ |
- | close(server); | + | rv = select(gpsd, 3); |
- | exit(0); | + | if (rv == -1) { |
- | + | nb_syslog("ERROR: select failed, re-connecting"); | |
- | </code> | + | close(gpsd); |
- | + | gpsd = -1; | |
- | ====== SDK Script gps-udp-broadcast.are ====== | + | |
- | <code c gps-udp-broadcast.are> | + | |
- | /* DESC: This script sends the local GPS NMEA via UDP broadcast to multiple ports | + | |
- | * selected by NMEA type. | + | |
- | * Copyright (C) 2013-2017 NetModule AG, Switzerland | + | |
- | */ | + | |
- | BroadcastIP = "192.168.1.255"; | + | |
- | + | ||
- | // Connect to GPS Deamon | + | |
- | int connect_gpsd() { | + | |
- | nb_syslog("Connecting to GPS daemon."); | + | |
- | sock = -1; | + | |
- | while(true) | + | |
- | { | + | |
- | sock = socket(AF_INET, SOCK_STREAM, IPPROTO_TCP); | + | |
- | if (sock < 0) | + | |
- | { | + | |
- | nb_syslog("Unable to open TCP socket. Retrying in 5 seconds\n"); | + | |
- | sleep(5); | + | |
continue; | continue; | ||
- | } | + | } else if (rv == 0) { |
- | + | /* nothing received */ | |
- | if (connect(sock, "127.0.0.1", 2947) < 0) | + | |
- | { | + | |
- | nb_syslog("Could not connect to GPS daemon. Retrying in 5 seconds\n"); | + | |
- | close(sock); | + | |
- | sleep(5); | + | |
continue; | continue; | ||
- | } | + | } |
- | nb_syslog("GPS daemon connected."); | + | data = recv(gpsd); |
- | return(sock); | + | len = strlen(data); |
- | break; | + | |
+ | 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); | ||
} | } | ||
} | } | ||
- | + | if (gpsd > -1) close(gpsd); | |
+ | if (server > -1) close(server); | ||
- | gpsd = connect_gpsd(); | + | exit(-1); |
- | /* open UDP server socket */ | ||
- | server = socket(AF_INET, SOCK_DGRAM, IPPROTO_UDP); | ||
- | if (server < 0) { | ||
- | nb_syslog("Unable to open server socket."); | ||
- | close(gpsd); | ||
- | exit(-1); | ||
- | } | ||
- | setsockopt(server, SOL_SOCKET, SO_BROADCAST, 1); | ||
- | nb_syslog("GPS Broadcast to %s", BroadcastIP); | ||
- | // process NMEA stream | ||
- | |||
- | nb_syslog("Processing NMEA data"); | ||
- | |||
- | while (true) { | ||
- | /* wait for socket data */ | ||
- | rv = select(gpsd, 3); | ||
- | if (rv > 0) { | ||
- | 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) | ||
- | exit(-2); | ||
- | } else { | ||
- | // printf("got >>>%s<<<\n", data); | ||
- | while(true) //handle CR /LF problems and separate data into singel $Gxxx frames | ||
- | { | ||
- | newlinepos = strstr(data, "\r\n"); | ||
- | seplen = 0; | ||
- | if (newlinepos != ()) { | ||
- | seplen = 2; | ||
- | } else { | ||
- | newlinepos = strstr(data, "\n"); | ||
- | if (newlinepos != ()) { | ||
- | seplen = 1; | ||
- | } | ||
- | } | ||
- | | ||
- | if (seplen != 0) { | ||
- | frame = strcat(left(data, newlinepos), "\r\n"); | ||
- | // printf("frame >>>%s<<<\n", frame); | ||
- | if (left(frame, 6) == "$GPRMC") { | ||
- | //printf("send $GPRMC\n"); | ||
- | if (sendto(server, frame, BroadcastIP, 5125) != len) | ||
- | nb_syslog("Unable to broadcast message $GPRMC to port 5125"); | ||
- | if (sendto(server, frame, BroadcastIP, 5126) != len) | ||
- | nb_syslog("Unable to broadcast message $GPRMC to port 5126"); | ||
- | if (sendto(server, frame, BroadcastIP, 6060) != len) | ||
- | nb_syslog("Unable to broadcast message $GPRMC to port 6060"); | ||
- | } | ||
- | if (left(frame, 6) == "$GPGGA") { | ||
- | //printf("send $GPGGA\n"); | ||
- | if (sendto(server, frame, BroadcastIP, 5126) != len) | ||
- | nb_syslog("Unable to broadcast message $GPGGA to port 5126"); | ||
- | } | ||
- | if (left(frame, 6) == "$GPGSA") { | ||
- | //printf("send $GPGSA\n"); | ||
- | if (sendto(server, frame, BroadcastIP, 5126) != len) | ||
- | nb_syslog("Unable to broadcast message $GPGGA to port 5126"); | ||
- | } | ||
- | data = substr(data, newlinepos + seplen); | ||
- | } | ||
- | else | ||
- | break; | ||
- | | ||
- | } | ||
- | } | ||
- | } | ||
- | exit(-3); | ||
</code> | </code> | ||
+ | |||