bma180.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 /*********************************************************************
00038  *
00039  * Disclaimer
00040  *
00041  * This source code is not officially released or supported by Bosch Sensortec.
00042  * Please contact the listed authors with bugs or other modifications.
00043  * If you would like the official Bosch Sensortec drivers, please contact:
00044  * contact@bosch-sensortec.com
00045  *
00046  *********************************************************************/
00047 
00048 //\Author Joshua Vasquez and Philip Roan, Robert Bosch LLC
00049 
00050 // For calibration routine
00051 #include <iostream> 
00052 
00053 // ROS headers for debugging output
00054 #include <ros/console.h>
00055 
00056 #include "bma180_driver/bma180.hpp"
00057 #include "bma180_driver/bma180_parameters.hpp"
00058 
00059 
00060 /**********************************************************************/
00061 // Constructor
00062 /**********************************************************************/
00063 BMA180::BMA180( bosch_hardware_interface* hw ) :
00064   sensor_driver( hw ),
00065   BMA180Parameters() 
00066 {
00067   // Set defaults:
00068   TempSlope_ = 0.5;
00069 }
00070 
00071 
00072 /**********************************************************************/
00073 // Destructor
00074 /**********************************************************************/
00075 BMA180::~BMA180()
00076 {
00077 }
00078 
00079 
00080 /**********************************************************************/
00081 /**********************************************************************/
00082 uint8_t BMA180::getDeviceAddress()
00083 {
00084   // depends on the protocol:
00085   switch( this->getProtocol() )
00086   {
00087   case I2C:
00088     // depends on hardware configuration:
00089     if( this->getSlaveAddressBit() == 0 )
00090       return SLAVE_ADDRESS0; 
00091     else
00092       if( this->getSlaveAddressBit() == 1 )
00093         return SLAVE_ADDRESS1;
00094       else   
00095         ROS_ERROR( "BMA180::getDeviceAddress(): invalid I2C address" );
00096     break;
00097   case SPI:
00098     return this->getPin();
00099   default:
00100     ROS_ERROR( "BMA180::getDeviceAddress(): sensor has no identification. Either setPin(uint8_t pin) for SPI or setSlaveAddress( 0 or 1) for I2C." );
00101     return 255;
00102   }
00103   ROS_ERROR( "BMA180::getDeviceAddress(): sensor has no identification. Either setPin(uint8_t pin) for SPI or setSlaveAddress( 0 or 1) for I2C." );
00104   return 255;
00105 }
00106 
00107 
00108 /**********************************************************************/
00109 // Initialize sensor and hardware interface on user-requested parameters
00110 /**********************************************************************/
00111 bool BMA180::initialize()
00112 {
00113   ROS_INFO( " " );
00114   ROS_INFO( "BMA180::initialize(): Device Address (hex): %x",this->getDeviceAddress() );
00115   ROS_INFO( "BMA180::initialize(): Protocol:             %d",this->getProtocol() );
00116   ROS_INFO( "BMA180::initialize(): Frequency:            %d",this->getFrequency() );
00117  
00118   // Initialize the hardware interface
00119   if( hardware_->initialize() == false )
00120     return false;
00121 
00122   // Reset the sensor:
00123   if( this->softReset() == false )
00124     return false;
00125   ROS_INFO( "BMA180::initialize(): soft reset applied." );
00126 
00127 // Disable I2C, if sensor is setup for SPI mode:
00128   switch( this->getProtocol() )
00129   {
00130   case I2C:
00131     break;
00132   case SPI:
00133     // disable i2c to prevent accidental malfunctions:
00134     if( this->DisableI2C() == false )
00135       return false;
00136     ROS_INFO( "BMA180::initialize(): Disabled I2C mode." );
00137     break;
00138   default: // shouldn't happen. User should know to select either SPI or I2C.
00139     ROS_ERROR( "BMA180::initialize(): BMA180 cannot be read with selected protocol." );
00140     return false;
00141   }
00142   
00143   // Enable EEPROM and Register Writing:
00144   if( this->EnableWriting() == false )
00145     return false;
00146   
00147   // Change accel_range on the sensor to match requested parameter range:
00148   if( this->changeAccelRange() == false )
00149     return false;
00150 
00151   // Change bandwidth_ on the sensor to match requested parameter bandwidth:
00152   if( this->changeBandwidth() == false )
00153     return false;
00154 
00155   ROS_INFO( "BMA180 initialized." );
00156   sleep( .1 );
00157 
00158   return true;
00159 }
00160 
00161 
00162 /**********************************************************************/
00163 // takeMeasurement
00164 /**********************************************************************/
00165 bool BMA180::takeMeasurement()
00166 {
00167   uint8_t Data[7];
00168   if( this->readSensorData( ADDRESS_ACCLXYZ, Data, 7 ) == false )
00169   {
00170     ROS_ERROR(" BMA180::takeMeasurement(): Unable to read accelerometer data from sensor." );
00171     return false;
00172   }
00173              
00174   AccelX_ = this->getSensitivity() * int( ( ((int16_t)(Data[1] << 8)) | ( ((int16_t)Data[0]) ) ) >> 2 ); // [g]
00175   AccelY_ = this->getSensitivity() * int( ( ((int16_t)(Data[3] << 8)) | ( ((int16_t)Data[2]) ) ) >> 2 ); // [g]
00176   AccelZ_ = this->getSensitivity() * int( ( ((int16_t)(Data[5] << 8)) | ( ((int16_t)Data[4]) ) ) >> 2 ); // [g]
00177   Temperature_ = ( TempSlope_* (int8_t)Data[6] ) + 24; // [C]
00178   return true;
00179 }
00180 
00181 /**********************************************************************/
00182 
00183 /**********************************************************************/
00184 double BMA180::getStaticPitch()
00185 {
00186   // returns the pitch based on the most recent measurements
00187   StaticPitch_ = atan2( AccelY_, AccelZ_ ) ;
00188   return StaticPitch_;
00189 }
00190 
00191 /**********************************************************************/
00192 
00193 /**********************************************************************/
00194 double BMA180::getStaticRoll()
00195 {
00196   StaticRoll_ = atan2( AccelX_, AccelZ_ );
00197   return ( StaticRoll_ );
00198 }
00199 
00200 
00201 /**********************************************************************/
00202 
00203 /**********************************************************************/
00204 bool BMA180::getAccelData()
00205 {
00206   uint8_t Data[6];
00207   if( this->readSensorData( ADDRESS_ACCLXYZ, Data, 6 ) == false )
00208   {
00209     ROS_ERROR( "BMA180::getAccelData(): Unable to read accelerometer data from sensor." );
00210     return false;
00211   }
00212   AccelX_ = this->getSensitivity() * int( ( ((int16_t)(Data[1] << 8)) | ( ((int16_t)Data[0]) ) ) >> 2 ); // [g]
00213   AccelY_ = this->getSensitivity() * int( ( ((int16_t)(Data[3] << 8)) | ( ((int16_t)Data[2]) ) ) >> 2 ); // [g]
00214   AccelZ_ = this->getSensitivity() * int( ( ((int16_t)(Data[5] << 8)) | ( ((int16_t)Data[4]) ) ) >> 2 ); // [g]
00215   return true;
00216 }
00217 
00218 
00219 /**********************************************************************/
00220 
00221 /**********************************************************************/
00222 double BMA180::getAccelX()  
00223 {
00224   uint8_t Data[2];
00225    
00226   // Must read LSB first.  MSB and LSB  must be read in one transaction:
00227   if( this->readSensorData( ADDRESS_ACCLXYZ, Data, 2 ) == false )
00228   {
00229     ROS_ERROR("BMA180: cannot read from this protocol.");
00230     return -9999.9;
00231   }
00232   int raw_data = int(( ((int16_t)(Data[1] << 8)) | ( ((int16_t)Data[0]) ) ) >> 2);
00233   AccelX_ = raw_data * this->getSensitivity();
00234 
00235   return AccelX_;
00236 }
00237 
00238 
00239 
00240 /**********************************************************************/
00241 
00242 /**********************************************************************/
00243 double BMA180::getAccelY()
00244 {
00245   uint8_t Data[2];
00246  
00247   if( this->readSensorData( ADDRESS_ACCLY_LSB, Data, 2 ) == false )
00248   {
00249     ROS_ERROR("BMA180::getAccelY(): failed.");
00250     return -9999.9;
00251   }
00252   double raw_data = (( ((int16_t)(Data[1] << 8)) | ( (int16_t)(Data[0]) ) ) >> 2);
00253   AccelY_ = raw_data * this->getSensitivity();
00254   
00255   return AccelY_;
00256 }
00257 
00258 
00259 /**********************************************************************/
00260 
00261 /**********************************************************************/
00262 double BMA180::getAccelZ()
00263 {
00264   uint8_t Data[2];
00265   if( this->readSensorData( ADDRESS_ACCLZ_LSB, Data, 2 ) == false ) 
00266   {
00267     ROS_ERROR( "BMA180: cannot read from this protocol." );
00268     return -9999.9;
00269   }
00270   
00271   double raw_data = (( ((int16_t)(Data[1] << 8)) | ( (int16_t)(Data[0]) ) ) >> 2);
00272   AccelZ_ = raw_data * this->getSensitivity();
00273     
00274   return AccelZ_;
00275 }
00276 
00277 
00278 /**********************************************************************/
00279 
00280 /**********************************************************************/
00281 double BMA180::getTemperature()
00282 { 
00283   uint8_t Temperature;
00284 
00285   if( this->readReg( ADDRESS_TEMPERATURE, &Temperature ) == false )
00286   {
00287     ROS_ERROR("BMA180::getTemperature(): failed.");
00288     return -9999.9;
00289   }
00290   // convert raw temperature to actual temperature:
00291   Temperature_ = ( TempSlope_* (int8_t)Temperature ) + 24;   
00292   return Temperature_;
00293 }
00294 
00295 
00296 /**********************************************************************/
00297 
00298 /**********************************************************************/
00299 bool BMA180::softReset()
00300 {
00301 // not sure why the 5th argument has to be set in this way:
00302   uint8_t Request_SoftReset = (uint8_t)CMD_SOFTRESET;
00303  
00304   // write 0xB6 to 0x10
00305   if( this->writeToReg( ADDRESS_SOFTRESET, Request_SoftReset ) == false )
00306   {
00307     ROS_ERROR("BMA180::softReset(): write failed.");
00308     return false;
00309   }      
00310 
00311   // mandatory wait --- see datasheet  page 49
00312   usleep( 10 );
00313 
00314   return true;
00315 }
00316 
00317 
00318 /**********************************************************************/
00319 // perform a fine-calibration ---  see datahseet page 44.
00320 /**********************************************************************/
00321 bool BMA180::Calibrate()
00322 {
00323   uint8_t bitFlags[3] = { en_offset_x, en_offset_y, en_offset_z };
00324   uint8_t oldOffsets[3];
00325   uint8_t newOffsets[3];
00326  
00327   for( int i = 0; i < 3; i++ )
00328   {
00329     ROS_INFO( "BMA180::Calibrate():  BEGIN  Calibrating axis %d", i );
00330   
00331     // extract offsets before: 
00332     // read fine_offset_x
00333     if( i == 0 )
00334     {
00335       if( this->readReg( 0x29, &oldOffsets[i] ) == false )
00336         return false;
00337       oldOffsets[i] = (oldOffsets[i] >> 1) & 0x7F; // see datasheet page 44 for more details.
00338     }
00339   
00340     // read fine_offset_y
00341     if( i == 1 )
00342     {
00343       if( this->readReg( 0x26, &oldOffsets[i] ) == false)
00344         return false;
00345       oldOffsets[i] = (oldOffsets[i] >> 1) & 0x7F; // see datasheet page 44 for more details.
00346     }   
00347   
00348     // read fine_offset_z:  it's stored in two registers...
00349     if( i == 2 )
00350     {
00351       uint8_t temp_reg1;
00352       uint8_t temp_reg2;
00353    
00354       if( this->readReg( 0x25, &temp_reg1 ) == false )
00355         return false;
00356       if( this->readReg( 0x23, &temp_reg2 ) == false )
00357         return false; 
00358       oldOffsets[i] = ( (temp_reg1 << 3) | (0x07 & temp_reg2) ) & 0x7F; // see datasheet page 44 for more details.
00359     } 
00360   
00361     do
00362     {   
00363       // Set Fine Calibration settings:
00364       uint8_t local_reg;
00365       // read reg for a local copy:
00366       if( this->readReg( ADDRESS_CTRL_REG4, &local_reg ) == false )
00367         return false;
00368    
00369       // remove old reg settings:
00370       local_reg &= ~0x03; // Mask: b11111100 
00371       // write modified local_reg to the sensor and expect local_reg back from the sensor:
00372       if( this->writeToRegAndVerify( ADDRESS_CTRL_REG4, local_reg, local_reg ) == false )
00373       {
00374         ROS_ERROR( "BMA180::Calibrate(): failed." );
00375         return false;
00376       } 
00377       //ROS_INFO("BMA180::Calibrate(...): old calibration settings cleared.");
00378    
00379    
00380       // apply new settings:
00381       local_reg |= (0x01 << offset_finetuning); // apply fine-calibration flags --- see datasheet page 43.
00382    
00383       if( this->writeToRegAndVerify( ADDRESS_CTRL_REG4, local_reg, local_reg ) == false )
00384       {
00385         ROS_ERROR( "BMA180::Calibrate(): failed." );
00386         return false;
00387       } 
00388       //ROS_INFO("BMA180::Calibrate(...): fine-calibration accepted");
00389 
00390    
00391       // enable the offset bit. Calibration begins afterwards.
00392       if( this->setEnOffsetBit( bitFlags[i] ) == false )
00393       {
00394         ROS_ERROR( "BMA180::Calibrate(): could not set bitFlag to begin calibration." );
00395         return false;
00396       }
00397  
00398       // delay for two seconds while calibration takes place:
00399       sleep( 2 );  
00400   
00401       // extract offsets after: 
00402       // read fine_offset_x
00403       if( i == 0 )
00404       {
00405         if( this->readReg( 0x29, &newOffsets[i] ) == false )
00406           return false;
00407         newOffsets[i] = (newOffsets[i] >> 1) & 0x7F;
00408       }
00409   
00410       // read fine_offset_y
00411       if( i == 1 )
00412       {
00413         if( this->readReg( 0x29, &newOffsets[i] ) == false )
00414           return false;
00415         newOffsets[i] = (newOffsets[i] >> 1) & 0x7F;
00416       }   
00417   
00418       // read fine_offset_z:  it's stored in two registers...
00419       if( i == 2 )
00420       {
00421         uint8_t temp_reg1;
00422         uint8_t temp_reg2;
00423    
00424         if( this->readReg( 0x25, &temp_reg1 ) == false )
00425           return false;
00426         if( this->readReg( 0x23, &temp_reg2 ) == false )
00427           return false;
00428     
00429         newOffsets[i] = ((temp_reg1 << 3)|(0x07 & temp_reg2)) & 0x7F;   // See datasheet page 44 for more details. 
00430    
00431       }
00432   
00433       // Calibration is now complete on that axis.
00434       ROS_INFO( "Old Offset: %x  New Offset: %x", oldOffsets[i], newOffsets[i] );
00435     } while( (newOffsets[i] == 0x3f) || (newOffsets[i] == 0x40) );    // 0x3f and 0x40 indicate an error.
00436  
00437  
00438     ROS_INFO( "BMA180::Calibrate(): END Calibrating axis %d.", i );
00439   }
00440   ROS_INFO( "BMA180::Calibrate(): Calibration COMPLETE." );
00441  
00442   return true;
00443 }
00444 
00445 
00446 /**********************************************************************/
00447 // perform a Quick coarse-calibration --- see datahseet page 44.
00448 /**********************************************************************/
00449 bool BMA180::CoarseCalibrate()
00450 {
00451   uint8_t bitFlags[3] = { en_offset_x, en_offset_y, en_offset_z };
00452   int16_t oldOffsets[3];
00453   int16_t newOffsets[3];
00454  
00455   uint8_t local_offset_z;
00456   uint8_t local_offset_y;
00457   uint8_t local_offset_x;
00458   uint8_t local_offset_lsb1;
00459   uint8_t local_offset_lsb2;
00460  
00461   // get old coarse offsets:  they're stored in two registers...
00462   if( this->readReg( ADDRESS_OFFSET_Z, &local_offset_z ) == false )
00463     return false;
00464   if( this->readReg( ADDRESS_OFFSET_Y, &local_offset_y ) == false )
00465     return false;
00466   if( this->readReg( ADDRESS_OFFSET_X, &local_offset_x ) == false )
00467     return false;
00468   if( this->readReg( ADDRESS_OFFSET_LSB1, &local_offset_lsb1 ) == false )
00469     return false;
00470   if( this->readReg( ADDRESS_OFFSET_LSB2, &local_offset_lsb2 ) == false )
00471     return false;
00472 
00473   // convert from offset-binary format to two's complement: 
00474   oldOffsets[0] = int(( ((int16_t)( (local_offset_x ^= 0x80) << 8)) | ( local_offset_lsb1) ) >> 4);
00475   oldOffsets[1] = int(( ((int16_t)( (local_offset_y ^= 0x80) << 8)) | ( local_offset_lsb2 << 4) ) >> 4);
00476   oldOffsets[2] = int(( ((int16_t)( (local_offset_z ^= 0x80) << 8)) | ( local_offset_lsb2) ) >> 4);
00477  
00478 
00479   ROS_INFO( "Old Coarse Offsets:" );
00480   ROS_INFO( "X: %f", oldOffsets[0] * this->getSensitivity() );
00481   ROS_INFO( "Y: %f", oldOffsets[1] * this->getSensitivity() );
00482   ROS_INFO( "Z: %f", oldOffsets[2] * this->getSensitivity() );
00483 
00484 
00485   for( int i = 0; i < 3; i++ )
00486   {
00487     ROS_INFO( "BMA180::Calibrate():  BEGIN  Calibrating axis %d", i );
00488  
00489     // Set Coarse Calibration settings:
00490     uint8_t local_reg;
00491     // read reg for a local copy:
00492     if( this->readReg( ADDRESS_CTRL_REG4, &local_reg ) == false )
00493       return false;
00494    
00495     // remove old reg settings:
00496     local_reg &= ~0x03; // Mask: b11111100 
00497     // write modified local_reg to the sensor and expect local_reg back from the sensor:
00498     if( this->writeToRegAndVerify( ADDRESS_CTRL_REG4, local_reg, local_reg ) == false )
00499     {
00500       ROS_ERROR( "BMA180::Calibrate(): failed." );
00501       return false;
00502     } 
00503     //ROS_INFO("BMA180::Calibrate(...): old calibration settings cleared.");
00504    
00505     // apply new settings:
00506     local_reg |= 0x02; // apply coarse-calibration flags --- see datasheet page 43.
00507    
00508     if( this->writeToRegAndVerify( ADDRESS_CTRL_REG4, local_reg, local_reg ) == false )
00509     {
00510       ROS_ERROR( "BMA180::Calibrate(): failed." );
00511       return false;
00512     } 
00513     //ROS_INFO("BMA180::Calibrate(...): fine-calibration accepted");
00514    
00515     // Calibrate x and y axis normally:
00516     if( i != 2 ) 
00517     {// initialize the sensor's calibration routine on that axis:
00518       if( this->setEnOffsetBit( bitFlags[i] ) == false )
00519       {
00520         ROS_ERROR( "BMA180::Calibrate(): could not set bitFlag to begin calibration." );
00521         return false;
00522       }
00523       // delay for three seconds while calibration takes place:
00524       sleep( 2 );  
00525     }
00526  
00527     // Calibrate Z axis in slightly alternate manner.  Assume Z axis is in the 1 [g] position:
00528     if( i == 2 ) 
00529     {
00530       // trigger the Z-axis calibration:
00531       if( this->setEnOffsetBit( bitFlags[i] ) == false )
00532       {
00533         ROS_ERROR( "BMA180::Calibrate(): could not set bitFlag to begin calibration." );
00534         return false;
00535       }
00536       // delay for three seconds while calibration takes place:
00537       sleep( 2 );
00538     
00539       // If successful, the z-offset registers now will compensate for zero, when we want it to adjust for 1 [g].
00540       // To fix this, calibrate in a 1[g] environment and add the 1[g] equivalent to the calibrated value.
00541       // See datasheet pg 43.
00542     
00543       uint8_t new_offset_z;
00544       uint8_t new_offset_lsb2;
00545       // read offset values after calibration:
00546       if( this->readReg( ADDRESS_OFFSET_Z, &new_offset_z ) == false )
00547         return false;
00548       if( this->readReg( ADDRESS_OFFSET_LSB2, &new_offset_lsb2 ) == false )
00549         return false;
00550     
00551       // combine to raw Two's Complement shifted over by 4:
00552       uint16_t rawTC = ((uint16_t)new_offset_z << 8) |(uint16_t)new_offset_lsb2;
00553       rawTC ^= 0x8000;
00554       // add 1[g] equivalent (shifted over, since the other value is still shifted):
00555       int16_t one_g;
00556     
00557       switch( accel_range_ )
00558       {
00559       case RANGE_1: {}
00560       case RANGE_1_5: {}
00561       case RANGE_2: {}
00562       case RANGE_4:   
00563         one_g = (0x009A << 4); // a magic number nearby 8F, which is about 1[g] in +/- 2 [g] mode.  This value comes from 143 * 7[mg].  
00564         break;
00565       case RANGE_8: {}
00566       case RANGE_16:
00567         one_g = (0x008F << 4);
00568         break;
00569       default:  // should never happen
00570         one_g = (0x008F << 4);
00571       } 
00572       // add:
00573       int16_t sum_ob = (int16_t)rawTC + one_g;
00574       // convert back to offset-binary format:      
00575       sum_ob ^= 0x8000;
00576       // insert back into registers:
00577       new_offset_z = (uint8_t)(sum_ob >> 8);  
00578       new_offset_lsb2 = (uint8_t)(0x00FF & sum_ob);
00579     
00580       ROS_INFO( "offset_z: %x", new_offset_z );
00581       ROS_INFO( "offset_lsb2: %x", new_offset_lsb2 );
00582    
00583     
00584       if( this->writeToRegAndVerify( ADDRESS_OFFSET_Z, new_offset_z, new_offset_z ) == false )
00585         return false;
00586       if( this->writeToRegAndVerify( ADDRESS_OFFSET_LSB2, new_offset_lsb2, new_offset_lsb2 ) == false )
00587         return false;
00588     }
00589     ROS_INFO( "BMA180::Calibrate(): END     Calibrating axis %d \r\n", i );
00590   } 
00591  
00592   // extract offsets after: they're stored in two registers...
00593   if( this->readReg( ADDRESS_OFFSET_Z, &local_offset_z ) == false )
00594     return false;
00595   if( this->readReg( ADDRESS_OFFSET_Y, &local_offset_y ) == false )
00596     return false;
00597   if( this->readReg( ADDRESS_OFFSET_X, &local_offset_x ) == false )
00598     return false;
00599   if( this->readReg( ADDRESS_OFFSET_LSB1, &local_offset_lsb1 ) == false )
00600     return false;
00601   if( this->readReg( ADDRESS_OFFSET_LSB2, &local_offset_lsb2 ) == false )
00602     return false;
00603 
00604   // convert from offset-binary format to two's complement: 
00605   newOffsets[0] = int(( ((int16_t)( (local_offset_x ^= 0x80) << 8)) | ( local_offset_lsb1) ) >> 4);
00606   newOffsets[1] = int(( ((int16_t)( (local_offset_y ^= 0x80) << 8)) | ( local_offset_lsb2 << 4) ) >> 4);
00607   newOffsets[2] = int(( ((int16_t)( (local_offset_z ^= 0x80) << 8)) | ( local_offset_lsb2) ) >> 4);
00608  
00609   ROS_INFO( "New Coarse Offsets:" );
00610   ROS_INFO( "X: %f", newOffsets[0] * this->getSensitivity() );
00611   ROS_INFO( "Y: %f", newOffsets[1] * this->getSensitivity() );
00612   ROS_INFO( "Z: %f", newOffsets[2] * this->getSensitivity() );
00613 
00614   ROS_INFO( "BMA180::Calibrate(): COARSE CALIBRATION COMPLETE." );
00615  
00616   return true;
00617 }
00618 
00619 
00620 /**********************************************************************/
00621 // perform a two-step coarse-calibration --- see datahseet page 44.
00622 /**********************************************************************/
00623 bool BMA180::TwoStepCoarseCalibrate()
00624 {
00625   uint8_t bitFlags[3] = { en_offset_x, en_offset_y, en_offset_z };
00626   std::string axes[3] = { "X", "Y", "Z" };
00627   int16_t oldOffsets[3];
00628   int16_t newOffsets[3];
00629  
00630   uint8_t local_offset_z;
00631   uint8_t local_offset_y;
00632   uint8_t local_offset_x;
00633   uint8_t local_offset_lsb1;
00634   uint8_t local_offset_lsb2;
00635  
00636   // get old coarse offsets:  Each axis is stored in two registers.
00637   if( this->readReg( ADDRESS_OFFSET_Z, &local_offset_z ) == false )
00638     return false;
00639   if( this->readReg( ADDRESS_OFFSET_Y, &local_offset_y ) == false )
00640     return false;
00641   if( this->readReg( ADDRESS_OFFSET_X, &local_offset_x ) == false )
00642     return false;
00643   if( this->readReg( ADDRESS_OFFSET_LSB1, &local_offset_lsb1 ) == false )
00644     return false;
00645   if( this->readReg( ADDRESS_OFFSET_LSB2, &local_offset_lsb2 ) == false )
00646     return false;
00647 
00648   oldOffsets[0] = ((((local_offset_x << 8) | local_offset_lsb1) >> 4) & 0x0FFF);
00649   oldOffsets[1] = (((local_offset_y << 8) | (local_offset_lsb2 << 4) >> 4) & 0x0FFF);
00650   oldOffsets[2] = ((((local_offset_z << 8) | local_offset_lsb2) >> 4) & 0x0FFF);
00651  
00652   for( int i = 0; i < 3; i++ )
00653   {
00654     ROS_INFO( "BEGIN Calibrating axis %s.", axes[i].c_str() );
00655     // Set Coarse Calibration settings:
00656     uint8_t local_reg;
00657     // read reg for a local copy:
00658     if( this->readReg( ADDRESS_CTRL_REG4, &local_reg ) == false )
00659       return false;
00660    
00661     // remove old reg settings:
00662     local_reg &= ~0x03; // Mask: b11111100 
00663     // write modified local_reg to the sensor and expect local_reg back from the sensor:
00664     if( this->writeToRegAndVerify( ADDRESS_CTRL_REG4, local_reg, local_reg )  == false )
00665     {
00666       ROS_ERROR( "BMA180::TwoStepCoarseCalibrate(): failed." );
00667       return false;
00668     } 
00669    
00670     // apply new settings:
00671     local_reg |= 0x02; // apply coarse-calibration flags --- datasheet page 43
00672    
00673     if( this->writeToRegAndVerify( ADDRESS_CTRL_REG4, local_reg, local_reg )  == false )
00674     {
00675       ROS_ERROR( "BMA180::TwoStepCoarseCalibrate(): failed." );
00676       return false;
00677     } 
00678 
00679     // Calibrate x and y axis normally:
00680     if( i != 2 ) 
00681     {
00682       if( i == 0 )
00683       {
00684         // Query user:
00685         // Place the sensor on a still flat surface.
00686         std::cout << std::endl << "Place the sensor on a still, flat surface." << std::endl;
00687         std::cout << "Ready to calibrate x and y axes. Continue? [y/n]" << std::endl;
00688         std::string reply;
00689         std::getline( std::cin, reply );
00690         if( reply != "Y" && reply != "y" )
00691         {
00692           ROS_INFO( "Calibration cancelled by user." );
00693           return true;
00694         }
00695       }
00696     
00697       // initialize the sensor's calibration routine on that axis:
00698       if( this->setEnOffsetBit( bitFlags[i] ) == false )
00699       {
00700         ROS_ERROR( "BMA180::TwoStepCoarseCalibrate(): could not set bitFlag to begin calibration." );
00701         return false;
00702       }
00703       // delay for two seconds while calibration takes place:
00704       sleep( 2 );
00705     }
00706  
00707     // Calibrate Z axis in slightly alternate manner:
00708     if( i == 2 ) 
00709     {
00710       // Query user again:
00711       std::cout << std::endl << "Ready to calibrate z axis." << std::endl
00712                 << "Orient sensor such that z-axis is perpendicular to gravity." << std::endl
00713                 << "Do not move the sensor in this position." <<std::endl
00714                 << "Enter [y/n] to continue/cancel calibration." << std::endl;
00715       std::string reply;
00716       std::getline( std::cin, reply );
00717       if( reply != "Y" && reply != "y" )
00718       {
00719         ROS_INFO( "Calibration cancelled by user." );
00720         return false;
00721       }
00722  
00723       // trigger the Z-axis calibration:
00724       if( this->setEnOffsetBit( bitFlags[i] ) == false )
00725       {
00726         ROS_ERROR( "BMA180::TwoStepCoarseCalibrate(): could not set bitFlag to begin calibration." );
00727         return false;
00728       }
00729       // delay for two seconds while calibration takes place:
00730       sleep( 2 );
00731     }
00732    
00733     ROS_INFO( "BMA180::TwoStepCoarseCalibrate(): END     Calibrating axis %d.", i );
00734   } 
00735 
00736   // get new coarse offsets:  they're stored in two registers...
00737   if( this->readReg( ADDRESS_OFFSET_Z, &local_offset_z ) == false )
00738     return false;
00739   if( this->readReg( ADDRESS_OFFSET_Y, &local_offset_y ) == false )
00740     return false;
00741   if( this->readReg( ADDRESS_OFFSET_X, &local_offset_x ) == false )
00742     return false;
00743   if( this->readReg( ADDRESS_OFFSET_LSB1, &local_offset_lsb1 ) == false )
00744     return false;
00745   if( this->readReg( ADDRESS_OFFSET_LSB2, &local_offset_lsb2 ) == false )
00746     return false;
00747 
00748   newOffsets[0] = ((((local_offset_x << 8)|local_offset_lsb1) >> 4) & 0x0FFF);
00749   newOffsets[1] = (((local_offset_y << 8)|(local_offset_lsb2 << 4) >> 4) & 0x0FFF);
00750   newOffsets[2] = ((((local_offset_z << 8)|local_offset_lsb2) >> 4) & 0x0FFF);
00751  
00752 
00753   std::cout << "(in 12-bit offset-binary format)" << std::endl;
00754   std::cout << "Old Coarse Offsets   |   New Coarse Offsets " << std::endl;
00755   std::cout << "            " << std::hex << oldOffsets[0]
00756             << "            " << std::hex << newOffsets[0] << std::endl;
00757   std::cout << "            " << std::hex << oldOffsets[1]
00758             << "            " << std::hex << newOffsets[1] << std::endl;
00759   std::cout << "            " << std::hex << oldOffsets[2]
00760             << "            " << std::hex << newOffsets[2] << std::endl;
00761 
00762   ROS_INFO( "BMA180::Calibrate(): COARSE CALIBRATION COMPLETE." );
00763  
00764   return true;
00765 }
00766 
00767 
00768 /**********************************************************************/
00769 // perform a full calibration --- see datahseet page 44.
00770 /**********************************************************************/
00771 bool BMA180::FullCalibration()
00772 {
00773   uint8_t bitFlags[3] = { en_offset_x, en_offset_y, en_offset_z };
00774   std::string axes[3] = { "X", "Y", "Z" };
00775   // coarse calibration:
00776   int16_t oldOffsets[3];
00777   int16_t newOffsets[3];
00778   // fine calibration:
00779   uint8_t oldFineOffsets[3];
00780   uint8_t newFineOffsets[3];
00781  
00782   uint8_t local_offset_z;
00783   uint8_t local_offset_y;
00784   uint8_t local_offset_x;
00785   uint8_t local_offset_lsb1;
00786   uint8_t local_offset_lsb2;
00787  
00788   // get old coarse offsets:  Each axis is stored in two registers.
00789   if( this->readReg( ADDRESS_OFFSET_Z, &local_offset_z ) == false )
00790     return false;
00791   if( this->readReg( ADDRESS_OFFSET_Y, &local_offset_y ) == false )
00792     return false;
00793   if( this->readReg( ADDRESS_OFFSET_X, &local_offset_x ) == false )
00794     return false;
00795   if( this->readReg( ADDRESS_OFFSET_LSB1, &local_offset_lsb1 ) == false )
00796     return false;
00797   if( this->readReg( ADDRESS_OFFSET_LSB2, &local_offset_lsb2 ) == false )
00798     return false;
00799 
00800   oldOffsets[0] = ((((local_offset_x << 8)|local_offset_lsb1) >> 4) & 0x0FFF);
00801   oldOffsets[1] = (((local_offset_y << 8)|(local_offset_lsb2 << 4) >> 4) & 0x0FFF);
00802   oldOffsets[2] = ((((local_offset_z << 8)|local_offset_lsb2) >> 4) & 0x0FFF);
00803  
00804   // get old fine offsets: Each offset is stored in two registers:   // see datasheet page 44 for more details.... 
00805   if( this->readReg( 0x29, &oldFineOffsets[0] ) == false )
00806     return false;
00807   oldFineOffsets[0] = (oldFineOffsets[0] >> 1) & 0x7F;         
00808  
00809   if( this->readReg( 0x26, &oldFineOffsets[1] ) == false )
00810     return false;
00811   oldFineOffsets[1] = (oldFineOffsets[1] >> 1) & 0x7F;  
00812  
00813   uint8_t temp_reg1, temp_reg2;
00814   if( this->readReg( 0x25, &temp_reg1 ) == false )
00815     return false;
00816   if( this->readReg( 0x23, &temp_reg2 ) == false )
00817     return false;  
00818   oldFineOffsets[2] = ((temp_reg1 << 3)|(0x07 & temp_reg2)) & 0x7F;   
00819 
00820   for( int i = 0; i < 3; i++ )
00821   {
00822     ROS_INFO( "BEGIN Calibrating axis %s.", axes[i].c_str() );
00823   
00824     do
00825     {   
00826       // Set Calibration settings:
00827       uint8_t local_reg;
00828       // read reg for a local copy:
00829       if( this->readReg( ADDRESS_CTRL_REG4, &local_reg ) == false )
00830         return false;
00831     
00832       // remove old reg settings:
00833       local_reg &= ~0x03; // Mask: b11111100 
00834       // write modified local_reg to the sensor and expect local_reg back from the sensor:
00835       if( this->writeToRegAndVerify( ADDRESS_CTRL_REG4, local_reg, local_reg ) == false )
00836       {
00837         ROS_ERROR( "BMA180::Calibrate(): failed." );
00838         return false;
00839       }    
00840     
00841       // apply new settings:
00842       local_reg |= 0x03; // apply fine-calibration flags --- datasheet page 43.
00843     
00844       if( this->writeToRegAndVerify( ADDRESS_CTRL_REG4, local_reg, local_reg ) == false )
00845       {
00846         ROS_ERROR( "BMA180::Calibrate(): failed." );
00847         return false;
00848       } 
00849 
00850       // Calibrate x and y axis normally:
00851       if( i == 0 )
00852       {
00853         // Query user:
00854         // Place the sensor on a still flat surface.
00855         std::cout << std::endl << "Place the sensor on a still, flat surface." << std::endl
00856                   << "Ready to calibrate x and y axes. Continue? [y/n]" << std::endl;
00857         std::string reply;
00858         std::getline( std::cin, reply );
00859         if( reply != "Y" && reply != "y" )
00860         {
00861           ROS_INFO( "Full Calibration cancelled by user." );
00862           return false;
00863         }
00864       }
00865     
00866       // Calibrate Z axis in slightly alternate manner:
00867       if( i == 2 )
00868       {
00869         // Query user again:
00870         std::cout << std::endl << "Ready to calibrate z-axis." << std::endl
00871                   << "Orient sensor such that z-axis is perpendicular to gravity." << std::endl
00872                   << "Do not move the sensor in this position." << std::endl 
00873                   << "Enter [y/n] to continue/cancel calibration." << std::endl;
00874         std::string reply;
00875         std::getline( std::cin, reply );
00876         if ( reply != "Y" && reply != "y" )
00877         {
00878           ROS_INFO( "Full Calibration cancelled by user." );
00879           return false;
00880         }
00881       }
00882 
00883       // enable the offset bit. Calibration begins afterwards.
00884       if( this->setEnOffsetBit( bitFlags[i] ) == false )
00885       {
00886         ROS_ERROR( "BMA180::Calibrate(): could not set bitFlag to begin calibration." );
00887         return false;
00888       }
00889       // delay for two seconds while calibration takes place:
00890       sleep( 2 );
00891    
00892       // extract newFineoffsets after: 
00893       // read fine_offset_x
00894       if( i == 0 )
00895       {
00896         if( this->readReg( 0x29, &newFineOffsets[i] ) == false )
00897           return false;
00898         newFineOffsets[i] = (newFineOffsets[i] >> 1) & 0x7F;
00899       }
00900       // read fine_offset_y
00901       if( i == 1 )
00902       {
00903         if( this->readReg( 0x29, &newFineOffsets[i] ) == false )
00904           return false;
00905         newFineOffsets[i] = (newFineOffsets[i] >> 1) & 0x7F;
00906       }   
00907       // read fine_offset_z:  it's stored in two registers...
00908       if( i == 2 )
00909       {
00910         uint8_t temp_reg1, temp_reg2;
00911     
00912         if( this->readReg( 0x25, &temp_reg1 ) == false )
00913           return false;
00914         if( this->readReg( 0x23, &temp_reg2 ) == false )
00915           return false;
00916         newFineOffsets[i] = ((temp_reg1 << 3)|(0x07 & temp_reg2)) & 0x7F;   // see datasheet page 44 for more details.
00917       }
00918       // if fineoffset values remain within range,
00919       // calibration is now complete on that axis. 
00920       // Do..while().. will restart calibration on that axis otherwise.
00921       ROS_INFO( "NewFineOffsets: %x", newFineOffsets[i] );
00922    
00923     } while( (newFineOffsets[i] == 0x3f) || (newFineOffsets[i] == 0x40) );    // 0x3f and 0x40 indicate an error.
00924     
00925     ROS_INFO( "BMA180::FullCalibration(): END     Calibrating axis %d.", i );
00926   } 
00927 
00928   // get new coarse offsets:  they're stored in two registers...
00929   if( this->readReg( ADDRESS_OFFSET_Z, &local_offset_z ) == false )
00930     return false;
00931   if( this->readReg( ADDRESS_OFFSET_Y, &local_offset_y ) == false )
00932     return false;
00933   if( this->readReg( ADDRESS_OFFSET_X, &local_offset_x ) == false )
00934     return false;
00935   if( this->readReg( ADDRESS_OFFSET_LSB1, &local_offset_lsb1 ) == false )
00936     return false;
00937   if( this->readReg( ADDRESS_OFFSET_LSB2, &local_offset_lsb2 ) == false )
00938     return false;
00939 
00940   newOffsets[0] = ((((local_offset_x << 8)|local_offset_lsb1) >> 4) & 0x0FFF);
00941   newOffsets[1] = (((local_offset_y << 8)|(local_offset_lsb2 << 4) >> 4) & 0x0FFF);
00942   newOffsets[2] = ((((local_offset_z << 8)|local_offset_lsb2) >> 4) & 0x0FFF);
00943  
00944   std::cout << "(in 12-bit offset-binary format)" << std::endl;
00945   std::cout << "Old Coarse Offsets   |   New Coarse Offsets " << std::endl;
00946   std::cout << "X:          " << std::hex << oldOffsets[0]
00947             << "            " << std::hex << newOffsets[0] << std::endl;
00948   std::cout << "Y:          " << std::hex << oldOffsets[1]
00949             << "            " << std::hex << newOffsets[1] << std::endl;
00950   std::cout << "Z:          " << std::hex << oldOffsets[2]
00951             << "            " << std::hex << newOffsets[2] << std::endl;
00952 
00953   std::cout << "(in 7-bit offset-binary format)" << std::endl;
00954   std::cout << "Old Fine Offsets   |   New Fine Offsets " << std::endl;
00955   std::cout << "X:        " << std::hex << (int)oldFineOffsets[0]
00956             << "          " << std::hex << (int)newFineOffsets[0] << std::endl;
00957   std::cout << "Y:        " << std::hex << (int)oldFineOffsets[1]
00958             << "          " << std::hex << (int)newFineOffsets[1] << std::endl;
00959   std::cout << "Z:        " << std::hex << (int)oldFineOffsets[2]
00960             << "          " << std::hex << (int)newFineOffsets[2] << std::endl;
00961 
00962   ROS_INFO( "BMA180::FullCalibration(): FULL CALIBRATION COMPLETE." );
00963  
00964   return true;
00965 }
00966 
00967 
00968 /**********************************************************************/
00969 
00970 /**********************************************************************/
00971 bool BMA180::DisableI2C()
00972 {
00973   // set the LSB of ADDRESS_HIGH_DUR high.
00974   uint8_t ctrl_register = (uint8_t)CMD_DISABLE_I2C;
00975 
00976   switch( this->getProtocol() )
00977   {
00978   case SPI:
00979     if( this->writeToReg( ADDRESS_HIGH_DUR, ctrl_register ) == false )
00980     {
00981       ROS_ERROR( "BMA180::DisableI2C(): write failed." );
00982       return false;
00983     }
00984     return true;
00985   default:
00986     ROS_ERROR( "BMA180::DisableI2C() cannot disable i2c from this protocol." );
00987   }
00988   return false;
00989 }
00990 
00991 
00992 /**********************************************************************/
00993 
00994 /**********************************************************************/
00995 bool BMA180::EnableWriting()
00996 {
00997   uint8_t reg_value;
00998  
00999   // Read the register that we want to write to first for a local copy
01000   if( this->readReg( ADDRESS_CTRLREG0, &reg_value ) == false )
01001   {
01002     ROS_ERROR( "BMA180::EnableWriting(): read failed." );
01003     return false;
01004   }
01005   
01006   // #ifdef DEBUG
01007   ROS_INFO( "EnableWriting flag before: %d. Default: 0", (reg_value & (1 << ee_w) ) >> ee_w );
01008   // #endif  
01009  
01010   // add our command to enable writing:
01011   reg_value |= (1 << ee_w); // NOTE: for other cases where we write to bits, we must mask off the existing bits.
01012  
01013   // write the adjusted register value back to the sensor:
01014   if( this->writeToReg( ADDRESS_CTRLREG0, reg_value ) == false )
01015   {
01016     ROS_ERROR( "BMA180::EnableWriting(): write failed." );
01017     return false;
01018   }
01019  
01020   // read back that register to make sure that changes worked:
01021   if( this->readReg( ADDRESS_CTRLREG0, &reg_value ) == false )
01022   {
01023     ROS_ERROR( "BMA180::EnableWriting(): read failed." );
01024     return false;
01025   }
01026  
01027   // Compare register values to what we expect:
01028   uint8_t enable_actual = ( reg_value & (1 << ee_w) ) >> ee_w;
01029   uint8_t enable_expected = 1; 
01030  
01031   // #ifdef DEBUG
01032   ROS_INFO("EnableWriting flag after:  %d.  Expected: %d", enable_actual, enable_expected );
01033   // #endif 
01034  
01035   if( enable_expected != enable_actual )
01036   {
01037     ROS_ERROR( "BMA180::EnableWriting(): failed." );
01038     return false;
01039   }
01040  
01041   // if new settings are correct:
01042   return true;
01043 }
01044 
01045 
01046 /**********************************************************************/
01047 
01048 /**********************************************************************/
01049 bool BMA180::changeAccelRange()
01050 {
01051   uint8_t local_range;
01052  
01053   // read current accel range register value for a local copy.
01054   if( this->readReg( ADDRESS_OFFSET_LSB1, &local_range ) == false )
01055   {
01056     ROS_ERROR("BMA180::changeAccelRange(): read failed.");
01057     return false;
01058   }
01059  
01060   // #ifdef DEBUG
01061   ROS_INFO( "AccelRange bits before: %d.  Default:  %d", ( (local_range & (0x07 << range)) >> range), 2); // Defaults on page 27
01062   // #endif 
01063 
01064   // add our command to change range:
01065   local_range &= ~(0x07 << range); // clear old range value. Mask: b11110001
01066   local_range |= (accel_range_ << range); // insert new range value.
01067 
01068   // write the adjusted register value back to the sensor's register:
01069   if( this->writeToReg( ADDRESS_OFFSET_LSB1, local_range ) == false )
01070   {
01071     ROS_ERROR( "BMA180::changeAccelRange(): write failed." );
01072     return false;
01073   }
01074  
01075   // read back that register to make sure that changes worked:
01076   if( this->readReg( ADDRESS_OFFSET_LSB1, &local_range ) == false )
01077   {
01078     ROS_ERROR( "BMA180::changeAccelRange(): read failed." );
01079     return false;
01080   }
01081  
01082   // Compare register values to what we expect:
01083   uint8_t range_actual = ( local_range & (0x07 << range) ) >> range; // mask: b00001110
01084   uint8_t range_expected = (uint8_t) accel_range_; // This is the value set in the parameters. 
01085  
01086   // #ifdef DEBUG
01087   ROS_INFO( "AccelRange bits after:  %d.  Expected: %d", range_actual, range_expected );
01088   // #endif 
01089  
01090   if( range_expected != range_actual )
01091   {
01092     ROS_ERROR( "BMA180::changeAccelRange(): failed." );
01093     return false;
01094   }
01095  
01096   // if new settings are correct:
01097   return true;
01098 }
01099 
01100 
01101 /**********************************************************************/
01102 /**********************************************************************/
01103 bool BMA180::changeBandwidth()
01104 {
01105   uint8_t local_bw_reg;
01106   // read register with the current bandwidth flags for a local copy.
01107   if( this->readReg( ADDRESS_BW_TCS, &local_bw_reg ) == false )
01108   {
01109     ROS_ERROR( "BMA180::changeBandwidth(): read failed." );
01110     return false;
01111   }
01112  
01113   // #ifdef DEBUG
01114   ROS_INFO( "Bandwidth bits before: %d.  Default:  %d", ( (local_bw_reg & (0x0F << bw)) >> bw), 4 ); // defaults on page 27
01115   // #endif 
01116  
01117   // add our command to change range:
01118   local_bw_reg &= ~(0x0F << bw); // clear old value. Mask: b00001111
01119   local_bw_reg |= bandwidth_ << bw; // insert new value.
01120 
01121   // write the adjusted register value back to the sensor's register
01122   if( this->writeToReg( ADDRESS_BW_TCS, local_bw_reg ) == false )
01123   {
01124     ROS_ERROR( "BMA180::changeBandwidth(): write failed." );
01125     return false;
01126   }
01127  
01128   // read back that register to make sure that changes worked:
01129   if( this->readReg( ADDRESS_BW_TCS, &local_bw_reg ) == false )
01130   {
01131     ROS_ERROR( "BMA180::changeBandwidth(): read failed." );
01132     return false;
01133   }
01134  
01135   // Compare register values to what we expect:
01136   uint8_t bandwidth_actual = ( local_bw_reg & (0x0F << bw) ) >> bw; // mask: b11110000
01137   uint8_t bandwidth_expected = (uint8_t) bandwidth_; // This is the value set in the parameters. 
01138  
01139   // #ifdef DEBUG
01140   ROS_INFO("Bandwidth bits after:  %d.  Expected: %d", bandwidth_actual, bandwidth_expected);
01141   // #endif 
01142  
01143   if( bandwidth_expected != bandwidth_actual )
01144   {
01145     ROS_ERROR( "BMA180::changeBandwidth(): failed." );
01146     return false;
01147   }
01148  
01149   // if new settings are correct:
01150   return true;
01151 }
01152 
01153 
01154 /**********************************************************************/
01155 /**********************************************************************/
01156 bool BMA180::setEnOffsetBit( uint8_t bit )
01157 {
01158   uint8_t local_ctrl_reg1;
01159  
01160   // BEGIN ENABLE OFFSET on selected axis: 
01161   // read CTRL_REG1 register value for a local copy.
01162   if( this->readReg( ADDRESS_CTRLREG1, &local_ctrl_reg1 ) == false )
01163   {
01164     ROS_ERROR( "BMA180::setEnOffsetBit(): read failed." );
01165     ROS_INFO("en_offset bit before:  %d.  Default:  0", ((local_ctrl_reg1 >> bit) & 0x01) );
01166     return false;
01167   }
01168   // #ifdef DEBUG 
01169   //ROS_INFO("en_offset bit before:  %d.  Default:  0", ((local_ctrl_reg1 >> bit) & 0x01) );
01170   // #endif 
01171   // add our command to change range:
01172   local_ctrl_reg1 &= ~(1 << bit);   // clear old  value. Mask: b01111111
01173   local_ctrl_reg1 |= (1 << bit); // insert new  value.
01174  
01175   // write the adjusted register value back to the sensor's register:
01176   if( this->writeToReg( ADDRESS_CTRLREG1, local_ctrl_reg1 ) == false )
01177   {
01178     ROS_ERROR( "BMA180::setEnOffsetBit(): write failed." );
01179     return false;
01180   }
01181   
01182   // read back that register to make sure that changes worked:
01183   if( this->readReg( ADDRESS_CTRLREG1, &local_ctrl_reg1 ) == false )
01184   {
01185     ROS_ERROR( "BMA180::setEnOffsetBit(): read failed." );
01186     return false;
01187   }
01188  
01189   // Compare register values to what we expect:
01190   uint8_t actual = ( local_ctrl_reg1 >> bit ) & 0x01;     
01191   uint8_t expected = 1; // flag should be set to 1.
01192    
01193   // #ifdef DEBUG
01194   //ROS_INFO("en_offset bit after:   %d.  Expected: %d", actual, expected);
01195   // #endif
01196   
01197   if( expected != actual )
01198   {
01199     ROS_ERROR( "BMA180::setEnOffsetBit(): failed." );
01200     ROS_INFO( "en_offset bit after:   %d.  Expected: %d", actual, expected );
01201     return false;
01202   }
01203   // if new settings are correct:
01204   return true;
01205 }
01206 
01207 /**********************************************************************/
01208 // read a register and return its value. 
01209 /**********************************************************************/
01210 bool BMA180::readReg( uint8_t reg, uint8_t* value )
01211 {
01212   // Reading depends on the protocol.
01213   switch( this->getProtocol() )
01214   {
01215   case I2C:
01216     if( hardware_->read( this->getDeviceAddress(), I2C, this->getFrequency(), this->getFlags(), reg, value, 1 ) < 0 ) 
01217     {
01218       ROS_ERROR( "BMA180::readReg(): Error reading register via I2C!" );
01219       return false;
01220     } 
01221     break;
01222   case SPI:
01223     // we must prepend the SPI_READ_FLAG.
01224     if( hardware_->read( this->getDeviceAddress(), SPI, this->getFrequency(), this->getFlags(), ( 1 << SPI_READ_FLAG ) | reg, value, 1 ) < 0 ) 
01225     {
01226       ROS_ERROR( "BMA180::readReg(): Error reading register via SPI!" );
01227       return false;
01228     } 
01229     break;
01230   default:
01231     // shouldn't happen:
01232     ROS_ERROR( "BMA180::readReg(...): invalid protocol." );
01233     return false;
01234   }
01235   return true;
01236 }
01237 
01238 /**********************************************************************/
01239 // writes a byte to a register.
01240 /**********************************************************************/
01241 bool BMA180::writeToReg( uint8_t reg, uint8_t value )
01242 {
01243   // technically, writing depends on the protocol.
01244   switch( this->getProtocol() )
01245   {
01246   case I2C:
01247     if( hardware_->write( this->getDeviceAddress(), I2C, this->getFrequency(), this->getFlags(), (uint8_t)reg, (uint8_t*)&value, 1 ) < 0 )
01248     {
01249       ROS_ERROR( "BMA180::writeToReg(): Error writing to register via I2C!" );
01250       return false;
01251     } 
01252     break;
01253   case SPI:
01254     // we must prepend the SPI_WRITE_FLAG, although, technically it's already there, since it's zero.
01255     if( hardware_->write( this->getDeviceAddress(), SPI, this->getFrequency(), this->getFlags(), (uint8_t) (~(1 << SPI_WRITE_FLAG)&reg), (uint8_t*)&value, 1) < 0 ) 
01256     {
01257       ROS_ERROR( "BMA180::writeToReg(): Error writing to register via SPI!" );
01258       return false;
01259     } 
01260     break;
01261   default:
01262     ROS_ERROR("BMA180::writeToReg(...): invalid protocol.");
01263     return false;
01264   }
01265   return true;
01266 }
01267 
01268 
01269 /**********************************************************************/
01270 // writes a byte to a register.
01271 /**********************************************************************/
01272 bool BMA180::writeToRegAndVerify( uint8_t reg, uint8_t value, uint8_t expected )
01273 {
01274   uint8_t actual;
01275  
01276   if( this->writeToReg( reg, value ) == false )
01277     return false;
01278   // read it back to make sure it worked.
01279   if( this->readReg( reg, &actual ) == false )
01280     return false;
01281 
01282   if( expected != actual )
01283   {
01284     ROS_ERROR( "BMA180::writeToRegAndVerify(...): failed." );
01285     ROS_ERROR( "(in Hex) expected:  %x  actual: %x", expected, actual );
01286     return false;
01287   }
01288 
01289   return true;
01290 }
01291 
01292 
01293 bool BMA180::readSensorData( uint8_t reg, uint8_t* data, uint8_t num_bytes )
01294 {
01295   // Reading depends on the protocol.
01296   switch( this->getProtocol() )
01297   {
01298   case I2C:
01299     if( hardware_->read( this->getDeviceAddress(), I2C, this->getFrequency(), this->getFlags(), reg, data, num_bytes ) < 0 ) 
01300     {
01301       ROS_ERROR( "BMA180::readSensorData(): Error reading register via I2C!" );
01302       return false;
01303     } 
01304     break;
01305   case SPI:
01306     // we must prepend the SPI_READ_FLAG.
01307     if( hardware_->read( this->getDeviceAddress(), SPI, this->getFrequency(), this->getFlags(), ((1 << SPI_READ_FLAG)|reg), data, num_bytes ) < 0 ) 
01308     {
01309       ROS_ERROR( "BMA180::readSensorData(): Error reading register via SPI!" );
01310       return false;
01311     } 
01312     break;
01313   default:
01314     // shouldn't happen:
01315     ROS_ERROR( "BMA180::readSensorData(...): invalid protocol." );
01316     return false;
01317   }
01318   return true; 
01319 }


bma180_driver
Author(s): Joshua Vasquez, Philip Roan. Maintained by Philip Roan
autogenerated on Sat Dec 28 2013 16:49:14