SHARE
TWEET

Untitled

zhivko Oct 21st, 2016 58 Never
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
  1. /* Autogenerated by /home/machinekit/git/machinekit/bin/instcomp on Fri Oct 21 13:29:59 2016 -- do not edit */
  2.  
  3. #include "rtapi.h"
  4. #ifdef RTAPI
  5. #include "rtapi_app.h"
  6. #endif
  7. #include "rtapi_string.h"
  8. #include "rtapi_errno.h"
  9. #include "hal.h"
  10. #include "hal_priv.h"
  11.  
  12. static int comp_id;
  13.  
  14. static char *compname = "udp";
  15.  
  16.  
  17. #ifdef MODULE_INFO
  18. MODULE_INFO(machinekit, "component:udp:Connects to laser esp8266 of laser distance sensor (ODSL 9/D26-450-S12 - Optical distance sensor)");
  19. MODULE_INFO(machinekit, "pin:distance:float:None:out::None");
  20. MODULE_INFO(machinekit, "instanceparam:pincount:int::6");
  21. MODULE_INFO(machinekit, "funct:_:1:");
  22. MODULE_INFO(machinekit, "author:test");
  23. MODULE_INFO(machinekit, "license:GPL");
  24. MODULE_LICENSE("GPL");
  25. #endif // MODULE_INFO
  26. RTAPI_TAG(HAL,HC_INSTANTIABLE);
  27.  
  28.  
  29. #define MAXCOUNT 6
  30. #define DEFAULTCOUNT 6
  31.  
  32. static int pincount = 6;
  33. RTAPI_IP_INT(pincount, "Instance integer param 'pincount'");
  34.  
  35. struct inst_data {
  36.  
  37.     hal_float_t *distance;
  38.     int sockfd;
  39.     int localpincount;
  40. };
  41.  
  42. static int maxpins __attribute__((unused)) = 6;
  43. static char *iprefix = "";
  44. static int _(void *arg, const hal_funct_args_t *fa);
  45.  
  46. static int instantiate(const int argc, const char**argv);
  47.  
  48. static int extra_inst_setup(struct inst_data* ip, const char *name, int argc, const char**argv);
  49.  
  50. #undef TRUE
  51. #undef FALSE
  52. #undef true
  53. #define true (1)
  54. #undef false
  55. #define false (0)
  56.  
  57. static int export_halobjs(struct inst_data *ip, int owner_id, const char *name)
  58. {
  59.     char buf[HAL_NAME_LEN + 1];
  60.     int r = 0;
  61.     int j __attribute__((unused)) = 0;
  62.     int z __attribute__((unused)) = 0;
  63.     r = hal_pin_float_newf(HAL_OUT, &(ip->distance), owner_id,
  64.             "%s.distance", name);
  65.     if(r != 0) return r;
  66.  
  67.  
  68. // if not set by instantiate() set to default
  69.     if(! ip->localpincount || ip->localpincount == -1)
  70.          ip->localpincount = DEFAULTCOUNT;
  71.  
  72.     hal_print_msg(RTAPI_MSG_DBG,"export_halobjs() ip->localpincount set to %d", ip->localpincount);
  73.     // export an extended thread function:
  74.     hal_export_xfunct_args_t __xf = {
  75.         .type = FS_XTHREADFUNC,
  76.         .funct.x = _,
  77.         .arg = ip,
  78.         .uses_fp = 1,
  79.         .reentrant = 0,
  80.         .owner_id = owner_id
  81.     };
  82.  
  83.     rtapi_snprintf(buf, sizeof(buf),"%s", name);
  84.     r = hal_export_xfunctf(&__xf, buf, name);
  85.     if(r != 0)
  86.         return r;
  87.     return 0;
  88. }
  89.  
  90. // constructor - init all HAL pins, params, funct etc here
  91. static int instantiate(const int argc, const char**argv)
  92. {
  93.     const char *name  __attribute__((unused)) = argv[1];
  94.     struct inst_data *ip;
  95.     int r;
  96.     int k;
  97.  
  98.     // allocate a named instance, and some HAL memory for the instance data
  99.     int inst_id = hal_inst_create(name, comp_id, sizeof(struct inst_data), (void **)&ip);
  100.  
  101.     if (inst_id < 0)
  102.         return -1;
  103.  
  104.     // here ip is guaranteed to point to a blob of HAL memory of size sizeof(struct inst_data).
  105.     hal_print_msg(RTAPI_MSG_DBG,"%s inst=%s argc=%d",__FUNCTION__, name, argc);
  106.  
  107.     // Debug print of params and values
  108.     hal_print_msg(RTAPI_MSG_DBG,"%s: int instance param: %s=%d",__FUNCTION__,"pincount", pincount);
  109. //  if pincount=NN is passed, set local variable here, if not set to default
  110.     int pin_param_value = pincount;
  111.     if((pin_param_value == -1) || (pin_param_value == 0))
  112.         pin_param_value = DEFAULTCOUNT;
  113.     else if((pin_param_value > 0) && (pin_param_value > MAXCOUNT))
  114.         pin_param_value = MAXCOUNT;
  115.     ip->localpincount = pincount = pin_param_value;
  116.     hal_print_msg(RTAPI_MSG_DBG,"ip->localpincount set to %d", pin_param_value);
  117.  
  118.     // These pins - params - functs will be owned by the instance
  119.     // which can be separately exited with delinst
  120.     if(strlen(iprefix))
  121.         r = export_halobjs(ip, inst_id, iprefix);
  122.     else
  123.         r = export_halobjs(ip, inst_id, name);
  124.     // if the extra_inst_setup returns non zero will abort module creation
  125.     k = extra_inst_setup(ip, name, argc, argv);
  126.  
  127.     if(k != 0)
  128.         return k;
  129.  
  130.     //reset pincount to -1 so that instantiation without it will result in DEFAULTCOUNT
  131.     pincount = -1;
  132.  
  133.     return r;
  134. }
  135.  
  136. int rtapi_app_main(void)
  137. {
  138.     // Debug print of params and values
  139.     hal_print_msg(RTAPI_MSG_DBG,"%s: int instance param: %s=%d",__FUNCTION__,"pincount", pincount);
  140.     comp_id = hal_xinit(TYPE_RT, 0, 0, instantiate, NULL, compname);
  141.     if (comp_id < 0)
  142.         return -1;
  143.  
  144.     hal_ready(comp_id);
  145.  
  146.     return 0;
  147. }
  148.  
  149. void rtapi_app_exit(void)
  150. {
  151.     hal_exit(comp_id);
  152. }
  153.  
  154.  
  155. #undef FUNCTION
  156. #define FUNCTION(name) static int name(void *arg, const hal_funct_args_t *fa)
  157. // if the extra_inst_setup returns non zero it will abort the module creation
  158. #undef EXTRA_INST_SETUP
  159. #define EXTRA_INST_SETUP() static int extra_inst_setup(struct inst_data *ip, const char *name, int argc, const char**argv)
  160. #undef fperiod
  161. #define fperiod (period * 1e-9)
  162. #undef distance
  163. #define distance (*ip->distance)
  164. #undef sockfd
  165. #define sockfd (ip->sockfd)
  166. #undef localpincount
  167. #define localpincount (ip->localpincount)
  168.  
  169.  
  170. #line 17 "./components/udp.icomp"
  171.  
  172. /*
  173. #include <arpa/inet.h>
  174. #include <ifaddrs.h>
  175. #include <stdlib.h>
  176. #include <pthread.h>
  177. */
  178.  
  179. #include <stdio.h>
  180. #include <unistd.h>
  181. #include <stdlib.h>
  182. #include <string.h>
  183. #include <netdb.h>
  184. #include <sys/types.h>
  185. #include <sys/socket.h>
  186. #include <netinet/in.h>
  187. #include <arpa/inet.h>
  188. #include <ifaddrs.h>
  189. #include <pthread.h>
  190.  
  191. #define BUFSIZE 1024
  192. #define SERVER_PORT 1234
  193. #define MAX_THREADS 4
  194.  
  195. int myPort;
  196. const char* myIp;
  197. float recValue;
  198.  
  199. pthread_t pth;  // this is our thread identifier
  200.  
  201. struct sockaddr_in server;
  202.  
  203.  
  204.  
  205. /* This is our thread function.  It is like main(), but for a thread */
  206. void *threadFunc(void *arg)
  207. {
  208.  
  209.   int sockfd; /* socket */
  210.   int portno; /* port to listen on */
  211.   int clientlen; /* byte size of client's address */
  212.   struct sockaddr_in serveraddr; /* server's addr */
  213.   struct sockaddr_in clientaddr; /* client addr */
  214.   struct hostent *hostp; /* client host info */
  215.   char buf[BUFSIZE]; /* message buf */
  216.   char *hostaddrp; /* dotted decimal host addr string */
  217.   int optval; /* flag value for setsockopt */
  218.   int n; /* message byte size */
  219.  
  220.   /*
  221.    * socket: create the parent socket
  222.    */
  223.   sockfd = socket(AF_INET, SOCK_DGRAM, 0);
  224.  
  225.   if (sockfd < 0)
  226.     error("ERROR opening socket");
  227.  
  228.   /* setsockopt: Handy debugging trick that lets
  229.    * us rerun the server immediately after we kill it;
  230.    * otherwise we have to wait about 20 secs.
  231.    * Eliminates "ERROR on binding: Address already in use" error.
  232.    */
  233.   optval = 1;
  234.   setsockopt(sockfd, SOL_SOCKET, SO_REUSEADDR,
  235.          (const void *)&optval , sizeof(int));
  236.  
  237.   /*
  238.    * build the server's Internet address
  239.    */
  240.   bzero((char *) &serveraddr, sizeof(serveraddr));
  241.   serveraddr.sin_family = AF_INET;
  242.   serveraddr.sin_addr.s_addr = htonl(INADDR_ANY);
  243.   serveraddr.sin_port = htons(SERVER_PORT);
  244.  
  245.   /*
  246.    * bind: associate the parent socket with a port
  247.    */
  248.   if (bind(sockfd, (struct sockaddr *) &serveraddr,
  249.        sizeof(serveraddr)) < 0)
  250.     error("ERROR on binding");
  251.  
  252.   /*
  253.    * main loop: wait for a datagram, then echo it
  254.    */
  255.   clientlen = sizeof(clientaddr);
  256.   while (1) {
  257.  
  258.     /*
  259.      * recvfrom: receive a UDP datagram from a client
  260.      */
  261.     bzero(buf, BUFSIZE);
  262.     n = recvfrom(sockfd, buf, BUFSIZE, 0,
  263.          (struct sockaddr *) &clientaddr, &clientlen);
  264.     if (n < 0)
  265.       error("ERROR in recvfrom");
  266.  
  267.     /*
  268.      * gethostbyaddr: determine who sent the datagram
  269.      */
  270.     hostp = gethostbyaddr((const char *)&clientaddr.sin_addr.s_addr,
  271.               sizeof(clientaddr.sin_addr.s_addr), AF_INET);
  272.     if (hostp == NULL)
  273.       error("ERROR on gethostbyaddr");
  274.     hostaddrp = inet_ntoa(clientaddr.sin_addr);
  275.     if (hostaddrp == NULL)
  276.       error("ERROR on inet_ntoa\n");
  277.     printf("server received datagram from %s (%s)\n",
  278.        hostp->h_name, hostaddrp);
  279.     printf("server received %d/%d bytes: %s\n", strlen(buf), n, buf);
  280.    
  281.     /*
  282.      * sendto: echo the input back to the client
  283.      */
  284.     n = sendto(sockfd, buf, strlen(buf), 0,
  285.            (struct sockaddr *) &clientaddr, clientlen);
  286.     if (n < 0)
  287.       error("ERROR in sendto");
  288.   }  
  289. }
  290.  
  291. EXTRA_INST_SETUP(){
  292.     rtapi_print("UDP Inst setup\n");
  293.     int x;
  294.    
  295.     recValue=5.5;
  296.     char** tokens;
  297.     const char* parValue;
  298.    
  299.     for(x = 0; x < argc; x++)
  300.     {
  301.         hal_print_msg(RTAPI_MSG_ERR,"argv[%d] = %s", x, argv[x]);
  302.         parValue = argv[x];
  303.         split(parValue, "=");
  304.         if(tokens[0] == "ip")
  305.             myIp=(char*)(*(tokens + 1));
  306.         if(tokens[0] == "port")
  307.             myPort=atoi(*(tokens + 1));
  308.     }
  309.    
  310.    
  311.    
  312.     int i = 0;
  313.  
  314.     /* Create worker thread */
  315.     pthread_create(&pth,NULL,threadFunc,"processing...");
  316.  
  317.     return 0;
  318. }
  319.  
  320. FUNCTION(_) {
  321.     long period __attribute__((unused)) = fa_period(fa);
  322.     struct inst_data *ip = arg;
  323.  
  324.     distance = recValue;
  325. }
  326.  
  327. char** str_split(char* a_str, const char a_delim)
  328. {
  329.     char** result    = 0;
  330.     size_t count     = 0;
  331.     char* tmp        = a_str;
  332.     char* last_comma = 0;
  333.     char delim[2];
  334.     delim[0] = a_delim;
  335.     delim[1] = 0;
  336.  
  337.     /* Count how many elements will be extracted. */
  338.     while (*tmp)
  339.     {
  340.         if (a_delim == *tmp)
  341.         {
  342.             count++;
  343.             last_comma = tmp;
  344.         }
  345.         tmp++;
  346.     }
  347.  
  348.     /* Add space for trailing token. */
  349.     count += last_comma < (a_str + strlen(a_str) - 1);
  350.  
  351.     /* Add space for terminating null string so caller
  352.        knows where the list of returned strings ends. */
  353.     count++;
  354.  
  355.     result = malloc(sizeof(char*) * count);
  356.  
  357.     if (result)
  358.     {
  359.         size_t idx  = 0;
  360.         char* token = strtok(a_str, delim);
  361.  
  362.         while (token)
  363.         {
  364.             assert(idx < count);
  365.             *(result + idx++) = strdup(token);
  366.             token = strtok(0, delim);
  367.         }
  368.         assert(idx == count - 1);
  369.         *(result + idx) = 0;
  370.     }
  371.  
  372.     return result;
  373. }
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