NetModule Router Wiki

User Tools

Site Tools

Differences

This shows you the differences between two versions of the page.

Link to this comparison view

Next revision		Previous revision
sdk:gps-tcp-server-gnsstogps [2020/04/28 18:08] – created jurascheksdk:gps-tcp-server-gnsstogps [2020/07/21 14:33] (current) – [tcp server proxy script] fachet
Line 57: Line 57:
  
 void usage() { void usage() {
- printf("usage: gps-udp-client.are <server> <port>\n"); +    printf("usage: gps-udp-client.are <server> <port>\n"); 
- exit(1);+    exit(1);
 } }
  
 int gpsd_restart () int gpsd_restart ()
 { {
- nb_syslog("Restarting GPS daemon");+    nb_syslog("Restarting GPS daemon");
  
- nb_config_set("gpsd.0.status=0"); +    nb_config_set("gpsd.0.status=0"); 
- sleep(3); +    sleep(3); 
- nb_config_set("gpsd.0.status=1"); +    nb_config_set("gpsd.0.status=1"); 
- sleep(5);+    sleep(5);
  
- return 0;+    return 0;
 } }
  
 int gpsd_connect () int gpsd_connect ()
 { {
- nb_syslog("Connecting to GPS daemon");+    nb_syslog("Connecting to GPS daemon");
  
- gpsd = socket(AF_INET, SOCK_STREAM, IPPROTO_TCP); +    gpsd = socket(AF_INET, SOCK_STREAM, IPPROTO_TCP); 
- if (gpsd < 0) { +    if (gpsd < 0) { 
- nb_syslog("ERROR: Unable to open socket"); +        nb_syslog("ERROR: Unable to open socket"); 
- exit(-1); +        exit(-1); 
- } +    } 
- for (attempt = 0; attempt <= 5; attempt++) { +    for (attempt = 0; attempt <= 5; attempt++) { 
- sleep(3);+        sleep(3);
  
- if (connect(gpsd, local_ip, local_port) < 0) { +        if (connect(gpsd, local_ip, local_port) < 0) { 
- nb_syslog("Could not connect to daemon"); +            nb_syslog("Could not connect to daemon"); 
- if (attempt == 3) { +            if (attempt == 3) { 
- nb_syslog("Unable to connect, restarting daemon"); +                nb_syslog("Unable to connect, restarting daemon"); 
- gpsd_restart(); +                gpsd_restart(); 
- } +            } 
- } else { +        } else { 
- break; +            break; 
- } +        } 
- } +    } 
- nb_syslog("Connected to daemon, requesting NMEA"); +    nb_syslog("Connected to daemon, requesting NMEA"); 
- if (nb_config_get("gpsd.0.cmode") == "0") send(gpsd, "R=1\n");+    if (nb_config_get("gpsd.0.cmode") == "0") send(gpsd, "R=1\n");
  
- nb_syslog("Processing NMEA");+    nb_syslog("Processing NMEA");
  
- return gpsd;+    return gpsd;
 } }
  
 string checksum(string s) string checksum(string s)
 { {
- pos = strchr(s, "*");+    pos = strchr(s, "*");
  
-  if (is_void(pos)) +    if (is_void(pos)) 
- return s; /* no checksum to correct */+        return s; /* no checksum to correct */
  
- cs = 0; +    cs = 0; 
- chars = explode(s);+    chars = explode(s);
         for (i = 1; i < pos; i++) { /* skip $ and *XX */          for (i = 1; i < pos; i++) { /* skip $ and *XX */ 
-     c = ord(chars[i]);+ c = ord(chars[i]);
             cs ^= c;             cs ^= c;
         }         }
         r = sprintf("%s%02X%s", substr(s, 0, pos + 1), cs, substr(s, pos + 3));         r = sprintf("%s%02X%s", substr(s, 0, pos + 1), cs, substr(s, pos + 3));
- return r;+    return r;
 } }
  
 /* main() */ /* main() */
  
-if (argc < 2) {+if (argc < 2)
     usage();     usage();
-} 
- 
  
 /* open TCP server socket */ /* open TCP server socket */
 server = socket(AF_INET, SOCK_STREAM, IPPROTO_TCP); server = socket(AF_INET, SOCK_STREAM, IPPROTO_TCP);
 if (server < 0) { if (server < 0) {
- nb_syslog("Unable to open tcp socket"); +    nb_syslog("Unable to open tcp socket"); 
- exit(-1);+    exit(-1);
 } }
- 
  
 ret = bind(server, server_port, ""); ret = bind(server, server_port, "");
Line 145: Line 142:
 if (ret == -1) { if (ret == -1) {
     printf("Unable to listen\n");     printf("Unable to listen\n");
-    close(sock); +      close(sock); 
-    exit(1); +      exit(1);
 } }
  
Line 154: Line 150:
     client = accept(server);     client = accept(server);
     if (client < 0) {     if (client < 0) {
-        nb_syslog("accept failed\n");+ nb_syslog("accept failed\n");
         sleep(1);         sleep(1);
         continue;         continue;
-    }  else { +    } else { 
- nb_syslog("new client connection: %i",client); +        nb_syslog("new client connection: %i",client); 
-     clients=mkarray(client); +        clients=mkarray(client); 
- } +    } 
     clientConnection=true;     clientConnection=true;
     nb_syslog("New client connected\n");     nb_syslog("New client connected\n");
  
  
- // Sending NMEA Loop+    // Sending NMEA Loop
  
- /* process NMEA stream */ +    /* process NMEA stream */ 
- nb_syslog("Processing NMEA data"); +    nb_syslog("Processing NMEA data"); 
- while(clientConnection) { +    while (clientConnection) { 
- /* connect to gpsd */ +        /* connect to gpsd */ 
- if(gpsd < 0) { +        if (gpsd < 0) { 
- gpsd = gpsd_connect(gpsd); +            gpsd = gpsd_connect(gpsd); 
- if(gpsd < 0) {+            if (gpsd < 0) {
  nb_syslog("ERROR: Unable to connect to daemon");  nb_syslog("ERROR: Unable to connect to daemon");
  sleep(5);  sleep(5);
- continue; +                continue; 
- } +            } 
- }+        }
  
- /* wait for client socket data  */ +        /* wait for client socket data  */ 
- rv = select(array_merge(clients,mkarray(gpsd,server)), 1);+        rv = select(array_merge(clients,mkarray(gpsd,server)), 1);
  
- if(rv == -1) { +        if (rv == -1) { 
- nb_syslog("ERROR: select failed, re-connecting"); +            nb_syslog("ERROR: select failed, re-connecting"); 
- close(gpsd); +            close(gpsd); 
- gpsd = -1; +            gpsd = -1; 
- continue; +            continue; 
- } else if(rv == 0) { +        } else if (rv == 0) { 
- /* nothing received */ +            /* nothing received */ 
- continue; +            continue; 
- } else if(rv == server) {+        } else if(rv == server) {
  newclient = accept(server);  newclient = accept(server);
  if (newclient < 0) {  if (newclient < 0) {
- nb_syslog("accept failed\n"); + nb_syslog("accept failed\n"); 
- continue;+ continue;
  } else {  } else {
- nb_syslog("new client connection: %i",newclient); +                nb_syslog("new client connection: %i",newclient); 
- clients=array_merge(clients,mkarray(newclient));+                clients=array_merge(clients,mkarray(newclient));
  
-     } +            } 
- }+        }
                  
 +        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"); 
- data = recv(gpsd); +            close(gpsd); 
- len = strlen(data); +            gpsd = -1; 
- /* Check for correct framing: CR+LF */ +            continue; 
- if(len > 2) { +        } 
- if(strrchr(data, "\r") != len-2 || strrchr(data, "\n") != len-1) { +        /* Convert message $GN to $GP for backward compatibility */ 
- nb_syslog("ERROR: NMEA framing wrong"); +        if (strstr(data, "$GN") == 0) {; 
- close(gpsd); +            a_arr = explode(data); 
- gpsd = -1; +            a_arr[2] = "P"; 
- gpsd_restart(); +            data = checksum(implode(a_arr)); 
- continue; +        } 
- } +        /* skip all messages different from GPGGA  
- } +        * if(strstr(data,"$GPGGA")==()) 
- +        * continue; 
- if(len == 0) { +        */  
- nb_syslog("ERROR: no data, re-connecting"); +        for (i=0;i<length(clients);i++) { 
- close(gpsd); + sent = send(clients[i], data); 
- gpsd = -1; + if (sent < 0) { 
- continue; + nb_syslog("ERROR: Unable to send, removing client (%i)",clients[i]);  
- } + clients=array_compact(array_unset(clients,i)); 
- /* 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; +
- */  +
-     for (i=0;i<length(clients);i++) { +
- sent = send(clients[i], data ); +
- if(sent < 0 ) { +
- nb_syslog("ERROR: Unable to send, removing client (%i)",clients[i]);  +
-            clients=array_compact(array_unset(clients,i)); +
- }  +
- }  +
- }+
  
  
Line 248: Line 241:
 } }
  
-if (gpsd > -1) { +if (gpsd > -1) 
- close(gpsd); +    close(gpsd); 
-} +if (server > -1) 
-if (server > -1) { +    close(server);
- close(server); +
-}+
 exit(-1); exit(-1);
- 
- 
 </code> </code>
  

Page Tools

sdk/gps-tcp-server-gnsstogps.1588097296.txt.gz · Last modified: 2020/04/28 18:08 by juraschek