SHARE
TWEET

Untitled

zhivko Oct 21st, 2016 60 Never
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
  1. component udp "Connects to laser esp8266 of laser distance sensor (ODSL 9/D26-450-S12 - Optical distance sensor)";
  2.  
  3. //http://www.leuze.com/selector/ci_pages/downloads.php?supplier_aid=50111159&key=9f0f255e2c9fb97529e5d7196847f76a174a9a206b9ec072f3d0228255151e6f
  4.  
  5. pin out float distance;
  6.  
  7. instanceparam int pincount = 6;
  8.  
  9. function _;
  10.  
  11. option extra_inst_setup yes;
  12. variable int sockfd;
  13.  
  14. author "test";
  15. license "GPL"; // indicates GPL v2 or later
  16. ;;
  17.  
  18. /*
  19. #include <arpa/inet.h>
  20. #include <ifaddrs.h>
  21. #include <stdlib.h>
  22. #include <pthread.h>
  23. */
  24.  
  25. #include <stdio.h>
  26. #include <unistd.h>
  27. #include <stdlib.h>
  28. #include <string.h>
  29. #include <netdb.h>
  30. #include <sys/types.h>
  31. #include <sys/socket.h>
  32. #include <netinet/in.h>
  33. #include <arpa/inet.h>
  34. #include <ifaddrs.h>
  35. #include <pthread.h>
  36.  
  37. #define BUFSIZE 1024
  38. #define SERVER_PORT 1234
  39. #define MAX_THREADS 4
  40.  
  41. int myPort;
  42. const char* myIp;
  43. float recValue;
  44.  
  45. pthread_t pth;  // this is our thread identifier
  46.  
  47. struct sockaddr_in server;
  48.  
  49.  
  50.  
  51. /* This is our thread function.  It is like main(), but for a thread */
  52. void *threadFunc(void *arg)
  53. {
  54.  
  55.   int sockfd; /* socket */
  56.   int portno; /* port to listen on */
  57.   int clientlen; /* byte size of client's address */
  58.   struct sockaddr_in serveraddr; /* server's addr */
  59.   struct sockaddr_in clientaddr; /* client addr */
  60.   struct hostent *hostp; /* client host info */
  61.   char buf[BUFSIZE]; /* message buf */
  62.   char *hostaddrp; /* dotted decimal host addr string */
  63.   int optval; /* flag value for setsockopt */
  64.   int n; /* message byte size */
  65.  
  66.   /*
  67.    * socket: create the parent socket
  68.    */
  69.   sockfd = socket(AF_INET, SOCK_DGRAM, 0);
  70.  
  71.   if (sockfd < 0)
  72.     error("ERROR opening socket");
  73.  
  74.   /* setsockopt: Handy debugging trick that lets
  75.    * us rerun the server immediately after we kill it;
  76.    * otherwise we have to wait about 20 secs.
  77.    * Eliminates "ERROR on binding: Address already in use" error.
  78.    */
  79.   optval = 1;
  80.   setsockopt(sockfd, SOL_SOCKET, SO_REUSEADDR,
  81.          (const void *)&optval , sizeof(int));
  82.  
  83.   /*
  84.    * build the server's Internet address
  85.    */
  86.   bzero((char *) &serveraddr, sizeof(serveraddr));
  87.   serveraddr.sin_family = AF_INET;
  88.   serveraddr.sin_addr.s_addr = htonl(INADDR_ANY);
  89.   serveraddr.sin_port = htons(SERVER_PORT);
  90.  
  91.   /*
  92.    * bind: associate the parent socket with a port
  93.    */
  94.   if (bind(sockfd, (struct sockaddr *) &serveraddr,
  95.        sizeof(serveraddr)) < 0)
  96.     error("ERROR on binding");
  97.  
  98.   /*
  99.    * main loop: wait for a datagram, then echo it
  100.    */
  101.   clientlen = sizeof(clientaddr);
  102.   while (1) {
  103.  
  104.     /*
  105.      * recvfrom: receive a UDP datagram from a client
  106.      */
  107.     bzero(buf, BUFSIZE);
  108.     n = recvfrom(sockfd, buf, BUFSIZE, 0,
  109.          (struct sockaddr *) &clientaddr, &clientlen);
  110.     if (n < 0)
  111.       error("ERROR in recvfrom");
  112.  
  113.     /*
  114.      * gethostbyaddr: determine who sent the datagram
  115.      */
  116.     hostp = gethostbyaddr((const char *)&clientaddr.sin_addr.s_addr,
  117.               sizeof(clientaddr.sin_addr.s_addr), AF_INET);
  118.     if (hostp == NULL)
  119.       error("ERROR on gethostbyaddr");
  120.     hostaddrp = inet_ntoa(clientaddr.sin_addr);
  121.     if (hostaddrp == NULL)
  122.       error("ERROR on inet_ntoa\n");
  123.     printf("server received datagram from %s (%s)\n",
  124.        hostp->h_name, hostaddrp);
  125.     printf("server received %d/%d bytes: %s\n", strlen(buf), n, buf);
  126.    
  127.     /*
  128.      * sendto: echo the input back to the client
  129.      */
  130.     n = sendto(sockfd, buf, strlen(buf), 0,
  131.            (struct sockaddr *) &clientaddr, clientlen);
  132.     if (n < 0)
  133.       error("ERROR in sendto");
  134.   }  
  135. }
  136.  
  137. EXTRA_INST_SETUP(){
  138.     rtapi_print("UDP Inst setup\n");
  139.     int x;
  140.    
  141.     recValue=5.5;
  142.     char** tokens;
  143.     const char* parValue;
  144.    
  145.     for(x = 0; x < argc; x++)
  146.     {
  147.         hal_print_msg(RTAPI_MSG_ERR,"argv[%d] = %s", x, argv[x]);
  148.         parValue = argv[x];
  149.         split(parValue, "=");
  150.         if(tokens[0] == "ip")
  151.             myIp=(char*)(*(tokens + 1));
  152.         if(tokens[0] == "port")
  153.             myPort=atoi(*(tokens + 1));
  154.     }
  155.    
  156.    
  157.    
  158.     int i = 0;
  159.  
  160.     /* Create worker thread */
  161.     pthread_create(&pth,NULL,threadFunc,"processing...");
  162.  
  163.     return 0;
  164. }
  165.  
  166. FUNCTION(_) {
  167.     distance = recValue;
  168. }
  169.  
  170. char** str_split(char* a_str, const char a_delim)
  171. {
  172.     char** result    = 0;
  173.     size_t count     = 0;
  174.     char* tmp        = a_str;
  175.     char* last_comma = 0;
  176.     char delim[2];
  177.     delim[0] = a_delim;
  178.     delim[1] = 0;
  179.  
  180.     /* Count how many elements will be extracted. */
  181.     while (*tmp)
  182.     {
  183.         if (a_delim == *tmp)
  184.         {
  185.             count++;
  186.             last_comma = tmp;
  187.         }
  188.         tmp++;
  189.     }
  190.  
  191.     /* Add space for trailing token. */
  192.     count += last_comma < (a_str + strlen(a_str) - 1);
  193.  
  194.     /* Add space for terminating null string so caller
  195.        knows where the list of returned strings ends. */
  196.     count++;
  197.  
  198.     result = malloc(sizeof(char*) * count);
  199.  
  200.     if (result)
  201.     {
  202.         size_t idx  = 0;
  203.         char* token = strtok(a_str, delim);
  204.  
  205.         while (token)
  206.         {
  207.             assert(idx < count);
  208.             *(result + idx++) = strdup(token);
  209.             token = strtok(0, delim);
  210.         }
  211.         assert(idx == count - 1);
  212.         *(result + idx) = 0;
  213.     }
  214.  
  215.     return result;
  216. }
RAW Paste Data
We use cookies for various purposes including analytics. By continuing to use Pastebin, you agree to our use of cookies as described in the Cookies Policy. OK, I Understand
 
Top