SDK Script gps-udp-client-GNSStoGPS.are

gps-udp-client-gnsstogps.are
/* DESC: This script sends the local GPS NMEA stream to a remote UDP server.
* DESC: It checks for the correct CR/LF end of line characters and convert messages $GN to $GP 
* Copyright (C) 2013-2019) NetModule AG, Switzerland
*/
 
/* default values */
local_ip = "127.0.0.1";
local_port = 2947; /* would be better to take it from the configuaration */
 
gpsd = -1;
 
 
void usage() {
	printf("usage: gps-udp-client.are <server> <port>\n");
	exit(1);
}
 
int gpsd_restart ()
{
	nb_syslog("Restarting GPS daemon");
 
	nb_config_set("gpsd.0.status=0");
	sleep(3);
	nb_config_set("gpsd.0.status=1");
	sleep(5);
 
	return 0;
}
 
int gpsd_connect ()
{
	nb_syslog("Connecting to GPS daemon");
 
	gpsd = socket(AF_INET, SOCK_STREAM, IPPROTO_TCP);
	if (gpsd < 0) {
		nb_syslog("ERROR: Unable to open socket");
		exit(-1);
	}
	for (attempt = 0; attempt <= 5; attempt++) {
		sleep(3);
 
		if (connect(gpsd, local_ip, local_port) < 0) {
			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 gpsd;
}
 
string checksum(string s)
{
	pos = strchr(s, "*");
 
  	if (is_void(pos))
		return s; /* no checksum to correct */
 
	cs = 0;
	chars = explode(s);
        for (i = 1; i < pos; i++) { /* skip $ and *XX */ 
	    c = ord(chars[i]);
            cs ^= c;
        }
        r = sprintf("%s%02X%s", substr(s, 0, pos + 1), cs, substr(s, pos + 3));
	return r;
}
 
/* main() */
 
if (argc < 2) {
    usage();
}
 
SERVER = trim((string) argv[1]);
PORT = (int) argv[2];
 
/* open UDP server socket */
server = socket(AF_INET, SOCK_DGRAM, IPPROTO_UDP);
if (server < 0) {
	nb_syslog("Unable to open socket");
	exit(-1);
}
 
/* process NMEA stream */
nb_syslog("Processing NMEA data");
while(1) {
	/* connect to gpsd */
	if(gpsd < 0) {
		gpsd = gpsd_connect(gpsd);
		if(gpsd < 0) {
			nb_syslog("ERROR: Unable to connect to daemon");
			sleep(5);
			continue;
		}
	}
 
	/* 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);
	/* Check for correct framing: CR+LF */
	if(len > 2) {
		if(strrchr(data, "\r") != len-2 || strrchr(data, "\n") != len-1) {
			nb_syslog("ERROR: NMEA framing wrong");
			close(gpsd);
			gpsd = -1;
			gpsd_restart();
			continue;
		}
	}
 
	if(len == 0) {
		nb_syslog("ERROR: no data, re-connecting");
		close(gpsd);
		gpsd = -1;
		continue;
	}
	/* Convert message $GN to $GP for backward compatibility */
	if(strstr(data, "$GN") == 0) {;
		a_arr = explode(data);
		a_arr[2] = "P";
		data = checksum(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("ERROR: Unable to send %d bytes to %s:%d", len, SERVER, PORT);
	}
}
 
if (gpsd > -1)	{
	close(gpsd);
}
if (server > -1) {
	close(server);
}
exit(-1);