embedded_linux_comms.c
Go to the documentation of this file.
00001 /*
00002  * embedded_linux_comms.c
00003  *
00004  *  Created on: Jun 16, 2012
00005  *      Author: bouchier
00006  */
00007 
00008 #ifndef ROS_EMBEDDED_LINUX_COMMS_H
00009 #define ROS_EMBEDDED_LINUX_COMMS_H
00010 
00011 #include <stdio.h>
00012 #include <stdlib.h>
00013 #include <unistd.h>
00014 #include <fcntl.h>
00015 #include <errno.h>
00016 #include <termios.h>
00017 #include <string.h>
00018 #include <time.h>
00019 #include <stdint.h>
00020 #include <sys/types.h>
00021 #include <sys/socket.h>
00022 #include <netinet/in.h>
00023 #include <netinet/tcp.h>
00024 #include <netdb.h>
00025 #include <assert.h>
00026 
00027 #define DEFAULT_PORTNUM 11411
00028 
00029 void error(const char *msg)
00030 {
00031     perror(msg);
00032     exit(0);
00033 }
00034 
00035 void set_nonblock(int socket)
00036 {
00037 int flags;
00038 flags = fcntl(socket,F_GETFL,0);
00039 assert(flags != -1);
00040 fcntl(socket, F_SETFL, flags | O_NONBLOCK);
00041 }
00042 
00043 int elCommInit(char *portName, int baud)
00044 {
00045         struct termios options;
00046         int fd;
00047         char *ip;
00048         char *tcpPortNumString;
00049         long int tcpPortNum;
00050         int sockfd;
00051         struct sockaddr_in serv_addr;
00052         struct hostent *server;
00053         int rv;
00054 
00055         if (*portName == '/') {                 // linux serial port names always begin with /dev
00056                 printf("Opening serial port %s\n", portName);
00057 
00058                 fd = open(portName, O_RDWR | O_NOCTTY | O_NDELAY);
00059 
00060                 if (fd == -1){
00061                         //Could not open the port.
00062                         perror("init(): Unable to open serial port - ");
00063                 }
00064                 else{
00065                         fcntl(fd, F_SETFL, FNDELAY); // Sets the read() function to return NOW and not wait for data to enter buffer if there isn't anything there.
00066 
00067                         //Configure port for 8N1 transmission
00068                         tcgetattr(fd, &options);                                        //Gets the current options for the port
00069                         cfsetispeed(&options, B57600);                          //Sets the Input Baud Rate
00070                         cfsetospeed(&options, B57600);                          //Sets the Output Baud Rate
00071                         options.c_cflag |= (CLOCAL | CREAD);            //? all these set options for 8N1 serial operations
00072                         options.c_cflag &= ~PARENB;
00073                         options.c_cflag &= ~CSTOPB;
00074                         options.c_cflag &= ~CSIZE;
00075                         options.c_cflag |= CS8;
00076                         options.c_cflag &= ~CRTSCTS;
00077                         options.c_lflag &= ~(ICANON | ECHO | ECHOE | ISIG);     // set raw mode
00078                         options.c_iflag &= ~(IXON | IXOFF | IXANY);             // disable SW flow control
00079                         options.c_oflag &= ~OPOST;
00080 
00081                         tcsetattr(fd, TCSANOW, &options);                       //Set the new options for the port "NOW"
00082                 }
00083                 return fd;
00084         } else {                // Connect to the rosserial server
00085                 // figure out the port number to use
00086                 ip = strtok(portName, ":");                     // IP address should be e.g. 192.168.1.10 or 192.168.1.10:11411
00087                 tcpPortNumString = strtok(NULL, ":");   // get the port # if specified
00088                 tcpPortNum = 0;
00089                 if (tcpPortNumString != NULL) {
00090                         tcpPortNum = strtol(tcpPortNumString, NULL, 10);        // convert port num string to long
00091                 }
00092                 if (tcpPortNum == 0){                           // if port was not specified, or was not numeric
00093                         tcpPortNum = DEFAULT_PORTNUM;
00094                 }
00095                 printf("Connecting to TCP server at %s:%ld....\n", ip, tcpPortNum);
00096 
00097                 // create the socket
00098             sockfd = socket(AF_INET, SOCK_STREAM, 0);
00099             if (sockfd < 0) {
00100                 error("ERROR opening socket");
00101                 exit(-1);
00102             }
00103 
00104             /* Disable the Nagle (TCP No Delay) algorithm */
00105             int flag = 1;
00106             rv = setsockopt( sockfd, IPPROTO_TCP, TCP_NODELAY, (char *)&flag, sizeof(flag) );
00107             if (rv == -1) {
00108               printf("Couldn't setsockopt(TCP_NODELAY)\n");
00109               exit( -1 );
00110             }
00111 
00112             // connect to the server
00113             server = gethostbyname(ip);
00114             if (server == NULL) {
00115                 fprintf(stderr,"ERROR, no such host\n");
00116                 exit(0);
00117             }
00118             bzero((char *) &serv_addr, sizeof(serv_addr));
00119             serv_addr.sin_family = AF_INET;
00120             bcopy((char *)server->h_addr,
00121                  (char *)&serv_addr.sin_addr.s_addr,
00122                  server->h_length);
00123             serv_addr.sin_port = htons(tcpPortNum);
00124             if (connect(sockfd,(struct sockaddr *) &serv_addr,sizeof(serv_addr)) < 0)
00125                 error("ERROR connecting");
00126             set_nonblock(sockfd);
00127             printf("connected to server\n");
00128             return sockfd;
00129         }
00130         return -1;
00131 }
00132 
00133 int elCommRead(int fd)
00134 {
00135         unsigned char c;
00136         unsigned int i;
00137         int rv;
00138         rv = read(fd, &c, 1);   // read one byte
00139         i = c;                                  // convert byte to an int for return
00140         if (rv > 0)
00141                 return i;                       // return the character read
00142         if (rv < 0) {
00143                 if ((errno != EAGAIN) && (errno != EWOULDBLOCK))
00144                         perror("elCommRead() error:");
00145         }
00146         return rv;                              // return -1 or 0 either if we read nothing, or if read returned negative
00147 }
00148 
00149 int elCommWrite(int fd, uint8_t* data, int len)
00150 {
00151         int rv;
00152         int length = len;
00153         int totalsent = 0;
00154         while (totalsent < length) {
00155                 rv = write(fd, data+totalsent, length);
00156                 if (rv < 0)
00157                         perror("write(): error writing - trying again - ");
00158                 else
00159                         totalsent += rv;
00160         }
00161         return rv;
00162 }
00163 
00164 #endif


rosserial_embeddedlinux
Author(s): Paul Bouchier
autogenerated on Mon Oct 6 2014 07:10:50