embedded_linux_comms.c
Go to the documentation of this file.
00001 /*
00002  * Software License Agreement (BSD License)
00003  *
00004  * Copyright (c) 2012, Paul Bouchier
00005  * All rights reserved.
00006  *
00007  * Redistribution and use in source and binary forms, with or without
00008  * modification, are permitted provided that the following conditions
00009  * are met:
00010  *
00011  *  * Redistributions of source code must retain the above copyright
00012  *    notice, this list of conditions and the following disclaimer.
00013  *  * Redistributions in binary form must reproduce the above
00014  *    copyright notice, this list of conditions and the following
00015  *    disclaimer in the documentation and/or other materials provided
00016  *    with the distribution.
00017  *  * Neither the name of Willow Garage, Inc. nor the names of its
00018  *    contributors may be used to endorse or promote prducts derived
00019  *    from this software without specific prior written permission.
00020  *
00021  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00022  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00023  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00024  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00025  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00026  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00027  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00028  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00029  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00030  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00031  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00032  * POSSIBILITY OF SUCH DAMAGE.
00033  */
00034 
00035 #ifndef ROS_EMBEDDED_LINUX_COMMS_H
00036 #define ROS_EMBEDDED_LINUX_COMMS_H
00037 
00038 #include <stdio.h>
00039 #include <stdlib.h>
00040 #include <unistd.h>
00041 #include <fcntl.h>
00042 #include <errno.h>
00043 #include <termios.h>
00044 #include <string.h>
00045 #include <time.h>
00046 #include <stdint.h>
00047 #include <sys/types.h>
00048 #include <sys/socket.h>
00049 #include <netinet/in.h>
00050 #include <netinet/tcp.h>
00051 #include <netdb.h>
00052 #include <assert.h>
00053 
00054 #define DEFAULT_PORTNUM 11411
00055 
00056 void error(const char *msg)
00057 {
00058   perror(msg);
00059   exit(0);
00060 }
00061 
00062 void set_nonblock(int socket)
00063 {
00064   int flags;
00065   flags = fcntl(socket, F_GETFL, 0);
00066   assert(flags != -1);
00067   fcntl(socket, F_SETFL, flags | O_NONBLOCK);
00068 }
00069 
00070 int elCommInit(const char *portName, int baud)
00071 {
00072   struct termios options;
00073   int fd;
00074   int sockfd;
00075   struct sockaddr_in serv_addr;
00076   struct hostent *server;
00077   int rv;
00078 
00079   if (*portName == '/')       // linux serial port names always begin with /dev
00080   {
00081     printf("Opening serial port %s\n", portName);
00082 
00083     fd = open(portName, O_RDWR | O_NOCTTY | O_NDELAY);
00084 
00085     if (fd == -1)
00086     {
00087       // Could not open the port.
00088       perror("init(): Unable to open serial port - ");
00089     }
00090     else
00091     {
00092       // Sets the read() function to return NOW and not wait for data to enter
00093       // buffer if there isn't anything there.
00094       fcntl(fd, F_SETFL, FNDELAY);
00095 
00096       // Configure port for 8N1 transmission, 57600 baud, SW flow control.
00097       tcgetattr(fd, &options);
00098       cfsetispeed(&options, B57600);
00099       cfsetospeed(&options, B57600);
00100       options.c_cflag |= (CLOCAL | CREAD);
00101       options.c_cflag &= ~PARENB;
00102       options.c_cflag &= ~CSTOPB;
00103       options.c_cflag &= ~CSIZE;
00104       options.c_cflag |= CS8;
00105       options.c_cflag &= ~CRTSCTS;
00106       options.c_lflag &= ~(ICANON | ECHO | ECHOE | ISIG);
00107       options.c_iflag &= ~(IXON | IXOFF | IXANY);
00108       options.c_oflag &= ~OPOST;
00109 
00110       // Set the new options for the port "NOW"
00111       tcsetattr(fd, TCSANOW, &options);
00112     }
00113     return fd;
00114   }
00115   else
00116   {
00117     // Split connection string into IP address and port.
00118     const char* tcpPortNumString = strchr(portName, ':');
00119     long int tcpPortNum;
00120     char ip[16];
00121     if (!tcpPortNumString)
00122     {
00123       tcpPortNum = DEFAULT_PORTNUM;
00124       strncpy(ip, portName, 16);
00125     }
00126     else
00127     {
00128       tcpPortNum = strtol(tcpPortNumString + 1, NULL, 10);
00129       strncpy(ip, portName, tcpPortNumString - portName);
00130     }
00131 
00132     printf("Connecting to TCP server at %s:%ld....\n", ip, tcpPortNum);
00133 
00134     // Create the socket.
00135     sockfd = socket(AF_INET, SOCK_STREAM, 0);
00136     if (sockfd < 0)
00137     {
00138       error("ERROR opening socket");
00139       exit(-1);
00140     }
00141 
00142     // Disable the Nagle (TCP No Delay) algorithm.
00143     int flag = 1;
00144     rv = setsockopt(sockfd, IPPROTO_TCP, TCP_NODELAY, (char *)&flag, sizeof(flag));
00145     if (rv == -1)
00146     {
00147       printf("Couldn't setsockopt(TCP_NODELAY)\n");
00148       exit(-1);
00149     }
00150 
00151     // Connect to the server
00152     server = gethostbyname(ip);
00153     if (server == NULL)
00154     {
00155       fprintf(stderr, "ERROR, no such host\n");
00156       exit(0);
00157     }
00158     bzero((char *) &serv_addr, sizeof(serv_addr));
00159     serv_addr.sin_family = AF_INET;
00160     bcopy((char *)server->h_addr,
00161           (char *)&serv_addr.sin_addr.s_addr,
00162           server->h_length);
00163     serv_addr.sin_port = htons(tcpPortNum);
00164     if (connect(sockfd, (struct sockaddr *) &serv_addr, sizeof(serv_addr)) < 0)
00165       error("ERROR connecting");
00166     set_nonblock(sockfd);
00167     printf("connected to server\n");
00168     return sockfd;
00169   }
00170   return -1;
00171 }
00172 
00173 int elCommRead(int fd)
00174 {
00175   unsigned char c;
00176   unsigned int i;
00177   int rv;
00178   rv = read(fd, &c, 1); // read one byte
00179   i = c;          // convert byte to an int for return
00180   if (rv > 0)
00181     return i;     // return the character read
00182   if (rv < 0)
00183   {
00184     if ((errno != EAGAIN) && (errno != EWOULDBLOCK))
00185       perror("elCommRead() error:");
00186   }
00187 
00188   // return -1 or 0 either if we read nothing, or if read returned negative
00189   return rv;
00190 }
00191 
00192 int elCommWrite(int fd, uint8_t* data, int len)
00193 {
00194   int rv;
00195   int length = len;
00196   int totalsent = 0;
00197   while (totalsent < length)
00198   {
00199     rv = write(fd, data + totalsent, length);
00200     if (rv < 0)
00201       perror("write(): error writing - trying again - ");
00202     else
00203       totalsent += rv;
00204   }
00205   return rv;
00206 }
00207 
00208 #endif


rosserial_embeddedlinux
Author(s): Paul Bouchier
autogenerated on Sat Oct 7 2017 03:08:43