SHARE
TWEET

Untitled

zhivko Oct 21st, 2016 60 Never
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
  1. /* Autogenerated by /home/machinekit/git/machinekit/bin/instcomp on Thu Oct  6 03:22:21 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:");
  19. MODULE_INFO(machinekit, "pin:pos_cmd0:float:None:in::None");
  20. MODULE_INFO(machinekit, "pin:pos_cmd1:float:None:in::None");
  21. MODULE_INFO(machinekit, "pin:pos_cmd2:float:None:in::None");
  22. MODULE_INFO(machinekit, "pin:pos_fb0:float:None:out::None");
  23. MODULE_INFO(machinekit, "pin:pos_fb1:float:None:out::None");
  24. MODULE_INFO(machinekit, "pin:pos_fb2:float:None:out::None");
  25. MODULE_INFO(machinekit, "instanceparam:pincount:int::6");
  26. MODULE_INFO(machinekit, "funct:_:1:");
  27. MODULE_INFO(machinekit, "author:test");
  28. MODULE_INFO(machinekit, "license:GPL");
  29. MODULE_LICENSE("GPL");
  30. #endif // MODULE_INFO
  31. RTAPI_TAG(HAL,HC_INSTANTIABLE);
  32.  
  33.  
  34. #define MAXCOUNT 16
  35. #define DEFAULTCOUNT 6
  36.  
  37. static int pincount = 6;
  38. RTAPI_IP_INT(pincount, "Instance integer param 'pincount'");
  39.  
  40. struct inst_data {
  41.  
  42.     hal_float_t *pos_cmd0;
  43.     hal_float_t *pos_cmd1;
  44.     hal_float_t *pos_cmd2;
  45.     hal_float_t *pos_fb0;
  46.     hal_float_t *pos_fb1;
  47.     hal_float_t *pos_fb2;
  48.     int sockfd;
  49.     int localpincount;
  50. };
  51.  
  52. static int maxpins __attribute__((unused)) = 16;
  53. static char *iprefix = "";
  54. static int _(void *arg, const hal_funct_args_t *fa);
  55.  
  56. static int instantiate(const int argc, const char**argv);
  57.  
  58. static int extra_inst_setup(struct inst_data* ip, const char *name, int argc, const char**argv);
  59.  
  60. #undef TRUE
  61. #undef FALSE
  62. #undef true
  63. #define true (1)
  64. #undef false
  65. #define false (0)
  66.  
  67. static int export_halobjs(struct inst_data *ip, int owner_id, const char *name)
  68. {
  69.     char buf[HAL_NAME_LEN + 1];
  70.     int r = 0;
  71.     int j __attribute__((unused)) = 0;
  72.     int z __attribute__((unused)) = 0;
  73.     r = hal_pin_float_newf(HAL_IN, &(ip->pos_cmd0), owner_id,
  74.             "%s.pos-cmd0", name);
  75.     if(r != 0) return r;
  76.  
  77.     r = hal_pin_float_newf(HAL_IN, &(ip->pos_cmd1), owner_id,
  78.             "%s.pos-cmd1", name);
  79.     if(r != 0) return r;
  80.  
  81.     r = hal_pin_float_newf(HAL_IN, &(ip->pos_cmd2), owner_id,
  82.             "%s.pos-cmd2", name);
  83.     if(r != 0) return r;
  84.  
  85.     r = hal_pin_float_newf(HAL_OUT, &(ip->pos_fb0), owner_id,
  86.             "%s.pos-fb0", name);
  87.     if(r != 0) return r;
  88.  
  89.     r = hal_pin_float_newf(HAL_OUT, &(ip->pos_fb1), owner_id,
  90.             "%s.pos-fb1", name);
  91.     if(r != 0) return r;
  92.  
  93.     r = hal_pin_float_newf(HAL_OUT, &(ip->pos_fb2), owner_id,
  94.             "%s.pos-fb2", name);
  95.     if(r != 0) return r;
  96.  
  97.  
  98. // if not set by instantiate() set to default
  99.     if(! ip->localpincount || ip->localpincount == -1)
  100.          ip->localpincount = DEFAULTCOUNT;
  101.  
  102.     hal_print_msg(RTAPI_MSG_DBG,"export_halobjs() ip->localpincount set to %d", ip->localpincount);
  103.     // export an extended thread function:
  104.     hal_export_xfunct_args_t __xf = {
  105.         .type = FS_XTHREADFUNC,
  106.         .funct.x = _,
  107.         .arg = ip,
  108.         .uses_fp = 1,
  109.         .reentrant = 0,
  110.         .owner_id = owner_id
  111.     };
  112.  
  113.     rtapi_snprintf(buf, sizeof(buf),"%s", name);
  114.     r = hal_export_xfunctf(&__xf, buf, name);
  115.     if(r != 0)
  116.         return r;
  117.     return 0;
  118. }
  119.  
  120. // constructor - init all HAL pins, params, funct etc here
  121. static int instantiate(const int argc, const char**argv)
  122. {
  123.     const char *name  __attribute__((unused)) = argv[1];
  124.     struct inst_data *ip;
  125.     int r;
  126.     int k;
  127.  
  128.     // allocate a named instance, and some HAL memory for the instance data
  129.     int inst_id = hal_inst_create(name, comp_id, sizeof(struct inst_data), (void **)&ip);
  130.  
  131.     if (inst_id < 0)
  132.         return -1;
  133.  
  134.     // here ip is guaranteed to point to a blob of HAL memory of size sizeof(struct inst_data).
  135.     hal_print_msg(RTAPI_MSG_DBG,"%s inst=%s argc=%d",__FUNCTION__, name, argc);
  136.  
  137.     // Debug print of params and values
  138.     hal_print_msg(RTAPI_MSG_DBG,"%s: int instance param: %s=%d",__FUNCTION__,"pincount", pincount);
  139. //  if pincount=NN is passed, set local variable here, if not set to default
  140.     int pin_param_value = pincount;
  141.     if((pin_param_value == -1) || (pin_param_value == 0))
  142.         pin_param_value = DEFAULTCOUNT;
  143.     else if((pin_param_value > 0) && (pin_param_value > MAXCOUNT))
  144.         pin_param_value = MAXCOUNT;
  145.     ip->localpincount = pincount = pin_param_value;
  146.     hal_print_msg(RTAPI_MSG_DBG,"ip->localpincount set to %d", pin_param_value);
  147.  
  148.     // These pins - params - functs will be owned by the instance
  149.     // which can be separately exited with delinst
  150.     if(strlen(iprefix))
  151.         r = export_halobjs(ip, inst_id, iprefix);
  152.     else
  153.         r = export_halobjs(ip, inst_id, name);
  154.     // if the extra_inst_setup returns non zero will abort module creation
  155.     k = extra_inst_setup(ip, name, argc, argv);
  156.  
  157.     if(k != 0)
  158.         return k;
  159.  
  160.     //reset pincount to -1 so that instantiation without it will result in DEFAULTCOUNT
  161.     pincount = -1;
  162.  
  163.     return r;
  164. }
  165.  
  166. int rtapi_app_main(void)
  167. {
  168.     // Debug print of params and values
  169.     hal_print_msg(RTAPI_MSG_DBG,"%s: int instance param: %s=%d",__FUNCTION__,"pincount", pincount);
  170.     comp_id = hal_xinit(TYPE_RT, 0, 0, instantiate, NULL, compname);
  171.     if (comp_id < 0)
  172.         return -1;
  173.  
  174.     hal_ready(comp_id);
  175.  
  176.     return 0;
  177. }
  178.  
  179. void rtapi_app_exit(void)
  180. {
  181.     hal_exit(comp_id);
  182. }
  183.  
  184.  
  185. #undef FUNCTION
  186. #define FUNCTION(name) static int name(void *arg, const hal_funct_args_t *fa)
  187. // if the extra_inst_setup returns non zero it will abort the module creation
  188. #undef EXTRA_INST_SETUP
  189. #define EXTRA_INST_SETUP() static int extra_inst_setup(struct inst_data *ip, const char *name, int argc, const char**argv)
  190. #undef fperiod
  191. #define fperiod (period * 1e-9)
  192. #undef pos_cmd0
  193. #define pos_cmd0 (0+*ip->pos_cmd0)
  194. #undef pos_cmd1
  195. #define pos_cmd1 (0+*ip->pos_cmd1)
  196. #undef pos_cmd2
  197. #define pos_cmd2 (0+*ip->pos_cmd2)
  198. #undef pos_fb0
  199. #define pos_fb0 (*ip->pos_fb0)
  200. #undef pos_fb1
  201. #define pos_fb1 (*ip->pos_fb1)
  202. #undef pos_fb2
  203. #define pos_fb2 (*ip->pos_fb2)
  204. #undef sockfd
  205. #define sockfd (ip->sockfd)
  206. #undef localpincount
  207. #define localpincount (ip->localpincount)
  208.  
  209.  
  210. #line 24 "./udp.icomp"
  211. #include <arpa/inet.h>
  212. #include <ifaddrs.h>
  213.  
  214. EXTRA_INST_SETUP(){
  215.     rtapi_print("hallo\n");
  216.  
  217.     int ret;
  218.  
  219.     sockfd = socket(PF_INET, SOCK_DGRAM, IPPROTO_IP);
  220.     if (sockfd < 0) {
  221.         rtapi_print("ERROR: can't open socket: %s\n", strerror(errno));
  222.         return -errno;
  223.     }
  224.  
  225.     struct sockaddr_in server_addr;
  226.     struct sockaddr_in local_addr;
  227.  
  228.     server_addr.sin_family = AF_INET;
  229.     server_addr.sin_port = htons(27181);
  230.     server_addr.sin_addr.s_addr = inet_addr("192.168.1.150");
  231.  
  232.     local_addr.sin_family      = AF_INET;
  233.     local_addr.sin_addr.s_addr = INADDR_ANY;
  234.  
  235.     ret = connect(sockfd, (struct sockaddr *) &server_addr, sizeof(struct sockaddr_in));
  236.     if (ret < 0) {
  237.         rtapi_print("ERROR: can't connect: %s\n", strerror(errno));
  238.         return -errno;
  239.     }
  240.  
  241.    
  242.  
  243.     struct timeval timeout;
  244.     timeout.tv_sec = 0;
  245.     timeout.tv_usec = 10;
  246.  
  247.     ret = setsockopt(sockfd, SOL_SOCKET, SO_RCVTIMEO, (char *)&timeout, sizeof(timeout));
  248.     if (ret < 0) {
  249.         rtapi_print("ERROR: can't set socket option: %s\n", strerror(errno));
  250.         return -errno;
  251.     }
  252.  
  253.     timeout.tv_usec = 10;
  254.     setsockopt(sockfd, SOL_SOCKET, SO_SNDTIMEO, (char *)&timeout, sizeof(timeout));
  255.     if (ret < 0) {
  256.         rtapi_print("ERROR: can't set socket option: %s\n", strerror(errno));
  257.         return -errno;
  258.     }
  259.  
  260.  
  261.     return 0;
  262. }
  263.  
  264. FUNCTION(_) {
  265.     long period __attribute__((unused)) = fa_period(fa);
  266.     struct inst_data *ip = arg;
  267.  
  268.     pos_fb0 = pos_cmd0;
  269.     pos_fb1 = pos_cmd1;
  270.     pos_fb2 = pos_cmd2;
  271.     uint8_t buf[16];
  272.  
  273.     buf[0] = 0x01;
  274.     buf[1] = 0x42;
  275.     buf[2] = 0x00;
  276.     buf[3] = 0x01;
  277.  
  278.     int ret = send(sockfd,buf,4,0);
  279.     if (ret < 0) {
  280.         rtapi_print("send fail %s\n", strerror(errno));
  281.     }
  282. }
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