NetModule Router Wiki

User Tools

Site Tools

Differences

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

Link to this comparison view

Both sides previous revisionPrevious revision
Next revision		Previous revision
sdk:gps-tcp-server-gnsstogps [2020/07/21 14:23] – [tcp server proxy script] fachetsdk: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.1595341414.txt.gz · Last modified: 2020/07/21 14:23 by fachet