dev8x.cc
Go to the documentation of this file.
00001 /*
00002  *  Copyright (C) 2005, 2007, 2009 Austin Robot Technology
00003  *
00004  *  License: Modified BSD Software License Agreement
00005  * 
00006  *  $Id: dev8x.cc 1876 2011-11-26 23:48:08Z jack.oquin $
00007  */
00008 
00018 #include <assert.h>
00019 #include <errno.h>
00020 #include <stdlib.h>
00021 #include <unistd.h>                     // for sleep
00022 #include <string.h>
00023 #include <fcntl.h>
00024 #include <termios.h>
00025 
00026 #include <ros/ros.h>
00027 
00028 #include "dev8x.h"
00029 
00030 dev8x::dev8x(const char *pn)
00031 {
00032   state = UNREADY;
00033 
00034   // handle config parameters
00035   strncpy(port_name, pn, sizeof(port_name));
00036   have_tty = (strcmp(port_name, "/dev/null") != 0);
00037   DBG("dev8x port: %s", port_name);
00038 
00039   // initialize simulated ports
00040   sim_relays = 0;
00041   for (int i = 0; i < IOADR_MAX_INPUTS; i++)
00042     sim_port[i] = 0;
00043 }
00044 
00045 int dev8x::setup()
00046 {
00047   DBG("setup()");
00048   // open the serial port
00049   fd = open(port_name, O_RDWR | O_NOCTTY);
00050   if (fd < 0) {
00051     ROS_WARN("Couldn't open %s", port_name);
00052     return -1;
00053   }  
00054 
00055   relay_wait_time = 0.0;
00056   state = READY;
00057 
00058   // Configure port 19.2k baud 8n1
00059   DBG("Configuring the ioadr8x.");
00060   configure_port(B19200 | CS8, 0);
00061   DBG("DONE: Configuring the ioadr8x.");
00062   
00063   return 0;
00064 }
00065 
00066 int dev8x::shutdown()
00067 {
00068   DBG("shutdown()");
00069   int rc = close(fd);
00070   state = UNREADY;
00071   return rc;
00072 }
00073 
00074 int dev8x::configure_port (int cflags, int iflags) 
00075 {
00076   assert(state == READY);
00077 
00078   struct termios newtio;
00079   memset(&newtio, 0, sizeof(newtio));
00080         
00081   /* 
00082      BAUDRATE: Set bps rate. You could also use cfsetispeed and cfsetospeed.
00083      CRTSCTS : output hardware flow control (only used if the cable has
00084      all necessary lines. See sect. 7 of Serial-HOWTO)
00085      CS8     : 8n1 (8bit,no parity,1 stopbit)
00086      CLOCAL  : local connection, no modem contol
00087      CREAD   : enable receiving characters
00088   */
00089   newtio.c_cflag = cflags | CLOCAL | CREAD;
00090   
00091   /*
00092     IGNPAR  : ignore bytes with parity errors
00093     ICRNL   : map CR to NL (otherwise a CR input on the other computer
00094     will not terminate input)
00095     otherwise make device raw (no other input processing)
00096   */
00097   newtio.c_iflag = iflags | IGNPAR; // Need ICRNL for QuickSilver Motor
00098   
00099 
00100   newtio.c_oflag = 0;
00101   newtio.c_lflag = 0;
00102   
00103   /* 
00104      initialize all control characters 
00105      default values can be found in /usr/include/termios.h, and are given
00106      in the comments, but we don't need them here
00107   */
00108   newtio.c_cc[VTIME]    = 0;     /* inter-character timer unused */
00109   newtio.c_cc[VMIN]     = 1;     /* blocking read until 1 character received */
00110   
00111   /* 
00112      now clean the modem line and activate the settings for the port
00113   */
00114   tcflush(fd, TCIOFLUSH);
00115 
00116   return tcsetattr(fd,TCSANOW,&newtio);
00117 }
00118 
00119 // get relay values from dev8x
00120 int dev8x::query_relays(uint8_t *relays)
00121 {
00122   if (relays_busy())
00123     return EBUSY;
00124 
00125   // send Status command
00126   buffer[0] = 254;
00127   buffer[1] = 14;
00128   buffer[2] = 24;                       // Get Status
00129   DBG("query_relays: writing command.");
00130   int res = write(fd, buffer, 3);
00131   if (res < 0)
00132     ROS_ERROR_THROTTLE(100, "write() error: %d", errno);
00133 
00134   // read result
00135   DBG("query_relays: reading result.");
00136   int rc = read(fd, buffer, 1);
00137   if (rc == 1)
00138     *relays = buffer[0];
00139   else if (have_tty == false)           // using /dev/null
00140     *relays = sim_relays;
00141   else                                  // I/O error
00142     {
00143       if (rc == 0) rc = EBUSY;
00144       ROS_WARN("ioadr8x read error: %s", strerror(rc));
00145       return rc;
00146     }
00147 
00148   DBG("relay status: 0x%02x", *relays);
00149   return 0;
00150 }
00151 
00152 int dev8x::read_8bit_port(int ch, int *data)
00153 {
00154   if (relays_busy())
00155     return EBUSY;
00156 
00157   // Send Command
00158   buffer[0] = 254;
00159   buffer[1] = 6;
00160   buffer[2] = ch;
00161   int res = write(fd, buffer, 3);
00162   if (res < 0)
00163     ROS_ERROR_THROTTLE(100, "write() error: %d", errno);
00164 
00165   int rc = read(fd, buffer, 1);
00166   if (rc == 1)                          // success?
00167     *data = buffer[0];
00168   else if (have_tty == false)           // simulated port?
00169     *data = sim_sawtooth(ch, 1<<8);
00170   else                                  // I/O error
00171     {
00172       ROS_WARN("ioadr8x read error: ch = %d, rc = %d", ch, rc);
00173       if (rc < 0)
00174         return errno;
00175       else
00176         return EIO;
00177     }
00178 
00179   DBG("port %d returns 0x%02x", ch, *data);
00180   return 0;
00181 }
00182 
00183 int dev8x::read_10bit_port(int ch, int *data)
00184 {
00185   if (relays_busy())
00186     return EBUSY;
00187 
00188   // can only read 10-bit data from analog ports (3-7)
00189   assert(ch >= 3);
00190 
00191   // Send Command
00192   buffer[0] = 254;
00193   buffer[1] = 6;
00194   buffer[2] = 8 + ch - 3;
00195   int res = write(fd, buffer, 3);
00196   if (res < 0)
00197     ROS_ERROR_THROTTLE(100, "write() error: %d", errno);
00198 
00199   int rc = read(fd, &buffer[1], 1);
00200   if (rc != 1 && have_tty)              // failure?
00201     {
00202       ROS_WARN("ioadr8x 10-bit read error: ch = %d, rc = %d", ch, rc);
00203       if (rc < 0)
00204         return errno;
00205       else
00206         return EIO;
00207     }
00208 
00209   rc = read(fd, buffer, 1);
00210   if (rc == 1)                          // success?
00211     *data = (buffer[1]<<8) | buffer[0];
00212   else if (have_tty == false)           // simulated port?
00213     *data = sim_sawtooth(ch, 1<<10);
00214   else                                  // I/O error
00215     {
00216       ROS_WARN("ioadr8x 10-bit read error: ch = %d, rc = %d", ch, rc);
00217       if (rc < 0)
00218         return errno;
00219       else
00220         return EIO;
00221     }
00222 
00223   DBG("port %d returns 0x%04x", ch, *data);
00224   return 0;
00225 }
00226 
00227 // returns true if last relay write has not completed
00228 bool dev8x::relays_busy(void)
00229 {
00230   if (state == RELAY_WAIT)
00231     {
00232       double now = ros::Time::now().toSec();
00233       if ((now - relay_wait_time) > MIN_RELAY_WAIT)
00234         {
00235           state = READY;
00236           DBG("accessing relays (time %.6f)", now);
00237         }
00238       else
00239         {
00240           DBG("tried to access relays too soon after setting them"
00241               " (time %.6f)", now);
00242           return true;
00243         }
00244     }
00245   return false;
00246 }
00247 
00248 // send new relay values to ioadr8x
00249 int dev8x::set_relays(uint8_t bitmask)
00250 {
00251   if (relays_busy())
00252     return EBUSY;
00253 
00254   DBG("setting relays to 0x%02x", bitmask);
00255 
00256   if (have_tty == false)
00257     {
00258       sim_relays = bitmask;
00259       return 0;
00260     }
00261 
00262   assert(state == READY);
00263   buffer[0] = 254;
00264   buffer[1] = 14;
00265   buffer[2] = 40;
00266   buffer[3] = bitmask;
00267   int rc = write(fd, buffer, 4);
00268   if (rc == 4)
00269     {
00270       // The device requires a wait after setting all relays before
00271       // accessing it again.  Since we return immediately, the ioadr
00272       // driver must leave them alone until its next cycle, which
00273       // should be long enough for the device to finish.  If it isn't,
00274       // the next operation will return EBUSY.
00275       relay_wait_time = ros::Time::now().toSec();
00276       state = RELAY_WAIT;
00277       return 0;
00278     }
00279   else                                  // I/O failed
00280     {
00281       if (rc < 0)
00282         rc = errno;
00283       else
00284         rc = EIO;
00285       ROS_WARN("write error: %s", strerror(rc));
00286       return rc;
00287     }
00288 }
00289 
00290 // Simulate analog port returning a sawtooth wave, making it easy to
00291 // see if messages get lost.
00292 //
00293 // entry: sim_port[ch] contains next value to use
00294 // exit:  sim_port[ch] updated
00295 uint16_t dev8x::sim_sawtooth(int ch, uint16_t limit)
00296 {
00297   uint16_t data = sim_port[ch]++;
00298   if (sim_port[ch] >= limit)
00299     sim_port[ch] = 0;
00300   return data;
00301 }


art_servo
Author(s): Austin Robot Technology, Jack O'Quin
autogenerated on Fri Jan 3 2014 11:09:12