sub20_interface.cpp
Go to the documentation of this file.
00001 /*********************************************************************
00002  *
00003  * Software License Agreement (BSD License)
00004  *
00005  *  Copyright (c) 2012, Robert Bosch LLC.
00006  *  All rights reserved.
00007  *
00008  *  Redistribution and use in source and binary forms, with or without
00009  *  modification, are permitted provided that the following conditions
00010  *  are met:
00011  *
00012  *   * Redistributions of source code must retain the above copyright
00013  *     notice, this list of conditions and the following disclaimer.
00014  *   * Redistributions in binary form must reproduce the above
00015  *     copyright notice, this list of conditions and the following
00016  *     disclaimer in the documentation and/or other materials provided
00017  *     with the distribution.
00018  *   * Neither the name of the Robert Bosch nor the names of its
00019  *     contributors may be used to endorse or promote products derived
00020  *     from this software without specific prior written permission.
00021  *
00022  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00023  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00024  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00025  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00026  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00027  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00028  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00029  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00030  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00031  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00032  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00033  *  POSSIBILITY OF SUCH DAMAGE.
00034  *
00035  *********************************************************************/
00036 
00037 //\Author Joshua Vasquez and Philip Roan, Robert Bosch LLC
00038 
00039 #include "sub20_interface/sub20_interface.hpp"
00040 
00041 /**********************************************************************/
00042 /**********************************************************************/
00043 Sub20Interface::Sub20Interface( std::string sub20_serial_number ) :
00044   serialNumber_( sub20_serial_number ),
00045   num_devices_( 0 ),
00046   subdev_( NULL ),
00047   is_initialized_( false )
00048 {
00049   // make sure serialNumber_ is at least 10 chars
00050 }
00051 
00052 
00053 /**********************************************************************/
00054 /**********************************************************************/
00055 Sub20Interface::~Sub20Interface()
00056 {
00057 }
00058 
00059 
00060 /**********************************************************************/
00061 /**********************************************************************/
00062 bool Sub20Interface::initialize()
00063 {
00064   /* To handle a setup with multiple sub20s where One or more of them is 
00065    * unplugged, we may want to rewrite the search routine so 
00066    * that it stores all the sub20s to a list and iterates through them
00067    */
00068   if( is_initialized_ == true )
00069     return true;
00070 
00071    error_code = 0;
00072   bool device_found = false;
00073 
00074   while( device_found == false )
00075   {
00076     // scan for sub20s, starting at the first.
00077     //ROS_INFO("Searching for sub20(%s).", serialNumber_);  
00078 
00079     while( subdev_ = sub_find_devices( subdev_ ) )
00080     { // get a handle on that sub20
00081       handle_ = sub_open( subdev_ );
00082 
00083       if( handle_ == NULL )
00084       {
00085         // if there's an error trying to get a handle on that sub20, keep going!  That one has probably already been setup.
00086         ROS_WARN("Sub20Interface::initialize(): sub_open status: %s", sub_strerror(sub_errno)); 
00087       }
00088       else  // FOUND an unused sub20 plugged in:
00089       {
00090         // get the current sub20's serial number. Put it in the buffer.
00091         char found_serial_number[11];
00092         sub_get_serial_number( handle_, found_serial_number, 10 );
00093         //ROS_INFO("Found: sub20(%s).", found_serial_number );
00094     
00095         // if no serial number was specified in the constructor:
00096         if( strcmp( "null", found_serial_number ) == 0 )
00097         {
00098           // make the discovered serial number the class's serial number:
00099           serialNumber_ = std::string( found_serial_number );
00100           device_found = true;
00101           break;
00102         }
00103     
00104         // if a serial number was specified, check if it's the right sub20:
00105         if( strcmp( found_serial_number, serialNumber_.c_str() ) == 0)
00106         {
00107           device_found = true;
00108           break;
00109         }
00110       }
00111       // Otherwise scan for the next sub20 up at the top
00112     }
00113  
00114     if( device_found == true )
00115       break;
00116    
00117     // TWO possibilities for failure:
00118     // 1. if no sub20s are available:
00119     if( strcmp( "null", serialNumber_.c_str() ) == 0 )
00120     {
00121       ROS_ERROR("Sub20Interface::initialize() Couldn't find any sub20s plugged in.");
00122       return false;
00123     }
00124     // 2. if we've looked through all sub20s, and none have the matching serial number:
00125     ROS_ERROR("Sub20Interface::initialize() Couldn't find sub20 [serial: \"%s\"]. ", serialNumber_.c_str() );  
00126     return false;
00127   }
00128   // SUCCESS:
00129   ROS_INFO("Sub20Interface::initialize() sub20 [serial: \"%s\"] initialization complete.", serialNumber_.c_str() );
00130   is_initialized_ = true;
00131   return true;
00132 }
00133 
00134 
00135 /**********************************************************************/
00136 /**********************************************************************/
00137 ssize_t Sub20Interface::read( int device_address, interface_protocol protocol, int frequency, int* flags, uint8_t reg_address, uint8_t* data, size_t num_bytes )
00138 {
00139   int error_code = 0;
00140 
00141   switch( protocol )
00142   {
00143   case I2C:
00144   {
00145     // configure i2c bus according to parameters:
00146     if( sub_i2c_freq( handle_, &frequency ) < 0 )
00147     {
00148       ROS_ERROR("Sub20Interface: I2c setup failed. \r Status: %s", sub_strerror(sub_errno) );
00149       return -1;
00150     }
00151     if( sub_i2c_read( handle_, device_address, reg_address, 1, (char*)data, num_bytes ) < 0 )
00152     {
00153       ROS_ERROR("Sub20Interface: I2c read failed. \r Status: %s", sub_strerror(sub_errno));
00154       return -1;
00155     }
00156     break;
00157   }
00158   case SPI:
00159   {
00160     // configure SPI bus according to parameters:
00161     if( spiConfigRoutine( frequency, *flags ) == false )
00162     {
00163       ROS_ERROR("Sub20Interface::read(...): SPI configuration failed.");
00164       return -1;
00165     }
00166 
00167     // SPI Reading has extra overhead:
00168     //CREATE OUTPUT DATA ARRAY: The output data is an array with reg address followed by num_bytes of dud values.
00169     uint8_t output_data[num_bytes + 1];
00170     output_data[0] = reg_address;
00171   
00172     for( int i = 0; i < (int)num_bytes; i++ )
00173     {
00174       output_data[i+1] = (uint8_t)0xFF;
00175     }
00176   
00177     // CREATE INPUT DATA ARRAY:
00178     uint8_t input_data[num_bytes + 1];
00179   
00180     // BEGIN transfer: two variations, depending on device address
00181     switch( device_address )
00182     {
00183     case NULL_DEVICE:
00184       // Transfer without toggling chip_select line:
00185       if( sub_spi_transfer( handle_, (char*)output_data, (char*)input_data, (num_bytes + 1), SS_CONF(device_address, SS_H) ) < 0)
00186       {
00187         ROS_ERROR( "Sub20Interface::read(...): SPI read failed. \r Status: %s", sub_strerror(sub_errno) );
00188         return -1;
00189       }
00190       break;
00191     default:
00192       // Transfer normally (pull chip_select line low):
00193       if( sub_spi_transfer( handle_, (char*)output_data, (char*)input_data, (num_bytes + 1), SS_CONF(device_address, SS_LO) ) < 0)
00194       {
00195         ROS_ERROR( "Sub20Interface::read(...): SPI read failed. \r Status: %s", sub_strerror(sub_errno) );
00196         return -1;
00197       } 
00198     }
00199   
00200     // Transfer reg_address, and then receive num_bytes of data back:
00201     if( sub_spi_transfer( handle_, (char*)output_data, (char*)input_data, (num_bytes + 1), SS_CONF(device_address, SS_LO) ) < 0)
00202     {
00203       ROS_ERROR("Sub20Interface::read(...): SPI read failed. \r Status: %s", sub_strerror(sub_errno));
00204       error_code = (-1);
00205       return error_code;
00206     }
00207   
00208     // Copy input_data into data:
00209     for( int j = 0; j < (int)num_bytes; j++ )
00210     {
00211       data[j] = input_data[j+1];
00212     }
00213   
00214     break;
00215   }
00216   case GPIO:
00217   case RS232:
00218     ROS_ERROR("Sub20Interface: Driver does not support this protocol at this time.");
00219     break;
00220   default:
00221     ROS_ERROR("Sub20Interface: Invalid protocol for Sub20.");
00222     return 1;
00223   }
00224   
00225   return error_code;
00226 }
00227 
00228 
00229 /**********************************************************************/
00230 /**********************************************************************/
00231 ssize_t Sub20Interface::write( int device_address, interface_protocol protocol, int frequency, int* flags, uint8_t reg_address, uint8_t* data, size_t num_bytes )
00232 {
00233   int error_code = 0;
00234   
00235   switch( protocol )
00236   {
00237   case I2C:
00238   {
00239     if( sub_i2c_freq(handle_, &frequency) < 0 )
00240     {
00241       ROS_ERROR("Sub20Interface::write(...): ERROR.  Could not set I2C frequency.");
00242       return -1;
00243     }
00244     if( sub_i2c_write( handle_, device_address, reg_address, 1, (char*)data, num_bytes ) < 0 )
00245     {
00246       ROS_ERROR( "Sub20Interface::write(...): ERROR. Could not write data over I2C interface." );
00247       return -1;
00248     }
00249     break;
00250   }
00251   case SPI:
00252   {
00253     if( spiConfigRoutine( frequency, *flags ) == false )
00254     {
00255       ROS_ERROR("Sub20Interface::write(...): SPI configuration failed.");
00256       return -1;
00257     }
00258 
00259     // SPI writing has extra overhead:  
00260     //CREATE OUTPUT DATA ARRAY:                     The output data is an array with reg address followed by the data.
00261     uint8_t output_data[num_bytes + 1];
00262     output_data[0] = reg_address;
00263   
00264     for( int i = 0; i < (int)num_bytes; i++ )
00265     {
00266       output_data[i+1] = data[i];
00267     }
00268     // transfer reg_address, then data:
00269     error_code += sub_spi_transfer( handle_, (char*)output_data, 0, (num_bytes+1), SS_CONF(device_address, SS_LO) ); //VERIFY SS_LO
00270     return error_code;
00271   }
00272   case GPIO:
00273   case RS232:
00274     ROS_ERROR("Sub20Interface: Driver does not support this protocol at this time.");
00275     break;
00276   default:
00277     ROS_ERROR("Sub20Interface: Invalid protocol for Sub20.");
00278     return -1;
00279   }
00280   
00281   return num_bytes;
00282 }
00283 
00284 
00285 /**********************************************************************/
00286 /**********************************************************************/
00287 bool Sub20Interface::supportedProtocol( interface_protocol protocol )
00288 {
00289   switch( protocol )
00290   {
00291   case GPIO:
00292     return false;
00293   case RS232:
00294     return false;
00295   case SPI:
00296   case I2C:
00297     return true;
00298   default:
00299     return false;
00300   }
00301 }
00302 
00303 
00304 /**********************************************************************/
00305 /**********************************************************************/
00306 std::string Sub20Interface::getID()
00307 {
00308   return serialNumber_;
00309 }
00310 
00311 
00312 /**********************************************************************/
00313 /**********************************************************************/
00314 int Sub20Interface::numDevices()
00315 {
00316   return num_devices_;
00317 }
00318 
00319 
00320 /**********************************************************************/
00321 /**********************************************************************/
00322 bool Sub20Interface::spiConfigRoutine( int frequency, int flags )
00323 {
00324   int spi_configuration_ = SPI_ENABLE;
00325  
00326   // set frequency:
00327   switch( frequency )
00328   {
00329   case 8000000:
00330     spi_configuration_ |= SPI_CLK_8MHZ;
00331     break;
00332   case 4000000:
00333     spi_configuration_ |= SPI_CLK_4MHZ;
00334     break;
00335   case 2000000:
00336     spi_configuration_ |= SPI_CLK_2MHZ;
00337     break;
00338   case 1000000:
00339     spi_configuration_ |= SPI_CLK_1MHZ;
00340     break;
00341   case 500000:
00342     spi_configuration_ |= SPI_CLK_500KHZ;
00343     break;
00344   case 250000:
00345     spi_configuration_ |= SPI_CLK_250KHZ;
00346     break;
00347   case 125000: 
00348     spi_configuration_ |= SPI_CLK_125KHZ;
00349     break;
00350   default:
00351     ROS_ERROR( "Sub20Interface::spiConfigRoutine(): Invalid Sub20 frequency." );
00352     return false;
00353   }
00354  
00355   // decrypt flags, which were set in the parameters class:
00356   uint8_t bit_order = ( (flags>>2) & 0x01 );  //0x01 = B00000001
00357   uint8_t mode = (flags & 0x03);        //0x03 = B00000011
00358   uint8_t spi_slave_select = ((flags >> 4) & 0x0F); //0x0F = B00001111
00359   
00360     // set bit-order:
00361   switch( bit_order )
00362   {
00363   case 0:
00364     spi_configuration_ |= SPI_LSB_FIRST;
00365     break;
00366   case 1:
00367     spi_configuration_ |= SPI_MSB_FIRST;
00368     break;
00369   default:
00370     ROS_ERROR("Sub20Interface: Invalid Sub20 bit-order.");
00371     return false;
00372   }
00373    
00374   // set CPOL and CPHA (identical to Arduino flags)
00375   switch( mode )
00376   {
00377   case 0:
00378     spi_configuration_ |= SPI_CPOL_RISE | SPI_SMPL_SETUP;       // CPOL = 0; CPHA = 0
00379     break;
00380   case 1:
00381     spi_configuration_ |= SPI_CPOL_RISE | SPI_SETUP_SMPL;       // CPOL = 0; CPHA = 1
00382     break;
00383   case 2:
00384     spi_configuration_ |= SPI_CPOL_FALL | SPI_SMPL_SETUP;       // CPOL = 1; CPHA = 0
00385     break;
00386   case 3:
00387     spi_configuration_ |= SPI_CPOL_FALL | SPI_SETUP_SMPL;       // CPOL = 1; CPHA = 1
00388     break;
00389   default:
00390     // impossible, since mode is a two-bit value.
00391     ROS_ERROR("Sub20Interface: Invalid Sub20 CPOL and CPHA settings.");
00392     return false;
00393   }
00394   
00395   // send configuration to the Sub20:
00396   if( sub_spi_config( handle_, spi_configuration_, 0 ) < 0 )
00397   {
00398     ROS_ERROR("Sub20Interface: SPI setup failed. \r Status: %s", sub_strerror(sub_errno)); 
00399     return false;
00400   }
00401   
00402   // verify that the spi_slave_select is valid:
00403   switch( spi_slave_select )
00404   {
00405   case 0:
00406     break;
00407   case 1:
00408     break;
00409   case 2:
00410     break;
00411   case 3:
00412     break;
00413   case 4:
00414     break;
00415   default:
00416     ROS_ERROR("Sub20Interface: Invalid slave select.  Choose a slave on pins 0-4.");
00417     return false;
00418   }
00419   
00420   return true;
00421 }


sub20_interface
Author(s): Joshua Vasquez and Philip Roan. Maintained by Philip Roan
autogenerated on Mon Oct 6 2014 10:09:57