int main() { lcm = lcm_create(NULL); channels_t tx_msg; channels_t_subscribe(lcm, "CHANNELS_.*_RX", handler_channels_lcm, rxdata); for(;;) { lcm_handle(lcm); //do transformation of incoming data here for(int i=0; i<8; i++){ txdata[i] = rxdata[i]-500; } //make tx msg tx_msg.utime = utime_now(); tx_msg.num_channels = 8; for(int i=0; i < tx_msg.num_channels; i++){ tx_msg.channels[i] = txdata[i]; } //pblish transmint msg channels_t_publish(lcm, "CHANNELS_1_TX", &tx_msg); } lcm_destroy(lcm); return 0; }
/* LCM processing (top-level loop) -- Only use to talk with BLOCKS */ void *lcm_thread_loop(void *data){ printf("lcm running"); lcm = lcm_create(NULL); channels_t_subscribe(lcm, "CHANNELS_1_RX", channels_handler, lcm); waypoint_trigger_t_subscribe(lcm, "WP_TRIGGER", wp_trigger_handler, lcm); while(1){ lcm_handle(lcm); usleep(10); } lcm_destroy(lcm); return 0; }
int main(int argc, char *argv[]) { uint8_t i; for(i = 0; i < argc; ++i) { if((strcmp(argv[i], "-h") == 0) || (strcmp(argv[i], "--help") == 0)) { fprintf(stdout, "%s\n", USAGE); exit(0); } if(strcmp(argv[i], "-v") == 0) { verbose = true; fprintf(stdout, "Running in verbose mode\n"); } if(strcmp(argv[i], "-l") == 0) { loopback_mode = true; fprintf(stdout, "Running in loopback mode\n"); } } (void) signal(SIGINT, interrupt); (void) signal(SIGTERM, interrupt); lcm = lcm_create(NULL); while(lcm == NULL) { fprintf(stderr, "LCM failed to initialize. Reattempting....\n"); usleep(1000000); lcm = lcm_create(NULL); } // Create Serial Device for XBee char *xbee_dev_name = "/dev/XBee"; for(i = 0; i < argc; ++i) { if(strcmp(argv[i], "-x") == 0) { if(i + 1 < argc) xbee_dev_name = argv[i+1]; break; } } xbee = serial_create(xbee_dev_name, B9600); if(xbee == NULL) fprintf(stderr, "XBee device does not exist at %s\n", xbee_dev_name); else fprintf(stdout, "XBee device successfully opened at 9600 baud on %s\n", xbee_dev_name); // Create Serial Device for USB char *usb_dev_name = "/dev/stack"; for(i = 0; i < argc; ++i) { if(strcmp(argv[i], "-u") == 0) { if(i + 1 < argc) usb_dev_name = argv[i+1]; break; } } usb = serial_create(usb_dev_name, B115200); if(usb == NULL) fprintf(stderr, "Usb device does not exist at %s\n", usb_dev_name); else fprintf(stdout, "Usb device successfully opened at 115200 baud on %s\n", usb_dev_name); // If no open comm ports and not in loopback mode, quit. if(!xbee && !usb && !loopback_mode) { fprintf(stderr, "Failed to open any comms devices. Exiting...\n"); lcm_destroy(lcm); exit(1); } // Create comms devices if(xbee) { xbee_comms = comms_create(1000, 1000, 1, publish_xbee); comms_subscribe(xbee_comms, CHANNEL_KILL, handler_kill, NULL); comms_subscribe(xbee_comms, CHANNEL_CHANNELS, handler_channels, NULL); comms_subscribe(xbee_comms, CHANNEL_CFG_USB_SN, handler_cfg_usb_serial_num, NULL); comms_subscribe(xbee_comms, CHANNEL_CFG_DATA_FREQUENCY, handler_cfg_data_frequency, NULL); pthread_create(&xbee_thread, NULL, xbee_run, NULL); } // Create comms devices if(usb) { usb_comms = comms_create(1000, 1000, 1, publish_usb); comms_subscribe(usb_comms, CHANNEL_KILL, handler_kill, NULL); comms_subscribe(usb_comms, CHANNEL_CHANNELS, handler_channels, NULL); comms_subscribe(usb_comms, CHANNEL_CFG_USB_SN, handler_cfg_usb_serial_num, NULL); comms_subscribe(usb_comms, CHANNEL_CFG_DATA_FREQUENCY, handler_cfg_data_frequency, NULL); pthread_create(&usb_thread, NULL, usb_run, NULL); } kill_t_subscription_t *kill_subs = kill_t_subscribe(lcm, "KILL_.*_TX", handler_kill_lcm, NULL); channels_t_subscription_t *channels_subs = channels_t_subscribe(lcm, "CHANNELS_.*_TX", handler_channels_lcm, NULL); cfg_data_frequency_t_subscription_t *data_freq_subs = cfg_data_frequency_t_subscribe(lcm, "CFG_DATA_FREQUENCY_.*_TX", handler_cfg_data_frequency_lcm, NULL); cfg_usb_serial_num_t_subscription_t *usb_serial_num_subs = cfg_usb_serial_num_t_subscribe(lcm, "CFG_USB_SERIAL_NUM_.*_TX", handler_cfg_usb_serial_num_lcm, NULL); while(!done) { // Set up timeout on lcm_handle to prevent blocking at program exit int lcm_fd = lcm_get_fileno(lcm); fd_set fds; FD_ZERO(&fds); FD_SET(lcm_fd, &fds); struct timeval timeout = { 0, 100000 }; // wait 100ms int status = select(lcm_fd + 1, &fds, NULL, NULL, &timeout); if(done) break; if(status != 0 && FD_ISSET(lcm_fd, &fds)) { if ( lcm_handle(lcm) ) { fprintf( stderr, "LCM handle error, aborting\n" ); done = true; } } } fprintf(stdout, "Lcm driver destroyed.\n"); kill_t_unsubscribe(lcm, kill_subs); channels_t_unsubscribe(lcm, channels_subs); cfg_data_frequency_t_unsubscribe(lcm, data_freq_subs); cfg_usb_serial_num_t_unsubscribe(lcm, usb_serial_num_subs); if(xbee) { pthread_cancel(xbee_thread); comms_destroy(xbee_comms); serial_destroy(xbee); fprintf(stdout, "Xbee driver destroyed.\n"); } if(usb) { pthread_cancel(usb_thread); comms_destroy(usb_comms); serial_destroy(usb); fprintf(stdout, "Usb driver destroyed.\n"); } lcm_destroy(lcm); return 0; }