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_parameters.hpp" 00051 00052 00053 /**********************************************************************/ 00054 /**********************************************************************/ 00055 BMG160Parameters::BMG160Parameters() 00056 { 00060 this->setFrequency( 400000 ); 00061 this->setProtocol( I2C ); 00062 this->setSlaveAddressBit( 0 ); 00063 00071 flags_ = 0; 00072 this->setSpiMode( SPI_MODE_0 ); 00073 this->setByteOrder( MSB_FIRST ); 00074 this->setPin( 10 ); 00075 00084 this->setRange( RANGE_2000 ); 00085 this->setBandwidth( BW_47HZ ); 00092 this->setFilter( true ); 00093 this->setRange( RANGE_2000 ); 00094 this->setBandwidth( BW_UNFILTERED ); 00095 } 00096 00097 00098 /**********************************************************************/ 00099 BMG160Parameters::~BMG160Parameters() 00100 { 00101 } 00102 00103 00104 /**********************************************************************/ 00105 double BMG160Parameters::getSensitivity() 00106 { 00107 return sensitivity_; 00108 } 00109 00110 00111 00112 /**********************************************************************/ 00113 bool BMG160Parameters::setProtocol( interface_protocol protocol ) 00114 { 00115 switch( protocol ) 00116 { 00117 case I2C: 00118 protocol_ = protocol; 00119 break; 00120 case SPI: 00121 protocol_ = protocol; 00122 break; 00123 default: 00124 ROS_ERROR("BMG160Parameters:Unsupported protocol."); 00125 return false; 00126 } 00127 return true; 00128 } 00129 00130 00131 /**********************************************************************/ 00132 bool BMG160Parameters::setFrequency( int frequency ) 00133 { 00134 frequency_ = frequency; 00135 return true; 00136 } 00137 00138 00139 00140 /**********************************************************************/ 00141 interface_protocol BMG160Parameters::getProtocol() 00142 { 00143 return protocol_; 00144 } 00145 00146 00147 /**********************************************************************/ 00148 int BMG160Parameters::getFrequency() 00149 { 00150 return frequency_; 00151 } 00152 00153 00154 /**********************************************************************/ 00155 bool BMG160Parameters::setPin( uint8_t pin ) 00156 { 00157 // adjust pin to desired value: 00158 pin_ = pin; 00159 // adjust the current flags to include the pin value: 00160 flags_ = ((0x0F)&flags_) | (pin_ << 4); 00161 00162 return true; 00163 } 00164 00165 00166 /**********************************************************************/ 00167 void BMG160Parameters::setSlaveAddressBit( bool high_or_low ) 00168 { 00169 slave_address_bit_ = high_or_low; 00170 } 00171 00172 00173 /**********************************************************************/ 00174 int BMG160Parameters::getPin() 00175 { 00176 return pin_; 00177 } 00178 00179 00180 /**********************************************************************/ 00181 bool BMG160Parameters::getSlaveAddressBit() 00182 { 00183 return slave_address_bit_; 00184 } 00185 00186 00187 00188 /**********************************************************************/ 00189 int* BMG160Parameters::getFlags() 00190 { 00191 return &flags_; 00192 } 00193 00194 00195 /**********************************************************************/ 00196 bool BMG160Parameters::setSpiMode( uint8_t mode ) 00197 { 00198 switch( mode ) 00199 { 00200 case SPI_MODE_3: 00201 break; 00202 case SPI_MODE_0: 00203 break; 00204 case SPI_MODE_1: {} 00205 case SPI_MODE_2: {} 00206 default: 00207 ROS_ERROR("BMG160Parameters: SPI_MODE can only be one of four choices"); 00208 return false; 00209 } 00210 00211 // adjust the flags 00212 flags_ = ((0xFC & flags_) | (mode) ); 00213 return true; 00214 } 00215 00216 00217 /**********************************************************************/ 00218 bool BMG160Parameters::setByteOrder( uint8_t value ) 00219 { 00220 if( value > 1 ) 00221 { 00222 ROS_ERROR("BMG160Parameters: Byte order must be either LSBFIRST or MSBFIRST"); 00223 return false; 00224 } 00225 // adjust the flags 00226 flags_ = ((0xFB & flags_) | (value << 2)); 00227 return true; 00228 } 00229 00230 00231 /**********************************************************************/ 00232 void BMG160Parameters::setRange( range new_range ) 00233 { 00234 // adjust the sensitivity accordingly: 00235 switch( new_range ) 00236 { 00237 case RANGE_2000: 00238 sensitivity_ = 0.06097561; // approx: (1/16.4) [(°/s) / LSB ] 00239 break; 00240 case RANGE_1000: 00241 sensitivity_ = 0.030487805; // approx: (1/32.8) [(°/s) / LSB ] 00242 break; 00243 case RANGE_500: 00244 sensitivity_ = 0.015243902; // approx: (1/65.6) [(°/s) / LSB ] 00245 break; 00246 case RANGE_250: 00247 sensitivity_ = 0.007621951; // approx: (1/131.2) [(°/s) / LSB ] 00248 break; 00249 case RANGE_125: 00250 sensitivity_ = 0.003810976; // approx: (1/262.4) [(°/s) / LSB ] 00251 break; 00252 default: // shouldn't happen because input argument is only an accel_range data type. 00253 ROS_ERROR("BMG160Parameters: invalid range setting."); 00254 return; 00255 } 00256 // reassign the class's new range: 00257 range_ = new_range; 00258 00263 } 00264 00265 00266 /**********************************************************************/ 00267 void BMG160Parameters::setFilter( bool request ) 00268 { 00269 gyro_is_filtered_ = request; 00270 } 00271 00272 00273 /**********************************************************************/ 00274 void BMG160Parameters::setBandwidth( bandwidth bw ) 00275 { 00276 bw_reg_ = bw; 00277 }