bmp085_w_sub20.cpp
Go to the documentation of this file.
00001 
00062 //\Author Philip Roan, Robert Bosch LLC
00063 
00064 //ROS Headers
00065 #include <ros/ros.h>
00066 #include <ros/time.h>
00067 
00068 #include <libsub.h> // sub20 device
00069 
00070 #include "bmp085/bmp085_bst.h"
00071 #include "bmp085/bmp085_measurement.h" //message
00072 
00073 
00074 
00075 // This file is a testbench to test the BMP085 pressure sensor
00076 // It uses a Sub20 device to provide I2C communication to the sensor
00077 
00078 
00079 
00080 // Xdimax Sub20 Device Variables
00081   
00083 sub_handle handle_;
00084 sub_device subdev_;
00085 // device serial number
00086 std::string serialNumber_;
00087 // I2C communication frequency (standard mode: 100 kHz)
00088 sub_int32_t i2c_frequency_; // value set in constructor
00089 
00090 
00091 bool sub20_init( void )
00092 {
00093   int error_code = 0; //Error code for Sub20 API
00094   bool device_found = false; 
00095 
00096   subdev_ = sub_find_devices( NULL ); // start a new search for Sub20 devices
00097   while( subdev_ != NULL )
00098   {
00099     handle_ = sub_open( subdev_ ); // open found subdevice
00100     
00101     if( handle_ == 0 ) // on success, sub_open returns non-zero handle
00102     {
00103       //sub_open was not successful
00104       ROS_ERROR("sub_open: %s", sub_strerror(sub_errno));
00105       return false;
00106     }
00107   
00108     // Subdevice successfully opened
00109     ROS_DEBUG("Device Handle: %ld", (long)handle_);
00110     // Read Serial number of subdevice
00111     if( sub_get_serial_number(handle_, const_cast<char*>(serialNumber_.c_str()), serialNumber_.size() ) >= 0 )
00112     {
00113       ROS_DEBUG("Serial Number: %s", serialNumber_.c_str());
00114     }
00115 
00116     ROS_DEBUG("Initializing I2C interface.");
00117     // !! Hack on !!
00118     i2c_frequency_ = 400000; // [Hz]
00119     // !! Hack off !!
00120     error_code = sub_i2c_freq(handle_, &i2c_frequency_); // Set I2C frequency
00121     ROS_DEBUG("I2C initialized. Communicating at %d Hz.", i2c_frequency_);
00122     if( error_code != 0 )
00123     {
00124       ROS_ERROR("Sub20 I2C configuration failed. Status: 0x%02x.", sub_i2c_status);
00125       sub_close( handle_ );
00126       return false;
00127     }
00128 
00129     //return true;
00130 
00131     // Scan for I2C slave devices
00132     ROS_DEBUG("Scanning for I2C devices.");
00133     int num_i2c_devices;        // number of i2c slave devices attached to this Sub20
00134     char device_addresses[112]; // i2c has a max of 112 devices per bus
00135     
00136     error_code = sub_i2c_scan( handle_, &num_i2c_devices, device_addresses ); //scan for devices
00137     if( error_code != 0 )
00138     {
00139       ROS_ERROR("No I2C devices connected to Sub20.");
00140       sub_close( handle_ );
00141       return false;
00142     }
00143     ROS_DEBUG("Number of I2C slaves detected: %d", num_i2c_devices);
00144     for(int i = 0; i < num_i2c_devices; i++ )
00145     {
00146       ROS_DEBUG("%d. Slave address: 0x%02x", i+1, int(device_addresses[i])); // display found devices
00147       
00148       //verify that an I2C device exists at the expected address
00149       if( BMP085_I2C_ADDR == (int)device_addresses[i] )
00150       {
00151         device_found = true;
00152         ROS_DEBUG("I2C device at BMP085 address (0x%02x) exists.", BMP085_I2C_ADDR);
00153       }
00154     }
00155     if( device_found == false )
00156     {
00157       ROS_ERROR("No I2C device at address 0x%02x.", BMP085_I2C_ADDR);
00158       sub_close( handle_ );
00159       return false;
00160     }
00161     else
00162     {
00163       return true;
00164     }
00165 
00166     // search for the next Sub20 device and loop
00167     subdev_ = sub_find_devices(subdev_);
00168   }
00169 
00170   ROS_ERROR("No Sub20 device found.");
00171   return false;
00172 }
00173 
00174 //* Write to the BMP085 over I2C using the Sub20 device.
00175 BMP085_BUS_WR_RETURN_TYPE sub20_bmp085_write( unsigned char device_addr, unsigned char register_addr, unsigned char* register_data, unsigned char write_length )
00176 {
00177   int error_code = 0;
00178 
00179   error_code = sub_i2c_write( handle_, device_addr, register_addr, 1, (char*)register_data, write_length );
00180 
00181   return (char)error_code;
00182 }
00183 
00184 //* Write to the BMP085 over I2C using the Sub20 device.
00185 BMP085_BUS_RD_RETURN_TYPE sub20_bmp085_read( unsigned char device_addr, unsigned char register_addr, unsigned char* register_data, unsigned char read_length )
00186 {
00187   int error_code = 0;
00188 
00189   error_code = sub_i2c_read( handle_, device_addr, register_addr, 1, (char*)register_data, read_length );
00190 
00191   return (char)error_code;
00192 }
00193 
00194 //* Have the node sleep while waiting for the conversion to complete
00195 BMP085_MDELAY_RETURN_TYPE bmp085_delay( BMP085_MDELAY_DATA_TYPE milliseconds)
00196 {
00197   ros::Duration( milliseconds / 1e3 ).sleep();
00198 }
00199 
00200 
00201 int main( int argc, char** argv )
00202 {
00203 
00204   // ROS init
00205   ros::init(argc, argv, "BMP085");
00206   ros::NodeHandle nh;
00207   
00208   // Hardware interface
00209   //sub20_ interface;
00210 
00211   // Create an instance of the sensor
00212   bmp085_t sensor;
00213   // Set function pointers
00214   sensor.bus_write = &sub20_bmp085_write;
00215   sensor.bus_read = &sub20_bmp085_read;
00216   sensor.delay_msec = &bmp085_delay;
00217 
00218   // Turn into ROS parameters
00219   bool run_continuous_ = true;
00220 
00221 
00222   // Create a publisher node
00223   ros::Publisher sensor_pub = nh.advertise<bmp085::bmp085_measurement> ("BMP085_measurements", 100);
00224   // Create message
00225   bmp085::bmp085_measurement msg;
00226 
00227   // Create service handles
00228   //ros::ServiceServer sensor_service1 = nh.advertiseService("bmp085/get_temperature", &bmp085_::
00229 
00230 
00231   // Initialize hardware
00232   if( sub20_init() == false )
00233     return 1;
00234 
00235 
00236   // Wait for BMP085 to be attached and configured (1 Hz loop rate)
00237   ros::Rate wait_interval(1);
00238   while( nh.ok() )
00239   {
00240     int return_value = 0;
00241 
00242     return_value = bmp085_init( &sensor );
00243 
00244     ROS_DEBUG("Tried BMP085 initialization: %d", return_value );
00245     ROS_DEBUG("Sensor type: %d", sensor.sensortype);
00246   
00247     if( return_value == 0 )
00248     {
00249       break;
00250     }
00251     else
00252     {
00253       msg.is_connected = false;
00254       msg.header.stamp = ros::Time::now();
00255     
00256       sensor_pub.publish( msg );
00257       ros::spinOnce();
00258       wait_interval.sleep();
00259     }
00260   }
00261 
00262   long T, P;
00263   unsigned long ut, up;
00264   
00265   while( run_continuous_ && nh.ok() )
00266   {
00267     msg.is_connected = true;
00268 
00269     // get measurement here
00270     ut = bmp085_get_ut();
00271     up = bmp085_get_up();
00272 
00273     T = bmp085_get_temperature( ut );
00274     P = bmp085_get_pressure( up );
00275 
00276 
00277     msg.header.stamp = ros::Time::now();
00278     msg.temperature = T / 10.0; // [C]
00279     msg.pressure = P / 1e3; // [kPa]
00280     
00281     sensor_pub.publish( msg );
00282     ros::spinOnce();
00283 
00284     wait_interval.sleep();
00285   }
00286 
00287   return 0;
00288 }


bmp085
Author(s): Philip Roan
autogenerated on Sat Dec 28 2013 16:49:31