SDK Script gps2serial-GNSStoGPS.are

<code c gps2serial-gnsstogps.are>

   /* Options to configure by user*/
  SER_SPEED = (int) argv[2];	/* Serial Baud Rate 4800, 9600, 19200, 38400, 57600, 115200 */
  SER_DATABIT = 8;	/*  number of data bits (5, 6, 7, 8) */
  SER_STOPBIT = 1;	/*  number of stop bits (1, 2) */	
  SER_PARITY = 0;		/* parity (0=no parity, 1=odd parity, 2=even parity) */
  SER_FLOW = 0;		/*  flow control (0=none, 1=xon/xoff, 2=hardware) */
  DEV = "SERIAL1";	/*  name of the Serial Interface */

  local_ip = "127.0.0.1";
  local_port = (int) nb_config_get("gpsd.0.port");
   
  gpsd = -1;
          
  // start serial connection
  int start_serial() 
  {
  	if (nb_config_get("serial.0.status") != "2") {
      	nb_syslog("serial not assinged to SDK\n");
          exit(2);
      }
      // printf("set serial attributes\n");
      if ((rc = nb_serial_setattr(DEV, SER_SPEED, SER_DATABIT, SER_STOPBIT, SER_PARITY, SER_FLOW)) != 0) {
          nb_syslog("ERROR: unable to set serial attributes (rc %d) to %s\n", rc, DEV);
          return -1;
      }
      // nb_syslog("Open serial device %s\n", DEV);
      if ((fd = nb_serial_open(DEV)) < 0) {
          nb_syslog("ERROR: can't open serial port %s\n", DEV);
          return -1;
      }
      return fd;
  }
  // restart GPS deamon
  int gpsd_restart () 
  {
  	nb_syslog("Restarting GPS daemon");
   
  	nb_config_set("gpsd.0.status=0");
                  do {
                                  sleep(3);
                  } while ( nb_config_done != 0)
   
  	nb_config_set("gpsd.0.status=1");
                  do {
  	                sleep(5);
                  } while ( nb_config_done != 0)
  	return 0;
  }

// start TCP Socket for GPS deamon loop

  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 <= 15; attempt++) {
  		sleep(15);
   
  		if (connect(gpsd, local_ip, local_port) < 0) {
  			nb_syslog("Could not connect to daemon");
  			if (attempt == 12) {
  				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();
  }
   
   
  /* starting serial */
  nb_syslog("starting serial");
  if ((serial_fd = start_serial()) < 0 ) {
      nb_syslog("Could not start serial, exiting\n");
      exit(4);
  }

   
  /* 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 = write(serial_fd, data, len);
   nb_syslog("sending Data via serial interface");
  	  if (write(serial_fd, data, len) != len){
         nb_syslog("Unable to send message via Serial, data %s, len: %i\n", data, len);
  }
  }
   
  close(gpsd);
  close(serial_fd);
  exit(6);