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 "bma180_driver/bma180_parameters.hpp" 00051 00052 00053 /**********************************************************************/ 00054 /**********************************************************************/ 00055 BMA180Parameters::BMA180Parameters() // Constructor 00056 { 00057 // set defaults: 00061 flags_ = 0; 00062 this->setFrequency( 400000 ); 00063 this->setProtocol( I2C ); 00064 this->setAccelRange( RANGE_2 ); // default: ±2 [g] 00065 this->setBandwidth( BW_150 ); // default 00066 this->setSlaveAddressBit( 0 ); // default: SDO connected to VSS 00067 00068 this->setPin( 10 ); // default: SPI Chip select pin. 00069 this->setSpiMode( SPI_MODE_3 ); // default: flags adjusted. These settings don't need to be tweaked to read the bma180 via SPI. 00070 this->setByteOrder( MSB_FIRST ); // default: flags adjusted. 00071 } 00072 00073 /**********************************************************************/ 00074 /**********************************************************************/ 00075 BMA180Parameters::~BMA180Parameters() // Destructor 00076 { 00077 } 00078 00079 00080 /**********************************************************************/ 00081 /**********************************************************************/ 00082 bool BMA180Parameters::setProtocol( interface_protocol protocol ) 00083 { 00084 switch( protocol ) 00085 { 00086 case I2C: 00087 protocol_ = protocol; 00088 break; 00089 case SPI: 00090 protocol_ = protocol; 00091 break; 00092 default: 00093 ROS_ERROR( "bma180_parameters:Unsupported protocol." ); 00094 return false; 00095 } 00096 return true; 00097 } 00098 00099 00100 /**********************************************************************/ 00101 /**********************************************************************/ 00102 bool BMA180Parameters::setFrequency( int frequency ) 00103 { 00104 frequency_ = frequency; 00105 return true; 00106 } 00107 00108 00109 /**********************************************************************/ 00110 /**********************************************************************/ 00111 interface_protocol BMA180Parameters::getProtocol() 00112 { 00113 return protocol_; 00114 } 00115 00116 00117 /**********************************************************************/ 00118 /**********************************************************************/ 00119 int BMA180Parameters::getFrequency() 00120 { 00121 return frequency_; 00122 } 00123 00124 00125 /**********************************************************************/ 00126 /**********************************************************************/ 00127 bool BMA180Parameters::setPin( uint8_t pin ) 00128 { 00129 // adjust pin to desired value: 00130 pin_ = pin; 00131 // adjust the current flags to include the pin value: 00132 flags_ = ( (0x0F)&flags_ ) | (pin_ << 4); // is this correct?? 00133 00134 return true; 00135 } 00136 00137 00138 /**********************************************************************/ 00139 /**********************************************************************/ 00140 int BMA180Parameters::getPin() 00141 { 00142 return pin_; 00143 } 00144 00145 00146 /**********************************************************************/ 00147 /**********************************************************************/ 00148 int* BMA180Parameters::getFlags() 00149 { 00150 return &flags_; 00151 } 00152 00153 00154 /**********************************************************************/ 00155 /**********************************************************************/ 00156 bool BMA180Parameters::setSlaveAddressBit( bool choice ) 00157 { 00158 slave_address_bit_ = choice; 00159 return true; 00160 } 00161 00162 00163 /**********************************************************************/ 00164 /**********************************************************************/ 00165 uint8_t BMA180Parameters::getSlaveAddressBit() 00166 { 00167 return slave_address_bit_; 00168 } 00169 00170 00171 /**********************************************************************/ 00172 /**********************************************************************/ 00173 bool BMA180Parameters::setByteOrder( uint8_t value ) 00174 { 00175 // adjust the flags 00176 flags_ = ( (0xFB & flags_) | (value << 2) ); 00177 if( value > 1 ) 00178 { 00179 ROS_ERROR("bma180_parameters: Byte order must be either LSB_FIRST or MSB_FIRST"); 00180 return false; 00181 } 00182 return true; 00183 00184 } 00185 00186 00187 /**********************************************************************/ 00188 /**********************************************************************/ 00189 bool BMA180Parameters::setSpiMode( uint8_t mode ) 00190 { 00191 // adjust the flags 00192 flags_ = ( (0xFC & flags_) | (mode) ); // 111111xx, where xx is the mode. 00193 00194 switch( mode ) 00195 { 00196 case SPI_MODE_3: {} 00197 return true; 00198 case SPI_MODE_0: {} 00199 case SPI_MODE_1: {} 00200 case SPI_MODE_2: {} 00201 default: 00202 ROS_ERROR( "bma180_parameters: bma180 can only be read in SPI_MODE_3" ); 00203 return false; 00204 } 00205 } 00206 00207 00208 /**********************************************************************/ 00209 /**********************************************************************/ 00210 bool BMA180Parameters::setAccelRange( accel_range new_range ) 00211 { 00212 // reassign the class's new range: 00213 accel_range_ = new_range; 00214 00215 // adjust the sensitivity accordingly: 00216 switch( new_range ) 00217 { 00218 case RANGE_1: 00219 sensitivity_ = 0.00013; 00220 break; 00221 case RANGE_1_5: 00222 sensitivity_ = 0.00019; 00223 break; 00224 case RANGE_2: 00225 sensitivity_ = 0.00025; 00226 break; 00227 case RANGE_3: 00228 sensitivity_ = 0.00038; 00229 break; 00230 case RANGE_4: 00231 sensitivity_ = 0.00050; 00232 break; 00233 case RANGE_8: 00234 sensitivity_ = 0.00099; 00235 break; 00236 case RANGE_16: 00237 sensitivity_ = 0.00198; 00238 break; 00239 default: // shouldn't happen because input argument is only an accel_range data type. 00240 ROS_ERROR( "bma180_parameters: invalid range setting." ); 00241 return false; 00242 } 00243 return true; 00244 00245 // NOTE: bma180::changeAccelRange() must be called after this method so that the sensor's range is adjusted as well! 00246 } 00247 00248 00249 /**********************************************************************/ 00250 /**********************************************************************/ 00251 void BMA180Parameters::setBandwidth( bandwidth bw ) 00252 { 00253 bandwidth_ = bw; 00254 } 00255 00256 00257 /**********************************************************************/ 00258 /**********************************************************************/ 00259 double BMA180Parameters::getSensitivity() 00260 { 00261 return sensitivity_; 00262 } 00263 00264 00265 /**********************************************************************/ 00266 /**********************************************************************/ 00267 void BMA180Parameters::setPreCalOffsets( bool choice ) 00268 { 00269 offsetsEnabled_ = choice; 00270 }