00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036
00037
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
00050 }
00051
00052
00053
00054
00055 Sub20Interface::~Sub20Interface()
00056 {
00057 }
00058
00059
00060
00061
00062 bool Sub20Interface::initialize()
00063 {
00064
00065
00066
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
00077
00078
00079 while( subdev_ = sub_find_devices( subdev_ ) )
00080 {
00081 handle_ = sub_open( subdev_ );
00082
00083 if( handle_ == NULL )
00084 {
00085
00086 ROS_WARN("Sub20Interface::initialize(): sub_open status: %s", sub_strerror(sub_errno));
00087 }
00088 else
00089 {
00090
00091 char found_serial_number[11];
00092 sub_get_serial_number( handle_, found_serial_number, 10 );
00093
00094
00095
00096 if( strcmp( "null", found_serial_number ) == 0 )
00097 {
00098
00099 serialNumber_ = std::string( found_serial_number );
00100 device_found = true;
00101 break;
00102 }
00103
00104
00105 if( strcmp( found_serial_number, serialNumber_.c_str() ) == 0)
00106 {
00107 device_found = true;
00108 break;
00109 }
00110 }
00111
00112 }
00113
00114 if( device_found == true )
00115 break;
00116
00117
00118
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
00125 ROS_ERROR("Sub20Interface::initialize() Couldn't find sub20 [serial: \"%s\"]. ", serialNumber_.c_str() );
00126 return false;
00127 }
00128
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
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
00161 if( spiConfigRoutine( frequency, *flags ) == false )
00162 {
00163 ROS_ERROR("Sub20Interface::read(...): SPI configuration failed.");
00164 return -1;
00165 }
00166
00167
00168
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
00178 uint8_t input_data[num_bytes + 1];
00179
00180
00181 switch( device_address )
00182 {
00183 case NULL_DEVICE:
00184
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
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
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
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
00260
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
00269 error_code += sub_spi_transfer( handle_, (char*)output_data, 0, (num_bytes+1), SS_CONF(device_address, 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
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
00356 uint8_t bit_order = ( (flags>>2) & 0x01 );
00357 uint8_t mode = (flags & 0x03);
00358 uint8_t spi_slave_select = ((flags >> 4) & 0x0F);
00359
00360
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
00375 switch( mode )
00376 {
00377 case 0:
00378 spi_configuration_ |= SPI_CPOL_RISE | SPI_SMPL_SETUP;
00379 break;
00380 case 1:
00381 spi_configuration_ |= SPI_CPOL_RISE | SPI_SETUP_SMPL;
00382 break;
00383 case 2:
00384 spi_configuration_ |= SPI_CPOL_FALL | SPI_SMPL_SETUP;
00385 break;
00386 case 3:
00387 spi_configuration_ |= SPI_CPOL_FALL | SPI_SETUP_SMPL;
00388 break;
00389 default:
00390
00391 ROS_ERROR("Sub20Interface: Invalid Sub20 CPOL and CPHA settings.");
00392 return false;
00393 }
00394
00395
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
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 }