bmg160.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 "bmg160_driver/bmg160.hpp"
00051 #include "bmg160_driver/bmg160_parameters.hpp"
00052 
00053 /**********************************************************************/
00054 // Constructor
00055 /**********************************************************************/
00056 BMG160::BMG160( bosch_hardware_interface* hw ) :
00057   sensor_driver( hw ),
00058   BMG160Parameters(),
00059   TempSlope_( 0.5 )
00060 {
00061 }
00062 
00063 
00064 /**********************************************************************/
00065 // Destructor
00066 /**********************************************************************/
00067 BMG160::~BMG160() 
00068 {
00069 }
00070 
00071 
00072 /**********************************************************************/
00073 // Initialize sensor and hardware on requested parameters:
00074 /**********************************************************************/
00075 bool BMG160::initialize()
00076 {  
00077   // Initialize the hardware interface with the selected parameters.
00078   if( hardware_->initialize() == false )
00079   {
00080     ROS_ERROR("BMG160::initialize(): Could not initialize a hardware interface!");
00081     return false;
00082   }
00083   if( this->softReset() == false )
00084     return false;
00085 
00086   usleep( 3000 ); // mandatory wait for sensor startup
00087 
00088   // change accel_range to the range set in the parameters class:     
00089   ROS_INFO( "BMG160::initialize(): adjusting Gyroscope range." );
00090   if( this->changeRange() == false )
00091     return false;
00092   ROS_INFO( "BMG160::initialize(): Gyroscope range adjusted." );
00093   // filter data at requested bandwidth, if requested in parameters:
00094   ROS_INFO( "BMG160::initialize(): Applying Filter parameter." );
00095   if( this->filterData( gyro_is_filtered_ ) == false )
00096     return false;
00097   ROS_INFO( "BMG160::initialize(): Filter parameter applied." );
00098   // change accelerometer bandwidth to the bandwidth set in the parameters:
00099   ROS_INFO( "BMG160::initialize(): Adjusting Gyroscope Bandwidth." );
00100   if( this->changeBandwidth() == false )
00101     return false;
00102   ROS_INFO( "BMG160::initialize(): Gyroscope Bandwidth adjusted." );
00103   
00104   return true;
00105 }
00106 
00107 
00108 /**********************************************************************/
00109 /**********************************************************************/
00110 bool BMG160::softReset()
00111 { 
00112   if( this->writeToReg( ADDRESS_SOFTRESET, SOFTRESET_CMD ) == false )
00113   {
00114     ROS_ERROR( "BMG160::SoftReset(): failed." );
00115     return false;
00116   }
00117   usleep( 2000 ); // mandatory delay. See datasheet page
00118   
00119   return true;
00120 }
00121 
00122 
00123 /**********************************************************************/
00124 /*INPUT: none directly. (indirectly) accel_range_, which was set 
00125  *        in the parameters.
00126  *OUTPUT: bool              -- whether or not it was successful.
00127  * This method performs two tasks:
00128  *    1. changes the accel_range to the accel_range set in the parameters.
00129  *    2. changes the class's accel_sensitivity, so that the output
00130  *      values will be multiplied correctly to produce correct values. 
00131  */
00132 /**********************************************************************/
00133 bool BMG160::changeRange()
00134 {
00135   uint8_t local_range;
00136   // Read register with range settings for a local copy:
00137   if( this->readReg( ADDRESS_RANGE, &local_range ) == false )
00138     return false;
00139     
00140   // #ifdef DEBUG
00141   ROS_INFO( "Range bits before: %d.  Default: %d", local_range & 0x07, 0 ); // Defaults on page 30
00142   // #endif
00143     
00144   // add our command to change range:
00145   local_range &= ~(0x07); // clear old range value. Mask: b11111000
00146   local_range |= range_; // insert new range value.
00147   
00148   // write the adjusted register value back to the sensor's register:
00149   if( this->writeToReg( ADDRESS_RANGE, local_range ) == false )
00150     return false;
00151     
00152   // read back that register to make sure that changes worked:
00153   if( this->readReg( ADDRESS_RANGE, &local_range ) == false )
00154     return false;
00155     
00156   // Compare register values to what we expect:
00157   uint8_t expected_reg = range_;
00158   uint8_t actual_reg  = local_range & 0x07;
00159   
00160   // #ifdef DEBUG
00161   ROS_INFO( "Range bits after:  %d.  Expected: %d", actual_reg, expected_reg );
00162   // #endif
00163     
00164   if( expected_reg != actual_reg )
00165     return false;
00166     
00167   // if everything works:
00168   return true;
00169 }
00170 
00171 
00172 /**********************************************************************/
00173 /**********************************************************************/
00174 bool BMG160::filterData( bool request )
00175 {
00176   uint8_t local_bandwidth;
00177   // Read register with accel range settings for a local copy:
00178   if( this->readReg( ADDRESS_FILTER, &local_bandwidth ) == false )
00179     return false;
00180     
00181   // #ifdef DEBUG
00182   ROS_INFO( "filter bits before: %d.  Default: %d", ( local_bandwidth >> DATA_HIGH_BW ) & 0x01, 0 ); // Defaults from page 30.
00183   // #endif 
00184 
00185   // add our command to apply filter: 
00186   if( request == true )
00187     local_bandwidth &= ~(1 << DATA_HIGH_BW);  // filtered: write 0
00188   else
00189     local_bandwidth |= (1 << DATA_HIGH_BW);   // unfiltered: write 1
00190     
00191   // write the adjusted register value back to the sensor's register:
00192   if( this->writeToReg( ADDRESS_FILTER, local_bandwidth ) == false )
00193     return false;
00194     
00195   // read back that register to make sure that changes worked:
00196   if( this->readReg( ADDRESS_FILTER, &local_bandwidth ) == false )
00197     return false;
00198     
00199   // Compare register values to what we expect:
00200   uint8_t expected_reg = !gyro_is_filtered_;
00201   uint8_t actual_reg  = ( local_bandwidth >> DATA_HIGH_BW ) & 0x01;
00202   
00203   // #ifdef DEBUG                                                       
00204   ROS_INFO( "filter bits after:  %d.  Expected: %d", actual_reg, expected_reg );
00205   // #endif
00206     
00207   if( expected_reg != actual_reg )
00208     return false;
00209     
00210   return true;
00211 }
00212 
00213 
00214 /**********************************************************************/
00215 /**********************************************************************/
00216 bool BMG160::changeBandwidth()
00217 {
00218   uint8_t local_bw_reg;
00219   // read reg for a local copy:
00220   if( this->readReg( ADDRESS_BANDWIDTH, &local_bw_reg ) == false )
00221     return false;
00222   
00223   ROS_INFO( "bandwidth setting before: %d.  Default:  %d", local_bw_reg & 0x0F, 0 ); // Defaults from page 50.
00224   
00225   // apply new bandwidth value:
00226   local_bw_reg &= ~(0x0F); //Mask: 1111000
00227   local_bw_reg |= bw_reg_;
00228   // write back reg with new bandwidth value:
00229   if( this->writeToReg( ADDRESS_BANDWIDTH, local_bw_reg ) == false )
00230     return false;
00231   // verify that we wrote back the correct value:
00232   uint8_t actual_reg;
00233   if( this->readReg( ADDRESS_BANDWIDTH, &actual_reg ) == false )
00234     return false;
00235   
00236   uint8_t actual_bw = actual_reg & 0x0F; // mask off unrelated bits.
00237   uint8_t expected_bw = bw_reg_;
00238   
00239   if( expected_bw != actual_bw )
00240   {
00241     ROS_ERROR( "bandwidth setting after: %d.  Expected: %d", actual_bw, bw_reg_ );
00242     ROS_ERROR( "BMG160::changeBandwidth(): failed." );
00243     return false;
00244   }
00245   else
00246     ROS_INFO( "bandwidth setting after: %d.  Expected: %d", actual_bw, bw_reg_ );
00247 
00248   return true;
00249 }
00250 
00251   
00252 /**********************************************************************/
00253 /**********************************************************************/
00254 void BMG160::SimpleCalibration()
00255 {
00256   uint8_t local_enable_cal_reg; 
00257   uint8_t calibration_done;
00258   uint8_t data_is_filtered;
00259   // check whether or not gyro is filtered: (read register directly)
00260   if( this->readReg( ADDRESS_FILTER, &data_is_filtered ) == false )
00261   {
00262     std::cout<< "Calibration Error occured" << std::endl;
00263     return;
00264   }
00265   data_is_filtered = !(bool)((data_is_filtered & (1 << DATA_HIGH_BW) >> DATA_HIGH_BW));
00266   // make sure this is what the class thinks:
00267   if( gyro_is_filtered_ != (bool)data_is_filtered )
00268   {
00269     ROS_ERROR("Calibration Error occured.");
00270     return;
00271   }
00272     
00273   // tell sensor which version of calibration it should perform:
00274   // the filtered version or unfiltered version, depending on user's
00275   // configuration:
00276   uint8_t local_filtered_reg;
00277   if( this->readReg( ADDRESS_FILTERED_CAL_FAST, &local_filtered_reg ) == false )
00278   {
00279     ROS_ERROR("Calibration Error occured.");
00280     return;
00281   }
00282   // Apply our changes:
00283   if( (bool)data_is_filtered )
00284     local_filtered_reg &= ~(1 << fast_offset_unfilt); // write 0
00285   else
00286     local_filtered_reg |= (1 << fast_offset_unfilt); // write 1
00287   // Write back Calibration type:
00288   if( this->writeToReg( ADDRESS_FILTERED_CAL_FAST, local_filtered_reg ) == false )
00289   {
00290     ROS_ERROR("Calibration Error occured.");
00291     return;
00292   }
00293 
00294   // Query User
00295   std::cout << "Place the sensor in a static position.  Press [y/n] to continue/cancel." << std::endl;
00296   std::string reply;
00297   std::getline( std::cin, reply );
00298   if( reply != "Y" && reply != "y" )
00299   {
00300     ROS_WARN("Calibration cancelled by user.");
00301     return;
00302   }   
00303   
00304   // Begin calibration on each axis simultaneously:
00305   std::cout << "Beginning Fast Calibration routine:" << std::endl << std::endl;
00306  
00307   if( this->readReg( ADDRESS_ENABLE_FAST_CAL, &local_enable_cal_reg ) == false )
00308   {
00309     std::cout<< "Calibration Error occured" << std::endl;
00310     return;
00311   }
00312   // set bits for all three axes:
00313   local_enable_cal_reg |= 0x07;  
00314   // add in fast_offset_en bit:
00315   local_enable_cal_reg |= (1 << fast_offset_en);
00316     
00317   if( this->writeToReg( ADDRESS_ENABLE_FAST_CAL, local_enable_cal_reg ) == false )
00318   {
00319     std::cout<< "Calibration Error occured" << std::endl;
00320     return;
00321   }
00322   std::cout << "Calibrating..." << std::endl;
00323     
00324   // continue reading fast_offset_en bit until it is reset to zero:
00325   do
00326   {
00327     this->readReg( ADDRESS_ENABLE_FAST_CAL, &calibration_done );
00328     calibration_done &= (1 << fast_offset_en);
00329     calibration_done = calibration_done >> fast_offset_en;
00330   }
00331   while( calibration_done == 1);
00332   
00333   std::cout << "GYROSCOPE CALIBRATION COMPLETE" << std::endl;
00334   
00335   // Print final register settings (filtered and unfiltered):
00336   this->printOffsets(); 
00337 }
00338 
00339 
00340 /**********************************************************************/
00341 // gets raw data and updates GyroX_, GyroY_, GyroZ_, and Temperature_
00342 /**********************************************************************/
00343 bool BMG160::takeMeasurement()
00344 {
00345   uint8_t Data[7];
00346 
00347   if( this->readSensorData( ADDRESS_GYRO_X_LSB, Data, 7 ) < 0 )
00348   {
00349     ROS_ERROR("BMG160::takeMeasurement(): Error reading from hardware interface!");
00350     return false;
00351   }
00352     
00353   uint16_t tempx = Data[1];
00354   uint16_t tempy = Data[3];
00355   uint16_t tempz = Data[5];
00356   GyroX_ = this->getSensitivity() * (int16_t)( (tempx << 8) | Data[0] );
00357   GyroY_ = this->getSensitivity() * (int16_t)( (tempy << 8) | Data[2] );
00358   GyroZ_ = this->getSensitivity() * (int16_t)( (tempz << 8) | Data[4] );
00359   
00360   Temperature_ = (TempSlope_ *(int8_t)Data[6]) + 24;            
00361               
00362   return true;
00363 }
00364 
00365 
00366 /**********************************************************************/
00367 // gets raw data and updates : GyroX_
00368 /**********************************************************************/
00369 double BMG160::getGyroX()
00370 {
00371   uint8_t Data[2];
00372 
00373   if( this->readSensorData( ADDRESS_GYRO_X_LSB, Data, 2) < 0 )
00374   {
00375     ROS_ERROR("BMG160::getGyroX(): Error reading from hardware interface!");
00376     return false;
00377   }
00378   
00379   uint16_t temp = Data[1];
00380   GyroX_ = this->getSensitivity() * (int16_t)( (temp << 8) | Data[0] );
00381   return GyroX_;
00382 }
00383   
00384  
00385 /**********************************************************************/
00386 // gets raw data and updates : GyroY_
00387 /**********************************************************************/
00388 double BMG160::getGyroY()
00389 {
00390   uint8_t Data[2];
00391   
00392   if( this->readSensorData( ADDRESS_GYRO_Y_LSB, Data, 2 ) < 0 )
00393   {
00394     ROS_ERROR("BMG160::getGyroY(): Error reading from hardware interface!");
00395     return false;
00396   }
00397   
00398   uint16_t temp = Data[1];
00399   GyroY_ = this->getSensitivity() * (int16_t)( (temp << 8) | Data[0] );
00400   return GyroY_;
00401 }
00402 
00403 
00404 /**********************************************************************/
00405 // gets raw data and updates : GyroZ_
00406 /**********************************************************************/
00407 double BMG160::getGyroZ()
00408 {
00409   uint8_t Data[2];
00410   
00411   if( this->readSensorData( ADDRESS_GYRO_Z_LSB, Data, 2 ) < 0 )
00412   {
00413     ROS_ERROR("BMG160::getGyroZ(): Error reading from hardware interface!");
00414     return false;
00415   }
00416 
00417   uint16_t temp = Data[1];
00418   GyroZ_ = this->getSensitivity() * (int16_t)( (temp << 8) | Data[0] );
00419   return GyroZ_;
00420 }
00421 
00422 
00423 /**********************************************************************/
00424 // gets raw data and updates : Temperature_
00425 /**********************************************************************/
00426 double BMG160::getTemperature()
00427 {
00428   uint8_t Data[1];
00429   
00430   if( this->readSensorData( ADDRESS_TEMPERATURE, Data, 1 ) < 0 )
00431   {
00432     ROS_ERROR("BMG160::getTemperature(): Error reading from hardware interface!");
00433     return false;
00434   }
00435 
00436   Temperature_ = ( TempSlope_ * (int8_t)Data[0] ) + 24;         
00437   return Temperature_;
00438 }
00439 
00440 
00441 /**********************************************************************/
00442 /**********************************************************************/
00443 uint8_t BMG160::getDeviceAddress()
00444 {
00445   // depends on the protocol:
00446   switch( this->getProtocol() )
00447   {
00448   case I2C:
00449     // depends on hardware configuration:
00450     if( this-> getSlaveAddressBit() == false )
00451       return SLAVE_ADDRESS0;  
00452     else 
00453       return SLAVE_ADDRESS1;
00454     break;
00455   case SPI:
00456     return this->getPin();
00457   default:
00458     ROS_ERROR("BMG160::getAccelAddress(): invalid protocol.");
00459     return 0;
00460   }
00461 }
00462 
00463 
00464 /**********************************************************************/
00465 // reads a register and returns its value to the second argument
00466 /**********************************************************************/
00467 bool BMG160::readReg( uint8_t reg, uint8_t* value )
00468 {
00469   // Reading depends on the protocol.
00470   switch( this->getProtocol() )
00471   {
00472   case I2C:
00473     if( hardware_->read( this->getDeviceAddress(), I2C, this->getFrequency(), this->getFlags(), reg, value, 1 ) < 0 ) 
00474     {
00475       ROS_ERROR("BMG160::readReg(): Error reading register via I2C!");
00476       return false;
00477     } 
00478     break;
00479   case SPI:
00480     // we must prepend the SPI_READ_FLAG.
00481     if( hardware_->read( this->getDeviceAddress(), SPI, this->getFrequency(), this->getFlags(), (1 << SPI_READ_FLAG) | reg, value, 1 ) < 0 ) 
00482     {
00483       ROS_ERROR("BMG160::readReg(): Error reading register via SPI!");
00484       return false;
00485     } 
00486     break;
00487   default:
00488     ROS_ERROR("BMG160::readReg(...): invalid protocol.");
00489     return false;
00490   }
00491   return true;
00492 }
00493 
00494 
00495 /**********************************************************************/
00496 /**********************************************************************/
00497 bool BMG160::writeToReg( uint8_t reg, uint8_t value )
00498 {
00499   // Technically, writing depends on the protocol.
00500   switch( this->getProtocol() )
00501   {
00502   case I2C:
00503     if( hardware_->write( this->getDeviceAddress(), I2C, this->getFrequency(), this->getFlags(), (uint8_t)reg, (uint8_t*)&value, 1 ) < 0 )
00504     {
00505       ROS_ERROR("BMG160::writeToReg(): Error writing to register via I2C!");
00506       return false;
00507     } 
00508     break;
00509   case SPI:
00510     // we must prepend the SPI_WRITE_FLAG, although, technically it's already there, since it's zero.
00511     if( hardware_->write( this->getDeviceAddress(), SPI, this->getFrequency(), this->getFlags(), (uint8_t) (~(1 << SPI_WRITE_FLAG)&reg), (uint8_t*)&value, 1 ) < 0 ) 
00512     {
00513       ROS_ERROR("BMG160::writeToReg(): Error writing to register via SPI!");
00514       return false;
00515     } 
00516     break;
00517   default:
00518     // shouldn't happen:
00519     ROS_ERROR("BMG160::writeToReg(...): invalid protocol.");
00520     return false;
00521   }
00522   return true;
00523 }
00524 
00525 
00526 /**********************************************************************/
00527 /**********************************************************************/
00528 bool BMG160::writeToRegAndVerify( uint8_t reg, uint8_t value, uint8_t expected )
00529 {
00530   this->writeToReg( reg, value );
00531   
00532   uint8_t actual;
00533   
00534   this->readReg( reg, &actual );
00535   
00536   if( expected != actual )
00537   {
00538     ROS_ERROR("BMG160::writeToRegAndVerify(...): Register: %d  actual: %d  expected: %d", reg, actual, expected); 
00539     return false;
00540   }
00541   
00542   // if it all works:
00543   return true;
00544 }
00545 
00546 
00547 /**********************************************************************/
00548 /**********************************************************************/
00549 bool BMG160::readSensorData( uint8_t reg, uint8_t* data_array, uint8_t num_bytes )
00550 {
00551   // Reading depends on the protocol.
00552   switch( this->getProtocol() )
00553   {
00554   case I2C:
00555     if( hardware_->read( this->getDeviceAddress(), I2C, this->getFrequency(), this->getFlags(), reg, data_array, num_bytes ) < 0 ) 
00556     {
00557       ROS_ERROR("BMG160::readSensorData(): Error reading register via I2C!");
00558       return false;
00559     } 
00560     break;
00561   case SPI:
00562     // we must prepend the SPI_READ_FLAG.
00563     if( hardware_->read( this->getDeviceAddress(), SPI, this->getFrequency(), this->getFlags(), (1 << SPI_READ_FLAG) | reg, data_array, num_bytes ) < 0 ) 
00564     {
00565       ROS_ERROR("BMG160::readSensorData(): Error reading register via SPI!");
00566       return false;
00567     } 
00568     break;
00569   default:
00570     ROS_ERROR("BMG160::readSensorData(...): invalid protocol.");
00571     return false;
00572   }
00573   return true;  
00574 }
00575 
00576 
00577 /**********************************************************************/
00578 /**********************************************************************/
00579 void BMG160::computeOffsets( uint8_t* raw_offset_data, int16_t* clean_offset_data )
00580 {
00581   // format offset x:
00582   clean_offset_data[0] = raw_offset_data[1] << 8;
00583   clean_offset_data[0] |= (raw_offset_data[0] & 0xC0); // mask: 0b11000000
00584   clean_offset_data[0] |= ((uint8_t)(raw_offset_data[4] & 0x0C) << 2); // mask: 0b00001100
00585   clean_offset_data[0] = clean_offset_data[0] >> 4; // sign-extend 12bit to 16bit
00586   // format offset y:
00587   clean_offset_data[1] = raw_offset_data[2] << 8;
00588   clean_offset_data[1] |= ((uint8_t)(raw_offset_data[0] & 0x38) << 2);
00589   clean_offset_data[1] |= ((uint8_t)(raw_offset_data[4] & 0x02) << 3);
00590   clean_offset_data[1] = clean_offset_data[1] >> 4; // sign-extend 12bit to 16bit
00591   // format offset z:
00592   clean_offset_data[2] = raw_offset_data[3] << 8;
00593   clean_offset_data[2] |= ((uint8_t)raw_offset_data[0] << 5);
00594   clean_offset_data[2] |= ((uint8_t)(raw_offset_data[4] & 0x01) << 4);
00595   clean_offset_data[2] = clean_offset_data[2] >> 4; // sign-extend 12bit to 16bit
00596 }
00597 
00598 
00599 /**********************************************************************/
00600 // prints computed offset values
00601 /**********************************************************************/
00602 bool BMG160::printOffsets()
00603 {
00604   std::string offset_reg_names[3] = { "offset_x", "offset_y", "offset_z" };
00605   uint8_t raw_offset_data[5];
00606   int16_t clean_offset_data[3];
00607   
00608   if( this->readSensorData( ADDRESS_OFFSETS, raw_offset_data, 5 ) == false )
00609     return false;
00610     
00611   this->computeOffsets( raw_offset_data, clean_offset_data );
00612   
00613   // print:
00614   for( uint8_t i = 0; i < 3; i++ )
00615   {
00616     std::cout << offset_reg_names[i] << ": " << (int)clean_offset_data[i] << std::endl;
00617   }
00618   return true;
00619 }


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