bmc050.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 #include "bmc050_driver/bmc050.hpp"
00051 #include "bmc050_driver/bmc050_parameters.hpp"
00052 
00053 
00054 // Constructor
00055 BMC050::BMC050( bosch_hardware_interface* hw ) :
00056   sensor_driver( hw ),
00057   BMC050Parameters()
00058 {
00059   // Compass sensitivity is nominally 0.3 uT/LSB at 20 deg C
00060   // The compass measurements are not calibrated or temperature compensated
00061   SensitivityXY_ = 0.3; // [uT/LSB]
00062   SensitivityZ_ = 0.3;  // [uT/LSB]
00063   TempSlope_ = 0.5;
00064  
00065   prev_bw_reg_ = BW_1000HZ; // sensor default value.
00066 }
00067 
00068 // Destructor
00069 BMC050::~BMC050() 
00070 {
00071 }
00072 
00073 
00074 /**********************************************************************/
00075 // Initialize sensor and hardware on requested parameters:
00076 /**********************************************************************/
00077 bool BMC050::initialize()
00078 {  
00079   // Initialize the hardware interface with the selected parameters.
00080   if( hardware_->initialize() == false )
00081   {
00082     ROS_ERROR("bmc050::initialize(): Could not initialize a hardware interface!");
00083     return false;
00084   }
00085 
00086   this->softResetCompass(); // Compass remains in suspend mode if softReset is applied and it is already in suspend mode. pg 73
00087   this->softResetAccel();
00088   // Compass sensor starts in suspend_ mode;
00089   // Accelerometer sensor starts in normal_ mode.
00090   
00091   usleep( 3000 ); // manditory wait for sensor startup
00092 
00093   // copy existing values of Compass registers into the class values:
00094   // (so we don't override existing values when we change modes by setting flags)
00095   if( this->readRegistersAccel() == false )
00096     return false;
00097   if( this->readRegistersCompass() == false )    
00098     return false;
00099  
00100   // Enter Normal mode for accessing sensor data. 
00101   if( this->enterNormalModeAccel() == false )
00102     return false;
00103   if( this->enterNormalModeCompass() == false )
00104     return false;
00105   this->readRegistersAccel();
00106   // Normal mode:  Accel reg 0x11 should have val 0x00
00107   this->readRegistersCompass();
00108   // Normal mode:  Compass reg 0x4C should have val 0x00
00109  
00110 
00111   // change accel_range to the range set in the parameters class:   
00112   ROS_INFO("bmc050::initialize(): adjusting Accelerometer range.");
00113   if( this->changeAccelRange() == false )
00114     return false;
00115   ROS_INFO("bmc050::initialize(): Accelerometer range adjusted.");
00116   // change compass_rate to the rate set in the parameters class: 
00117   ROS_INFO("bmc050::initialize(): adjusting Compass output data rate.");
00118   if( this->changeCompassRate() == false )
00119     return false;
00120   ROS_INFO("bmc050::initialize(): Compass rate adjusted.");
00121   // filter data at requested bandwidth, if requested in parameters:
00122   ROS_INFO("bmc050::initialize(): Applying Filter parameter.");
00123   if( this->filterData(accel_is_filtered_) == false )
00124     return false;
00125   ROS_INFO("bmc050::initialize(): Filter parameter applied.");
00126   // change accelerometer bandwidth to the bandwidth set in the parameters:
00127   ROS_INFO("bmc050::initialize(): Adjusting Accelerometer Bandwidth.");
00128   if( this->changeAccelBandwidth() == false )
00129     return false;
00130   ROS_INFO("bmc050::initialize(): Bandwidth adjusted.");
00131   // change compass repetitions register to reflect # reps set in parameter:
00132   ROS_INFO("bmc050::initialize(): Adjusting Compass X,Y,Z number of samples.");
00133   if( this->changeNumRepetitionsXY() == false )
00134     return false;
00135   if( this->changeNumRepetitionsZ() == false )
00136     return false;
00137   if( this->readRegistersAccel() == false )
00138     return false;
00139   if( this->readRegistersCompass() == false )
00140     return false;
00141  
00142   return true;
00143 }
00144 
00145 
00146 
00147 /**********************************************************************/
00148 
00149 /**********************************************************************/
00150 bool BMC050::softResetAccel()
00151 {
00152   // Write 0xB6 to Accel Reg 0x14:
00153   if( this->writeToAccelReg( SOFTRESET_ACCEL, SOFTRESET_ACCEL_CMD ) == false )
00154   {
00155     ROS_ERROR("bmc050::softResetAccel(): failed.");
00156     return false;
00157   }
00158  
00159   usleep( 2000 ); // mandatory delay --- see datasheet page 8.
00160   accel_mode_ = accel_normal_; 
00161  
00162   // clear accel bandwidth settings to sensor default:
00163   prev_bw_reg_ = BW_1000HZ;
00164  
00165   return true;
00166 }
00167 
00168 
00169 bool BMC050::softResetCompass()
00170 {
00171   if( this->writeToAccelReg( SOFTRESET_COMPASS_REG, SOFTRESET_COMPASS_CMD ) == false )
00172   {
00173     ROS_ERROR("bmc050::softResetCompass(): failed.");
00174     return false;
00175   } 
00176 
00177   usleep( 1000 ); // mandatory delay --- see datasheet page 10.
00178 
00179   // compass starts in suspend mode;
00180   compass_mode_ = compass_suspend_;
00181  
00182   return true;
00183 }
00184 
00185 /**********************************************************************/
00186 /* INPUT: none
00187  *OUTPUT: boolean indicating success.
00188  *  This method changes the sensor's output data rate to the data rate
00189  *  specified in the parameters by writing the specified value to the 
00190  *  correct register.
00191  */
00192 /**********************************************************************/
00193 bool BMC050::changeCompassRate()
00194 {
00195   uint8_t local_rate;
00196 
00197   // Read register with accel range settings for a local copy:
00198   if( this->readRegCompass(RATE_COMPASS, &local_rate ) == false )
00199     return false;
00200   
00201   // #ifdef DEBUG                            
00202   ROS_INFO("Compass rate bits before: %d.  Default: %d", ((local_rate >> Data_Rate) & 0x07), 0); // Defaults from page 50
00203   // #endif 
00204   
00205   // add our command to change range:
00206   local_rate &= 0xC7; // clear old range value. Mask: b11000111
00207  
00208   // set compass rate value to write:
00209   switch( compass_rate_ )
00210   {
00211   case ODR_10HZ:
00212     local_rate |= (ODR_10HZ << Data_Rate); // (ODR_10HZ << 3) | 11000111b
00213     break;
00214   case ODR_2HZ: 
00215     local_rate |= (ODR_2HZ << Data_Rate);
00216     break;
00217   case ODR_6HZ: 
00218     local_rate |= (ODR_6HZ << Data_Rate);
00219     break;
00220   case ODR_8HZ: 
00221     local_rate |= (ODR_8HZ << Data_Rate);
00222     break;
00223   case ODR_15HZ:
00224     local_rate |= (ODR_15HZ << Data_Rate);
00225     break;
00226   case ODR_20HZ:
00227     local_rate |= (ODR_20HZ << Data_Rate);
00228     break;
00229   case ODR_25HZ:
00230     local_rate |= (ODR_25HZ << Data_Rate);
00231     break;
00232   case ODR_30HZ:
00233     local_rate |= (ODR_30HZ << Data_Rate);
00234     break;
00235   default:
00236     // should never happen because of the enums
00237     ROS_ERROR("bmc050::changeCompassRate(): invalid compass rate.");
00238     return false;
00239   }
00240  
00241  
00242   // write the adjusted register value back to the sensor's register:
00243   if( this->writeToCompassReg(RATE_COMPASS, local_rate ) == false )
00244     return false;
00245   
00246   // read back that register to make sure that changes worked:
00247   if( this->readRegCompass(RATE_COMPASS, &local_rate ) == false )
00248     return false;
00249   
00250   // Compare register values to what we expect:
00251   uint8_t expected_reg = compass_rate_ ;
00252   uint8_t actual_reg = (local_rate >> Data_Rate) & 0x07;
00253   
00254   if( expected_reg != actual_reg )
00255   {
00256     ROS_ERROR("Compass rate bits after:  %d.  Expected: %d", actual_reg, expected_reg);
00257     return false;
00258   }
00259   else
00260   {
00261     // #ifdef DEBUG                            
00262     ROS_INFO("Compass rate bits after:  %d.  Expected: %d", actual_reg, expected_reg);
00263     // #endif
00264   }
00265   // if everything works:
00266   return true;
00267 }
00268 
00269 
00270 /**********************************************************************/
00271 /* INPUT: none
00272  *OUTPUT: boolean indicating success.
00273  *  Because we MUST write an entire register to the accelerometer when
00274  *  setting bitflags, this function saves a copy of those registers 
00275  *  so that we write our modified copy of that register rather than
00276  *  overwriting the sensor's entire register just to set a single bit.
00277  */
00278 /**********************************************************************/
00279 bool BMC050::readRegistersAccel()
00280 {
00281   // reading depends on the protocol:
00282   uint8_t mode_change_accel_reg;
00283   uint8_t set_filter_reg;
00284 
00285   switch( protocol_ )
00286   {
00287   case I2C:
00288     // normal registers
00289     mode_change_accel_reg  = MODECHANGE_ACCEL;
00290     set_filter_reg = SET_FILTER;
00291     break;
00292   case SPI:
00293     // registers with the FLAG_R inserted
00294     mode_change_accel_reg  = FLAG_R | MODECHANGE_ACCEL;
00295     set_filter_reg = FLAG_R | SET_FILTER;
00296     break;
00297   default:
00298     ROS_ERROR("bmc050::readRegistersAccel(): invalid protocol.");
00299     return false;
00300   }
00301   // copy MODECHANGE_ACCEL located in accelerometer registers:
00302   if( hardware_->read( this->getAccelAddress(), this->getProtocol(), this->getFrequency(), this->getFlags(), mode_change_accel_reg, &modechange_accel_, 1 ) < 0 )
00303   {
00304     ROS_ERROR("bmc050::readRegistersAccel(): MODECHANGE_ACCEL read failed.");
00305     return false;
00306   } 
00307 
00308   if( hardware_->read( this->getAccelAddress(), this->getProtocol(), this->getFrequency(), this->getFlags(), set_filter_reg, &set_filter_, 1 ) < 0 )
00309   {
00310     ROS_ERROR("bmc050::readRegistersAccel(): SET_FILTER read failed.");
00311     return false;
00312   } 
00313 
00314   //ROS_INFO("ACC: 0x11.   Val 0x%x",modechange_accel_);
00315   //ROS_INFO("ACC: 0x13.   Val 0x%x",set_filter_);
00316 
00317   return true;
00318 }
00319 
00320 
00321 /**********************************************************************/
00322 
00323 /**********************************************************************/
00324 bool BMC050::readRegistersCompass()
00325 {
00326   // Read RATE_COMPASS and SOFTRESET registers into our copies
00327   // (which are modechange_accel_, modechange_, and softreset_)
00328   // so that we can set bits by writing back the whole register.
00329 
00330   // reading depends on the protocol:
00331   uint8_t mode_reg;
00332   uint8_t reset_reg;
00333 
00334   switch( protocol_ )
00335   {
00336   case I2C:
00337     // normal registers
00338     mode_reg  = RATE_COMPASS;
00339     reset_reg = SOFTRESET_COMPASS_REG;
00340     break;
00341   case SPI:
00342     // registers with the FLAG_R inserted
00343     mode_reg  = FLAG_R | RATE_COMPASS;
00344     reset_reg = FLAG_R | SOFTRESET_COMPASS_REG;
00345     break;
00346   default:
00347     ROS_ERROR("bmc050::readRegistersCompass(): invalid protocol.");
00348     return false;
00349   }
00350    
00351   // copy RATE_COMPASS located in compass registers:
00352   if( hardware_->read( this->getCompassAddress(), this->getProtocol(), this->getFrequency(), this->getFlags(), mode_reg, &rate_compass_, 1 ) < 0 )
00353   {
00354     ROS_ERROR("bmc050::readRegistersCompass(): RATE_COMPASS read failed.");
00355     return false;
00356   }
00357 
00358   // copy SOFTRESET located in compass registers:
00359   if( hardware_->read( this->getCompassAddress(), this->getProtocol(), this->getFrequency(), this->getFlags(), reset_reg, &softreset_, 1 ) < 0 )
00360   {
00361     ROS_ERROR("bmc050::readRegistersCompass(): SOFTRESET read failed.");
00362     return false;
00363   }
00364   //ROS_INFO("COMPASS: 0x4C.   Val 0x%x",rate_compass_);
00365   //ROS_INFO("COMPASS: 0x4B.   Val 0x%x",softreset_);
00366   return true;
00367 }
00368 
00369 
00370 /**********************************************************************/
00371 /*INPUT: none directly. (indirectly) accel_range_, which was set 
00372  *     in the parameters.
00373  *OUTPUT: bool       -- whether or not it was successful.
00374  * This method performs two tasks:
00375  *   1. changes the accel_range to the accel_range set in the parameters.
00376  *   2. changes the class's accel_sensitivity, so that the output
00377  *    values will be multiplied correctly to produce correct values. 
00378  */
00379 /**********************************************************************/
00380 bool BMC050::changeAccelRange()
00381 {
00382   // first change sensitivity based on current range: 
00383   switch( this->getAccelRange() )
00384   {
00385   case RANGE_2: // ±2 [g]
00386     accel_sensitivity_ =  0.00391; // units: [g/LSB]
00387     break;
00388   case RANGE_4: // ±4 [g]
00389     accel_sensitivity_ = 0.00781;  // units: [g/LSB]
00390     break;
00391   case RANGE_8: // ±8 [g]
00392     accel_sensitivity_ = 0.01562; // units: [g/LSB]
00393     break;
00394   case RANGE_16: // ±16 [g]
00395     accel_sensitivity_ = 0.03125;  // units: [g/LSB]
00396     break;
00397   default:
00398     ROS_ERROR("BMC050::changeAccelRange(): invalid accel range.");
00399     return false;
00400   }
00401   
00402   uint8_t local_range;
00403   // Read register with accel range settings for a local copy:
00404   if( this->readRegAccel( ADDRESS_ACCEL_RANGE, &local_range ) == false )
00405     return false;
00406   
00407   ROS_INFO("AccelRange bits before: %d.  Default: %d", (local_range & (0x0F)), 3); // Defaults on page 50
00408   
00409   // add our command to change range:
00410   local_range &= ~( 0x0F ); // clear old range value. Mask: b11110000
00411   local_range |= accel_range_; // insert new range value.
00412  
00413   // write the adjusted register value back to the sensor's register:
00414   if( this->writeToAccelReg( ADDRESS_ACCEL_RANGE, local_range ) == false )
00415     return false;
00416   
00417   // read back that register to make sure that changes worked:
00418   if( this->readRegAccel( ADDRESS_ACCEL_RANGE, &local_range ) == false)
00419     return false;
00420   
00421   // Compare register values to what we expect:
00422   uint8_t expected_reg = accel_range_;
00423   uint8_t actual_reg = local_range;
00424  
00425   // #ifdef DEBUG                       
00426   ROS_INFO("AccelRange bits after:  %d.  Expected: %d", actual_reg, expected_reg);
00427   // #endif
00428   
00429   if( expected_reg != actual_reg )
00430     return false;
00431   
00432   return true;
00433 }
00434 
00435 
00436 /**********************************************************************/
00437 /*INPUT:  bool request  -- whether or not we want to filter data
00438  *OUTPUT: bool     -- whether or not changing the filter setting 
00439  *             worked.
00440  * This method performs two tasks:
00441  *  1. writes the user-defined bandwidth to sensor registers, if it 
00442  *   differs from the default.
00443  *  2. writes 1 or 0 to the DATA_HIGH_BW bit (without changing any other
00444  *   bits) to the appropriate SET_FILTER register: 0x13. 
00445  */
00446 /**********************************************************************/
00447 bool BMC050::filterData( bool request )
00448 {
00449   uint8_t local_bandwidth;
00450   // Read register with accel range settings for a local copy:
00451   if( this->readRegAccel( SET_FILTER, &local_bandwidth ) == false )
00452     return false;
00453   
00454   // #ifdef DEBUG                            
00455   ROS_INFO("Accelerometer bandwidth bits before: %d.  Default: %d",  ((local_bandwidth >> DATA_HIGH_BW) & 0x01), 0); // Defaults on page 50.
00456   // #endif 
00457 
00458   // add our command to apply filter: (see datasheet page 21)
00459   if( request == true )
00460     local_bandwidth &= ~( 1 << DATA_HIGH_BW ); // filtered: write 0
00461   else
00462     local_bandwidth |= ( 1 << DATA_HIGH_BW );  // unfiltered: write 1
00463   
00464   // write the adjusted register value back to the sensor's register:
00465   if( this->writeToAccelReg( SET_FILTER, local_bandwidth ) == false )
00466     return false;
00467   
00468   // read back that register to make sure that changes worked:
00469   if( this->readRegAccel( SET_FILTER, &local_bandwidth ) == false )
00470     return false;
00471   
00472   // Compare register values to what we expect:
00473   uint8_t expected_reg = !accel_is_filtered_;
00474   uint8_t actual_reg = ( (local_bandwidth >> DATA_HIGH_BW) & 0x01 );
00475  
00476   // #ifdef DEBUG                            
00477   ROS_INFO("Accelerometer bandwidth bits after:  %d.  Expected: %d", actual_reg, expected_reg);
00478   // #endif
00479   
00480   if( expected_reg != actual_reg )
00481     return false;
00482   
00483   return true;
00484 }
00485 
00486 
00487 /**********************************************************************/
00488 /**********************************************************************/
00489 bool BMC050::changeAccelBandwidth()
00490 {
00491   uint8_t local_bw_reg;
00492   // read reg for a local copy:
00493   if( this->readRegAccel( 0x10, &local_bw_reg ) == false )
00494     return false;
00495  
00496   ROS_INFO("Accelerometer bandwidth setting before: %d.  Default:  %d", (local_bw_reg & 0x1F), 0x1F); // Defaults on page 50.
00497  
00498   // apply new bandwidth value:
00499   local_bw_reg &= ~( 0x1F ); //Mask: 1110000
00500   local_bw_reg |= bw_reg_;
00501   // write back reg with new bandwidth value:
00502   if( this->writeToAccelReg( 0x10, local_bw_reg ) == false )
00503     return false;
00504   // verify that we wrote back the correct value:
00505   uint8_t actual_reg;
00506   if( this->readRegAccel( 0x10, &actual_reg) == false )
00507     return false;
00508  
00509   uint8_t actual_bw = actual_reg & 0x1F; // mask off unrelated bits.
00510   uint8_t expected_bw = bw_reg_;
00511  
00512   if( expected_bw != actual_bw )
00513   {
00514     ROS_ERROR("Accelerometer bandwidth setting after: %d.  Expected: %d", actual_bw, bw_reg_);
00515     ROS_ERROR("BMC050::changeAccelBandwidth(): failed.");
00516     return false;
00517   }
00518   else
00519     ROS_INFO("Accelerometer bandwidth setting after: %d.  Expected: %d", actual_bw, bw_reg_);
00520   return true;
00521 }
00522 
00523 
00524 /**********************************************************************/
00525 /**********************************************************************/
00526 bool BMC050::changeNumRepetitionsXY()
00527 {
00528   uint8_t reg_before;
00529  
00530   this->readRegCompass( ADDRESS_COMPASS_REPXY, &reg_before );
00531   ROS_INFO("Compass REPXY before: %d  Default: %d  ", reg_before, 0x00 ); // on datasheet page 69
00532 
00533   // write class-defined value to register:
00534   this->writeToCompassReg( ADDRESS_COMPASS_REPXY, repsXY_ );
00535  
00536   uint8_t actual;
00537  
00538   this->readRegCompass( ADDRESS_COMPASS_REPXY, &actual );
00539   // verify that repsXY_ was written
00540   if( repsXY_ != actual )
00541   {
00542     ROS_ERROR("Compass REPXY after: %d  Expected: %d ", actual, repsXY_); 
00543     return false;
00544   }
00545   // if it all works: 
00546   ROS_INFO("Compass REPXY after: %d  Expected: %d ", actual, repsXY_); 
00547   return true;
00548 }
00549 
00550 
00551 /**********************************************************************/
00552 /**********************************************************************/
00553 bool BMC050::changeNumRepetitionsZ()
00554 {
00555   uint8_t reg_before;
00556  
00557   this->readRegCompass( ADDRESS_COMPASS_REPZ, &reg_before );
00558   ROS_INFO( "Compass REPZ before: %d  Default: %d", reg_before, 0x00 ); // on datasheet page 69 
00559 
00560   // write class-defined value to register:
00561   this->writeToCompassReg( ADDRESS_COMPASS_REPZ, repsZ_ );
00562  
00563   uint8_t actual;
00564  
00565   this->readRegCompass( ADDRESS_COMPASS_REPZ, &actual );
00566   // verify that repsXY_ was written
00567   if( repsZ_ != actual )
00568   {
00569     ROS_ERROR("Compass REPZ after: %d  Expected: %d ", actual, repsZ_); 
00570     return false;
00571   }
00572 
00573   ROS_INFO("Compass REPZ after: %d  Expected: %d", actual, repsZ_); 
00574   return true;
00575 }
00576 
00577   
00578 /**********************************************************************/
00579 /**********************************************************************/
00580 void BMC050::simpleCalibrationAccel()
00581 {
00582   std::string axes[3] = { "X", "Y", "Z" };
00583   uint8_t offset_target_val[3] = { 0, 0, 1 }; // These bits go into register 0x37 to define the current xyz-alignment of the sensor with gravity (in [g]s)
00584   uint8_t offset_target[3] = { offset_target_x, offset_target_y, offset_target_z };
00585   uint8_t axis_bit[3] = { hp_x_en, hp_y_en, hp_z_en };
00586   uint8_t local_offset_reg;
00587   uint8_t local_calibration_status;               
00588  
00589   // Apply initial setup parameters:
00590  
00591   // Print initial register settings (filtered and unfiltered):
00592   this->printOffsets();
00593 
00594   // Query User
00595   std::cout << "Place the sensor flat and upright on a level surface." << std::endl
00596             << "Warning: Do not move the sensor during calibration." << std::endl
00597             <<"Press [y/n] to continue/cancel." << std::endl;
00598   std::string reply;
00599   std::getline( std::cin, reply );
00600   if ((reply != "Y") && (reply != "y"))
00601   {
00602     ROS_INFO("Calibration cancelled by user.");
00603     return;
00604   }  
00605  
00606   // Begin calibration on each axis sequentially:
00607   std::cout << "Beginning Simple Calibration routine:" << std::endl << std::endl;
00608   for (int i=0;i<3;i++)
00609   {
00610     std::cout << "Beginning Axis: " << axes[i] << std::endl;
00611     // First Write offset_target value to that axis to define its orientation:
00612     if( this->readRegAccel( ADDRESS_OFFSET_TARGET, &local_offset_reg ) == false )
00613     {
00614       std::cout<< "Error occured" << std::endl;
00615       return;
00616     }
00617     local_offset_reg &= (0x03 << offset_target[i]); // mask off old bitflags;
00618     local_offset_reg |= (offset_target_val[i] << offset_target[i]); // insert new bitflags;
00619   
00620     if( this->writeToAccelReg( ADDRESS_OFFSET_TARGET, local_offset_reg ) == false )
00621     {
00622       std::cout<< "Error occured" << std::endl;
00623       return;
00624     }
00625   
00626     // Next Trigger Calibration for that axis:
00627     if( this->readRegAccel( ADDRESS_ENABLE_CALIBRATION, &local_calibration_status ) == false )
00628     {
00629       std::cout<< "Error occured" << std::endl;
00630       return;
00631     }
00632     local_calibration_status &= ~0x07; // mask off current calibration-enable flags;
00633     local_calibration_status |= (1 << axis_bit[i]);
00634   
00635     if( this->writeToAccelReg( ADDRESS_ENABLE_CALIBRATION, local_calibration_status ) == false )
00636     {
00637       std::cout<< "Error occured" << std::endl;
00638       return;
00639     }
00640     std::cout << "Calibrating..." << std::endl;
00641   
00642     sleep( 3 ); // during this time, the sensor is updating the offset value every 8 samples (at 2 [kHz]) and adjusting the offset register in small increments.
00643     std::cout << "Done Calibrating Axis: " << axes[i] << std::endl << std::endl;
00644   
00645   }
00646   std::cout << "ACCELEROMETER CALIBRATION COMPLETE" << std::endl;
00647   // ask about permanently writing values to EEPROM:
00648  
00649   // Print final register settings (filtered and unfiltered):
00650   this->printOffsets(); 
00651 }
00652 
00653 
00654 /**********************************************************************/
00655 // read both sensors separately
00656 /**********************************************************************/
00657 bool BMC050::takeMeasurement()
00658 {
00659   // read the accel data and compass data and update their values:
00660  
00661   // We must perform two reads, since they are separate devices:
00662   this->getAccelData();
00663   this->getCompassData();
00664   return true;
00665 }
00666 
00667 
00668 /**********************************************************************/
00669 
00670 /**********************************************************************/
00671 bool BMC050::getAccelData()
00672 {
00673   // gets raw data and updates 4 values: AccelX_, AccelY_, AccelZ_, Temperature_;
00674   uint8_t Data[7];
00675   uint8_t address;
00676   // The sensor treats reads differently depending on the protocol: 
00677   switch( this->getProtocol() )
00678   {
00679   case I2C:
00680     address = ADDRESS_ACCLXYZ;
00681     break;
00682   case SPI:
00683     address = (FLAG_R | ADDRESS_ACCLXYZ);
00684     break;
00685   default:
00686     ROS_ERROR("bmc050::getAccelData(): cannot read from this protocol.");
00687     return false;
00688   }
00689   
00690   if( hardware_->read( this->getAccelAddress(), this->getProtocol(), this->getFrequency(), this->getFlags(), address, Data, 7 ) < 0 )
00691   {
00692     ROS_ERROR("bmc050::getAccelData(): Error reading from hardware interface!");
00693     return false;
00694   }
00695   
00696   AccelX_ = this->getAccelSensitivity() * int( ( ((int16_t)(Data[1] << 8)) | ( ((int16_t)Data[0]) ) ) >> 6 );
00697   AccelY_ = this->getAccelSensitivity() * int( ( ((int16_t)(Data[3] << 8)) | ( ((int16_t)Data[2]) ) ) >> 6 );
00698   AccelZ_ = this->getAccelSensitivity() * int( ( ((int16_t)(Data[5] << 8)) | ( ((int16_t)Data[4]) ) ) >> 6 );
00699  
00700   Temperature_ = ( TempSlope_ *(int8_t)Data[6] ) + 24;       
00701        
00702   return true;
00703 }
00704 
00705 
00706 double BMC050::getAccelX()
00707 {
00708   // gets raw data and updates : AccelX_
00709   uint8_t Data[2];
00710   uint8_t address;
00711   // The sensor treats reads differently depending on the protocol: 
00712   switch( this->getProtocol() )
00713   {
00714   case I2C:
00715     address = ADDRESS_ACCLXYZ;
00716     break;
00717   case SPI:
00718     address = (FLAG_R | ADDRESS_ACCLXYZ);
00719     break;
00720   default:
00721     ROS_ERROR("BMC050::getAccelX(): cannot read from this protocol.");
00722     return false;
00723   }
00724   // perform the read:  
00725   if( hardware_->read( this->getAccelAddress(), this->getProtocol(), this->getFrequency(), this->getFlags(), address, Data, 2 ) < 0 )
00726   {
00727     ROS_ERROR("BMC050::getAccelX(): Error reading from I2C interface!");
00728     return false;
00729   }    
00730               
00731   AccelX_ = this->getAccelSensitivity() * int( ( ((int16_t)(Data[1] << 8)) | ( ((int16_t)Data[0]) ) ) >> 6 );
00732   return AccelX_;
00733 }
00734  
00735  
00736 /**********************************************************************/
00737 
00738 /**********************************************************************/
00739 double BMC050::getAccelY()
00740 {
00741   // gets raw data and updates : AccelY_
00742   uint8_t Data[2];
00743   uint8_t address;
00744   // The sensor treats reads differently depending on the protocol: 
00745   switch( this->getProtocol() )
00746   {
00747   case I2C:
00748     address = ADDRESS_ACC_Y_LSB;
00749     break;
00750   case SPI:
00751     address = (FLAG_R | ADDRESS_ACC_Y_LSB);
00752     break;
00753   default:
00754     ROS_ERROR("BMC050::getAccelY(): cannot read from this protocol.");
00755     return false;
00756   }
00757 
00758   if( hardware_->read( this->getAccelAddress(), this->getProtocol(), this->getFrequency(), this->getFlags(), address, Data, 2 ) < 0 )
00759   {
00760     ROS_ERROR("BMC050::getAccelY(): Error reading from I2C interface!");
00761     return false;
00762   }
00763               
00764   AccelY_ = this->getAccelSensitivity() * int( ( ((int16_t)(Data[1] << 8)) | ( ((int16_t)Data[0]) ) ) >> 6 );
00765   return AccelY_;
00766 }
00767 
00768 
00769 /**********************************************************************/
00770 
00771 /**********************************************************************/
00772 double BMC050::getAccelZ()
00773 {
00774   // gets raw data and updates : AccelZ_
00775   uint8_t Data[2];
00776   uint8_t address;
00777   // The sensor treats reads differently depending on the protocol: 
00778   switch( this->getProtocol() )
00779   {
00780   case I2C:
00781     address = ADDRESS_ACC_Z_LSB;
00782     break;
00783   case SPI:
00784     address = (FLAG_R | ADDRESS_ACC_Z_LSB);
00785     break;
00786   default:
00787     ROS_ERROR("BMC050::getAccelZ(): cannot read from this protocol.");
00788     return false;
00789   }
00790 
00791   if( hardware_->read( this->getAccelAddress(), this->getProtocol(), this->getFrequency(), this->getFlags(), address, Data, 2 ) < 0 )
00792   {
00793     ROS_ERROR("BMC050::getAccelZ(): Error reading from I2C interface!");
00794     return false;
00795   }
00796               
00797   AccelZ_ = this->getAccelSensitivity() * int( ( ((int16_t)(Data[1] << 8)) | ( ((int16_t)Data[0]) ) ) >> 6 );
00798   return AccelZ_;
00799 }
00800 
00801 
00802 /**********************************************************************/
00803 
00804 /**********************************************************************/
00805 double BMC050::getTemperature()
00806 {
00807   // gets raw data and updates : Temperature_
00808   uint8_t Data[2];
00809   uint8_t address;
00810   // The sensor treats reads differently depending on the protocol: 
00811   switch( this->getProtocol() )
00812   {
00813   case I2C:
00814     address = ADDRESS_TEMPERATURE;
00815     break;
00816   case SPI:
00817     address = (FLAG_R | ADDRESS_TEMPERATURE);
00818     break;
00819   default:
00820     ROS_ERROR("BMC050::getTemperature(): cannot read from this protocol.");
00821     return false;
00822   }
00823 
00824   if( hardware_->read( this->getAccelAddress(), this->getProtocol(), this->getFrequency(), this->getFlags(), address, Data, 2 ) < 0 )
00825   {
00826     ROS_ERROR("BMC050::getTemperature(): Error reading from I2C interface!");
00827     return false;
00828   }
00829  
00830   Temperature_ = ( TempSlope_ *(int8_t)Data[0] ) + 24;       
00831   return Temperature_;
00832 }
00833 
00834 
00835 /**********************************************************************/
00836 
00837 /**********************************************************************/
00838 bool BMC050::getCompassData()
00839 {
00840   // gets raw data and updates 4 values: CompassX_, CompassY_, CompassZ_, RHall_;
00841   uint8_t Data[8];
00842   uint8_t address;
00843   // The sensor treats reads differently depending on the protocol: 
00844   switch( this->getProtocol() )
00845   {
00846   case I2C:
00847     address = ADDRESS_COMPASS_XYZ;
00848     break;
00849   case SPI:
00850     address = (FLAG_R | ADDRESS_COMPASS_XYZ);
00851     break;
00852   default:
00853     ROS_ERROR("BMC050::getCompassData(): cannot read from this protocol.");
00854     return false;
00855   }
00856   
00857   if( hardware_->read( this->getCompassAddress(), this->getProtocol(), this->getFrequency(), this->getFlags(), address, Data, 8 ) < 0 )
00858   {
00859     ROS_ERROR("BMC050::getCompassData(): Error reading from hardware interface!");
00860     return false;
00861   }          
00862  
00863   CompassX_ = SensitivityXY_ * ( (int16_t)( (int16_t)Data[0] | ((int16_t)Data[1]) << 8 ) / 8);
00864   CompassY_ = SensitivityXY_ * ( (int16_t)( (int16_t)Data[2] | ((int16_t)Data[3]) << 8 ) / 8);
00865   CompassZ_ = SensitivityZ_ *  ( (int16_t)( (int16_t)Data[4] | ((int16_t)Data[5]) << 8 ) / 2);
00866   RHall_ = (uint16_t)((uint16_t)Data[6] | ((uint16_t)Data[7] << 8) >> 2 );
00867   return true;
00868 }
00869 
00870 
00871 /**********************************************************************/
00872 
00873 /**********************************************************************/
00874 double BMC050::getCompassX()
00875 {
00876   // gets raw data and updates CompassX_;
00877   uint8_t Data[2];
00878   uint8_t address;
00879  
00880   // reading depends on the Protocol! 
00881   switch( this->getProtocol() )
00882   {// The sensor treats reads differently depending on the protocol:
00883   case I2C:
00884     address = ADDRESS_COMPASS_X_LSB;
00885     break;
00886   case SPI:
00887     address = (FLAG_R | ADDRESS_COMPASS_X_LSB);
00888     break;
00889   default:
00890     ROS_ERROR("BMC050::getCompassX(): cannot read from this protocol.");
00891     return false;
00892   }            
00893   if( hardware_->read( this->getCompassAddress(), this->getProtocol(), this->getFrequency(), this->getFlags(), address, Data, 2 ) < 0 )
00894   {
00895     ROS_ERROR("BMC050::getCompassX(): Error reading from hardware interface!");
00896     return false;
00897   }          
00898 
00899   CompassX_ = SensitivityXY_ * ( (int16_t)( (uint16_t)Data[0] | ((uint16_t)Data[1]) << 8 ) / 8);
00900   return CompassX_; 
00901 }
00902 
00903 
00904 
00905 /**********************************************************************/
00906 
00907 /**********************************************************************/
00908 double BMC050::getCompassY()
00909 {
00910   // gets raw data and updates CompassY_;
00911   uint8_t Data[2];
00912   uint8_t address;
00913 
00914   // reading depends on the Protocol! 
00915   switch( this->getProtocol() )
00916   {// The sensor treats reads differently depending on the protocol:
00917   case I2C:
00918     address = ADDRESS_COMPASS_Y_LSB;
00919     break;
00920   case SPI:
00921     address = (FLAG_R | ADDRESS_COMPASS_Y_LSB);
00922     break;
00923   default:
00924     ROS_ERROR("BMC050::getCompassY(): cannot read from this protocol.");
00925     return false;
00926   }            
00927   if( hardware_->read( this->getCompassAddress(), this->getProtocol(), this->getFrequency(), this->getFlags(), address, Data, 2 ) < 0 )
00928   {
00929     ROS_ERROR("BMC050::getCompassY(): Error reading from hardware interface!");
00930     return false;
00931   }          
00932 
00933   CompassY_ = SensitivityXY_ * ( (int16_t)( (uint16_t)Data[0] | ((uint16_t)Data[1]) << 8 ) / 8);
00934   return CompassY_;
00935 }
00936 
00937 
00938 
00939 /**********************************************************************/
00940 
00941 /**********************************************************************/
00942 double BMC050::getCompassZ()
00943 {
00944   // gets raw data and updates CompassZ_;
00945   uint8_t Data[2];
00946   uint8_t address;
00947 
00948   // reading depends on the Protocol! 
00949   switch( this->getProtocol() )
00950   {// The sensor treats reads differently depending on the protocol:
00951   case I2C:
00952     address = ADDRESS_COMPASS_Z_LSB;
00953     break;
00954   case SPI:
00955     address = (FLAG_R | ADDRESS_COMPASS_Z_LSB);
00956     break;
00957   default:
00958     ROS_ERROR("BMC050::getCompassZ(): cannot read from this protocol.");
00959     return false;
00960   }            
00961   if( hardware_->read( this->getCompassAddress(), this->getProtocol(), this->getFrequency(), this->getFlags(), address, Data, 2 ) < 0 )
00962   {
00963     ROS_ERROR("BMC050::getCompassZ(): Error reading from hardware interface!");
00964     return false;
00965   }          
00966       
00967   CompassZ_ = SensitivityZ_ * ( (int16_t)( (uint16_t)Data[0] | ((uint16_t)Data[1]) << 8 ) / 2);
00968   return CompassZ_;
00969 }
00970 
00971 
00972 /**********************************************************************/
00973 
00974 /**********************************************************************/
00975 uint16_t BMC050::getRHall()
00976 {
00977   // gets raw data and updates RHall_;
00978   uint8_t Data[2];
00979   uint8_t address;
00980 
00981   // reading depends on the Protocol! 
00982   switch( this->getProtocol() )
00983   {// The sensor treats reads differently depending on the protocol:
00984   case I2C:
00985     address = ADDRESS_RHALL;
00986     break;
00987   case SPI:
00988     address = (FLAG_R | ADDRESS_RHALL);
00989     break;
00990   default:
00991     ROS_ERROR("BMC050::getRHall(): cannot read from this protocol.");
00992     return false;
00993   }            
00994   if( hardware_->read( this->getCompassAddress(), this->getProtocol(), this->getFrequency(), this->getFlags(), address, Data, 2 ) < 0 )
00995   {
00996     ROS_ERROR("BMC050::getRHall(): Error reading from hardware interface!");
00997     return false;
00998   }          
00999 
01000   RHall_ = (uint16_t)((uint16_t)Data[0] | ((uint16_t)Data[1] << 8) >> 2 );
01001   return RHall_; 
01002 }
01003 
01004 
01005 /**********************************************************************/
01006 
01007 /**********************************************************************/
01008 double BMC050::getUncompensatedYaw()
01009 {
01010   // assumes you've already taken a measurement.
01011   return atan2( CompassX_, CompassY_ ) * ( 180.0 / M_PI );
01012 }
01013 
01014 
01015 /**********************************************************************/
01016 
01017 /**********************************************************************/
01018 uint8_t BMC050::getDeviceAddress()
01019 {
01020   // The bmc050 cannot identify itself generically since it has two device addresses.
01021   ROS_ERROR("BMC050::getDeviceAddress(): Invalid request.  Accelerometer and Compass have separate device addresses.");
01022   return 0;
01023 }
01024 
01025 
01026 /**********************************************************************/
01027 
01028 /**********************************************************************/
01029 bool BMC050::enterLowPowerModeAccel()
01030 {
01031   switch( accel_mode_ )
01032   {
01033   case accel_low_power_:
01034     ROS_WARN("BMC050::enterLowPowerModeAccel(): sensor already in low-power mode.");
01035     break;
01036     // normal and low-power modes enter suspend mode in the same way:
01037   case accel_normal_: {}
01038   case accel_suspend_:
01039     // write 1 to the suspend bit in accel register 0x11:
01040     // prepare our copy of the register:
01041     modechange_accel_ = ( (1 << LOWPOWER_EN) | modechange_accel_ ) & ~(1 << SUSPEND);
01042     // write our copy of the register:
01043     if( this->writeToAccelReg(MODECHANGE_ACCEL, modechange_accel_ ) == false )
01044     {
01045       ROS_ERROR("BMC050::enterLowPowerModeAccel(): write failed.");
01046       // retreive the sensor copy of the 0x11 register:
01047       this->readRegAccel( MODECHANGE_ACCEL, &modechange_accel_ );
01048       return false;
01049     }
01050     break;
01051   default:
01052     ROS_ERROR("BMC050::enterLowPowerModeAccel(): failed.");
01053     return false;
01054   }
01055   // set new mode:
01056   accel_mode_ = accel_low_power_;
01057   return true;
01058 }
01059 
01060 
01061 /**********************************************************************/
01062 bool BMC050::enterSuspendModeAccel()
01063 {
01064   switch( accel_mode_ )
01065   {
01066   case accel_suspend_:
01067     ROS_INFO("BMC050::enterSuspendModeAccel(): sensor already in suspend mode.");
01068     break;
01069     // normal and low-power modes enter suspend mode in the same way:
01070   case accel_normal_: {}
01071   case accel_low_power_:
01072     // write 1 to the suspend bit in accel register 0x11:
01073     // prepare our copy of the register:
01074     modechange_accel_ = ( (1 << SUSPEND) | modechange_accel_ ) & ~(1 << LOWPOWER_EN);
01075     // write our copy of the register:
01076     if( this->writeToAccelReg( MODECHANGE_ACCEL, modechange_accel_ ) == false )
01077     {
01078       ROS_ERROR("BMC050::enterSuspendModeAccel(): write failed.");
01079       // retreive the sensor copy of the 0x11 register:
01080       this->readRegAccel( MODECHANGE_ACCEL, &modechange_accel_ );
01081       return false;
01082     }
01083     break;
01084   default:
01085     ROS_ERROR("BMC050::enterSuspendModeAccel(): failed.");
01086     return false;
01087   }
01088   // set new mode:
01089   accel_mode_ = accel_suspend_;
01090   return true;
01091 }
01092 
01093 
01094 /**********************************************************************/
01095 bool BMC050::enterNormalModeAccel()
01096 {
01097   switch( accel_mode_ )
01098   {
01099   case accel_normal_:
01100     ROS_WARN("BMC050::enterNormalModeAccel(): sensor already in normal mode.");
01101     break;
01102     // suspend and low-power modes enter normal mode in the same way:
01103   case accel_suspend_: {}
01104   case accel_low_power_:
01105     // Clear both SUSPEND bit and LOWPOWER_EN bit in accel reg 0x11:
01106     // prepare our copy of the register:
01107     modechange_accel_ &= ~( (1 << SUSPEND) | (1 << LOWPOWER_EN) );
01108     // write our copy of the register:
01109     if( this->writeToAccelReg( MODECHANGE_ACCEL, modechange_accel_ ) == false )
01110     {
01111       ROS_ERROR("BMC050::enterNormalModeAccel(): write failed.");
01112       // retreive the sensor copy of the 0x11 register:
01113       this->readRegAccel( MODECHANGE_ACCEL, &modechange_accel_ );
01114       return false;
01115     }
01116     usleep( 800 ); // mandatory delay.  See datasheet page 8.
01117     break;
01118   default:
01119     ROS_ERROR("BMC050::enterNormalModeAccel(): failed.");
01120     return false;
01121   }
01122   // set new mode:
01123   accel_mode_ = accel_normal_;
01124   return true;
01125 }
01126 
01127 
01128 /**********************************************************************/
01129 bool BMC050::enterSuspendModeCompass()
01130 {
01131   // depends on the mode the sensor is currently in:
01132   switch( compass_mode_ )
01133   {
01134   case compass_suspend_:
01135     ROS_WARN("BMC050::enterSuspendModeCompass(): device already in Suspend Mode.");
01136     break;
01137   case compass_forced_:
01138     // sensor must enterSleepMode first; then enterSuspendMode from there:
01139     this->enterSleepModeCompass();  // don't break;
01140     // sleep_ and normal_ mode enterSuspendMode in the exact same way:
01141   case compass_sleep_: {}
01142   case compass_normal_: {}
01143     // adjust bitflags in our copy of the register:
01144     softreset_ &= (0 << POWER_CONTROL_BIT);
01145     // write our copy of the register:
01146     if( this->writeToCompassReg( SOFTRESET_COMPASS_REG, softreset_ ) == false )
01147     {
01148       ROS_ERROR("BMC050::enterSuspendModeCompass(): failed to write to registers.");
01149       // if unsuccessful, get an old copy of the register:
01150       this->readRegCompass( SOFTRESET_COMPASS_REG, &softreset_ );
01151       return false;
01152     }
01153     break;
01154   default:
01155     ROS_ERROR( "BMC050::enterSuspendModeCompass(): failed to enter Suspend Mode." );
01156     return false;
01157   }
01158   // adjust current mode:
01159   compass_mode_ = compass_suspend_;
01160   return true;
01161 }
01162 
01163 
01164 /**********************************************************************/
01165 bool BMC050::enterSleepModeCompass()
01166 {
01167   // depends on what mode the sensor is currently in:
01168   switch( compass_mode_ )
01169   {
01170   case compass_normal_: {}
01171   case compass_forced_:
01172     // adjust flags:
01173     rate_compass_ |= (SET_OPMODE_SLEEP << OPMODE); // shift b00000011 left by one bit and set those bits
01174     // set OPMODE bits here
01175     if( this->writeToCompassReg( RATE_COMPASS, rate_compass_ ) == false )
01176     {
01177       ROS_ERROR("BMC050::enterSleepModeCompass(): failed.");
01178       // revert flags:
01179       this->readRegCompass( RATE_COMPASS, &rate_compass_ );
01180       return false;
01181     }
01182     break;
01183   case compass_suspend_:
01184     // adjust flags
01185     softreset_ = (1 << POWER_CONTROL_BIT);
01186     // set new register value here
01187     if( this->writeToCompassReg( SOFTRESET_COMPASS_REG, softreset_ ) == false )
01188     {
01189       ROS_ERROR("BMC050::enterSleepModeCompass(): writing to registers failed.");
01190       // revert flags:
01191       this->readRegCompass( SOFTRESET_COMPASS_REG, &softreset_ );
01192       return false;
01193     }
01194     usleep( 3000 ); // mandatory delay --- see datasheet page 10
01195     break;
01196   case compass_sleep_:
01197     ROS_WARN("BMC050::enterSleepModeCompass():Device already in sleep mode.");
01198     break;
01199   default:
01200     ROS_ERROR("BMC050::enterSleepModeCompass(): failed.");
01201     return false;
01202   }
01203   // set new mode:
01204   compass_mode_ = compass_sleep_; 
01205   return true;
01206 }
01207 
01208 
01209 /**********************************************************************/
01210 bool BMC050::enterNormalModeCompass()
01211 {
01212   // depends on what mode we're already in:
01213   switch( compass_mode_ )
01214   {
01215     // suspend_ and forced_ mode must first enter sleep mode:
01216   case compass_suspend_: {}
01217   case compass_forced_: 
01218     // first enter sleep mode:
01219     this->enterSleepModeCompass(); // don't break;
01220     // then the sensor can directly enter normal mode:
01221   case compass_sleep_:
01222     // adjust flags
01223     rate_compass_ &= ~(SET_OPMODE_SLEEP << OPMODE);  // set flags;
01224     // set new register value here
01225     if( this->writeToCompassReg( RATE_COMPASS, rate_compass_ ) == false )
01226     {
01227       ROS_ERROR("BMC050::enterNormalModeCompass(): failed to write to registers.");
01228       // revert flags to sensor's flags:
01229       this->readRegCompass( RATE_COMPASS, &rate_compass_ );
01230       return false;
01231     } 
01232     break;
01233   case compass_normal_:
01234     // done!
01235     ROS_WARN("BMC050::enterNormalModeCompass(): sensor already in normal mode.");
01236     break;
01237   default:
01238     ROS_ERROR("BMC050::enterNormalModeCompass(): error entering Normal Mode.");
01239     return false;
01240   }
01241   // set new mode:
01242   compass_mode_ = compass_normal_;
01243   return true;
01244 }
01245 
01246 
01247 /**********************************************************************/
01248 bool BMC050::enterForcedModeCompass()
01249 { 
01250   // depends on the mode the sensor is currently in:
01251   switch( compass_mode_ )
01252   {
01253   case compass_forced_:
01254     ROS_WARN("BMC050::enterForcedModeCompass(): device already in forced mode.");
01255     break;
01256     // can only directly be entered from sleepmode:
01257   case compass_suspend_: {}
01258   case compass_normal_: {}
01259     this->enterSleepModeCompass();  // don't break;
01260   case compass_sleep_:
01261     // adjust flags
01262     rate_compass_ &= ~(SET_OPMODE_SLEEP << OPMODE);  // clear flags;
01263     rate_compass_ |= (SET_OPMODE_FORCED << OPMODE ); // set new flags;
01264     // set new register value here
01265     if( this->writeToCompassReg( RATE_COMPASS, rate_compass_ ) == false )
01266     {
01267       ROS_ERROR("BMC050::enterForcedModeCompass(): failed to write to registers.");
01268       // revert flags to sensor's flags:
01269       this->readRegCompass( RATE_COMPASS, &rate_compass_ );
01270       return false;
01271     }
01272     break;
01273   default:
01274     ROS_ERROR("BMC050::enterForcedModeCompass(): failed.");
01275     return false;
01276   }
01277   compass_mode_ = compass_forced_;
01278   return true;
01279 }
01280 
01281 
01282 /**********************************************************************/
01283 // reads a register and returns its value to the second argument
01284 /**********************************************************************/
01285 bool BMC050::readRegAccel( uint8_t reg, uint8_t* value )
01286 {
01287   // Reading depends on the protocol.
01288   switch( this->getProtocol() )
01289   {
01290   case I2C:
01291     if( hardware_->read( this->getAccelAddress(), I2C, this->getFrequency(), this->getFlags(), reg, value, 1 ) < 0 ) 
01292     {
01293       ROS_ERROR("BMC050::readRegAccel(): Error reading register via I2C!");
01294       return false;
01295     } 
01296     break;
01297   case SPI:
01298     // we must prepend the SPI_READ_FLAG.
01299     if( hardware_->read( this->getAccelAddress(), SPI, this->getFrequency(), this->getFlags(), (1 << SPI_READ_FLAG) | reg, value, 1 ) < 0 )
01300     {
01301       ROS_ERROR("BMC050::readRegAccel(): Error reading register via SPI!");
01302       return false;
01303     } 
01304     break;
01305   default:
01306     ROS_ERROR("BMC050::readRegAccel(...): invalid protocol.");
01307     return false;
01308   }
01309   return true;
01310 }
01311 
01312 
01313 /**********************************************************************/
01314 // reads a register and returns its value.  (for testing)
01315 /**********************************************************************/
01316 bool BMC050::readRegCompass( uint8_t reg, uint8_t* value )
01317 {
01318   switch( this->getProtocol() )
01319   {
01320   case I2C:
01321     if( hardware_->read( this->getCompassAddress(), I2C, this->getFrequency(), this->getFlags(), reg, value, 1 ) < 0 ) 
01322     {
01323       ROS_ERROR("BMC050::readRegCompass(): Error reading register via I2C!");
01324       return false;
01325     } 
01326     break;
01327   case SPI:
01328     // we must prepend the SPI_READ_FLAG, although, technically it's already there, since it's zero.
01329     if (hardware_->read( this->getCompassAddress(), SPI, this->getFrequency(), this->getFlags(), (1 << SPI_READ_FLAG) | reg, value, 1 ) < 0 ) 
01330     {
01331       ROS_ERROR("BMC050::readRegCompass(): Error reading register via SPI!");
01332       return false;
01333     } 
01334     break;
01335   default:
01336     ROS_ERROR("BMC050::readRegistersCompass(...): invalid protocol.");
01337     return false;
01338   }
01339   return true;
01340 }
01341 
01342 
01343 /**********************************************************************/
01344 // writes a byte to a register in the Accelerometer
01345 /**********************************************************************/
01346 bool BMC050::writeToAccelReg( uint8_t reg, uint8_t value )
01347 {
01348   switch( this->getProtocol() )
01349   {
01350   case I2C:
01351     if( hardware_->write( this->getAccelAddress(), I2C, this->getFrequency(), this->getFlags(), (uint8_t)reg, (uint8_t*)&value, 1 ) < 0 )
01352     {
01353       ROS_ERROR("BMC050::writeToAccelReg(): Error writing to register via I2C!");
01354       return false;
01355     } 
01356     break;
01357   case SPI:
01358     // we must prepend the SPI_WRITE_FLAG, although, technically it's already there, since it's zero.
01359     if( hardware_->write( this->getAccelAddress(), SPI, this->getFrequency(), this->getFlags(), (uint8_t) (~(1 << SPI_WRITE_FLAG)&reg), (uint8_t*)&value, 1 ) < 0 ) 
01360     {
01361       ROS_ERROR("BMC050::writeToAccelReg(): Error writing to register via SPI!");
01362       return false;
01363     } 
01364     break;
01365   default:
01366     ROS_ERROR("BMC050::writeToAccelReg(...): invalid protocol.");
01367     return false;
01368   }
01369   return true;
01370 }
01371 
01372 
01373 /**********************************************************************/
01374 // writes a byte to a register in the compass.
01375 /**********************************************************************/
01376 bool BMC050::writeToCompassReg( uint8_t reg, uint8_t value )
01377 {
01378   switch( this->getProtocol() )
01379   {
01380   case I2C:
01381     if( hardware_->write( this->getCompassAddress(), I2C, this->getFrequency(), this->getFlags(), (uint8_t)reg, (uint8_t*)&value, 1 ) < 0)
01382     {
01383       ROS_ERROR("BMC050::writeToCompassReg(): Error writing to register via I2C!");
01384       return false;
01385     } 
01386     break;
01387   case SPI:
01388     // we must prepend the SPI_WRITE_FLAG, although, technically it's already there, since it's zero.
01389     if( hardware_->write( this->getCompassAddress(), SPI, this->getFrequency(), this->getFlags(), (uint8_t) (~(1 << SPI_WRITE_FLAG)&reg), (uint8_t*)&value, 1 ) < 0 ) 
01390     {
01391       ROS_ERROR("BMC050::writeToCompassReg(): Error writing to register via SPI!");
01392       return false;
01393     } 
01394     break;
01395   default:
01396     ROS_ERROR("BMC050::writeToCompassReg(...): invalid protocol.");
01397     return false;
01398   }
01399   return true;
01400 }
01401 
01402 
01403 /**********************************************************************/
01404 // writes a byte to a compass register.  Reads it back to verify.
01405 /**********************************************************************/
01406 bool BMC050::writeToCompassRegAndVerify( uint8_t reg, uint8_t value, uint8_t expected )
01407 {
01408   this->writeToCompassReg( reg, value );
01409  
01410   uint8_t actual;
01411  
01412   this->readRegCompass( reg, &actual );
01413  
01414   if( expected != actual )
01415   {
01416     ROS_ERROR("BMC050::writeToCompassRegAndVerify(...): Compass Register: %d  actual: %d  expected: %d ", reg, actual, expected); 
01417     return false;
01418   }
01419  
01420   return true;
01421 }
01422 
01423 
01424 /**********************************************************************/
01425 // writes a byte to an accel register.  Reads it back to verify.
01426 /**********************************************************************/
01427 bool BMC050::writeToAccelRegAndVerify( uint8_t reg, uint8_t value, uint8_t expected )
01428 {
01429   this->writeToAccelReg( reg, value );
01430  
01431   uint8_t actual;
01432  
01433   this->readRegAccel( reg, &actual );
01434  
01435   if( expected != actual )
01436   {
01437     ROS_ERROR("BMC050::writeToAccelRegAndVerify(...): Accelerometer Register: %d  actual: %d  expected: %d", reg, actual, expected); 
01438     return false;
01439   }
01440 
01441   return true;
01442 }
01443 
01444 
01445 /**********************************************************************/
01446 /**********************************************************************/
01447 bool BMC050::printOffsets()
01448 {
01449   uint8_t reg_to_print;    
01450   std::string offset_reg_names[6] = { "offset_filt_x", "offset_filt_y", "offset_filt_z", "offset_unfilt_x", "offset_unfilt_y", "offset_unfilt_z" };
01451  
01452   for( uint8_t i = OFFSET_FILT_X; i <= OFFSET_UNFILT_Z; i++ )
01453   {
01454     if( this->readRegAccel( i, &reg_to_print ) == false )
01455       return false;
01456 
01457     std::cout << offset_reg_names[i - OFFSET_FILT_X] << ": " << (int)reg_to_print << std::endl;
01458   }
01459   return true;
01460 }


bmc050_driver
Author(s): Joshua Vasquez, Philip Roan. Maintained by Philip Roan
autogenerated on Mon Oct 6 2014 10:09:40