Differences

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

Link to this comparison view

Both sides previous revision Previous revision
sdk:scripts:gps-udp-client [2019/07/08 12:47]
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 = 0attempt ​<= 5; attempt++) { 
-        if (connect(sock,​ "​127.0.0.1", 2947) < 0){ +        sleep(3);
-            nb_syslog("​Could not connect to GPS daemon"​); +
-            ​close(sock);​ +
-            continue; +
-        ​+
-        ​nb_syslog("​Requesting raw NMEA stream"​); +
-        if (send(sock, "​R\n"​) ​0) { +
-            ​nb_syslog("​Unable to request raw NMEA stream"​); +
-            close(sock);​ +
-            continue; +
-        }+
  
-        nb_syslog("​Successfully connected ​to GPS daemon"​);​ +        ​if (connect(gpsd,​ "​127.0.0.1",​ 2947) < 0) { 
-        ​return sock;+            ​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 ​-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;
-       +
-       /* Convert message $GN to $GP */ +
-       /* if (strstr(data,​"$GNGGA"​)==0) { +
-        *  a_arr = explode(data); +
-        ​* ​ a_arr[2]="​P";​ +
-        *  data = 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("​Unable to send message to %s:​%d",​ SERVER, PORT);+
         }         }
 +    }
 +
 +    /* 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);​
 +
 +    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);
     }     }
 } }
  
-close(gpsd);​ +if (gpsd > -1) close(gpsd);​ 
-close(server);​+if (server > -1) close(server);​ 
 + 
 +exit(-1); 
  
-exit(0); 
  
 </​code>​ </​code>​