Differences
This shows you the differences between two versions of the page.
| — |
sdk:gps-udp-broadcast [2017/10/11 16:34] (current) fachet created |
||
|---|---|---|---|
| Line 1: | Line 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) 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"); | ||
| + | sleep(5); | ||
| + | continue; | ||
| + | } | ||
| + | |||
| + | if (connect(sock, "127.0.0.1", 2947) < 0) | ||
| + | { | ||
| + | nb_syslog("Could not connect to GPS daemon. Retrying in 5 seconds"); | ||
| + | close(sock); | ||
| + | sleep(5); | ||
| + | continue; | ||
| + | } | ||
| + | nb_syslog("GPS daemon connected."); | ||
| + | return(sock); | ||
| + | break; | ||
| + | } | ||
| + | } | ||
| + | |||
| + | |||
| + | |||
| + | gpsd = connect_gpsd(); | ||
| + | |||
| + | /* 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") { | ||
| + | sendto(server, frame, BroadcastIP, 5125); | ||
| + | sendto(server, frame, BroadcastIP, 5126); | ||
| + | sendto(server, frame, BroadcastIP, 6060); | ||
| + | } | ||
| + | if (left(frame, 6) == "$GPGGA") { | ||
| + | sendto(server, frame, BroadcastIP, 5126); | ||
| + | } | ||
| + | if (left(frame, 6) == "$GPGSA") { | ||
| + | sendto(server, frame, BroadcastIP, 5126); | ||
| + | } | ||
| + | data = substr(data, newlinepos + seplen); | ||
| + | } | ||
| + | else | ||
| + | break; | ||
| + | | ||
| + | } | ||
| + | } | ||
| + | } | ||
| + | exit(-3); | ||
| + | </code> | ||