$search
00001 /* 00002 * Copyright (c) 2011, Robert Bosch LLC 00003 * All rights reserved. 00004 * 00005 * Redistribution and use in source and binary forms, with or without 00006 * modification, are permitted provided that the following conditions are met: 00007 * 00008 * * Redistributions of source code must retain the above copyright 00009 * notice, this list of conditions and the following disclaimer. 00010 * * Redistributions in binary form must reproduce the above copyright 00011 * notice, this list of conditions and the following disclaimer in the 00012 * documentation and/or other materials provided with the distribution. 00013 * * Neither the name of Robert Bosch LLC nor the names of its 00014 * contributors may be used to endorse or promote products derived from 00015 * this software without specific prior written permission. 00016 * 00017 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 00018 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 00019 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 00020 * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 00021 * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 00022 * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 00023 * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 00024 * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 00025 * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 00026 * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 00027 * POSSIBILITY OF SUCH DAMAGE. 00028 */ 00029 00030 /* 00031 * bma180.h 00032 * 00033 * Created on: Jul 26, 2011 00034 * Author: Lucas Marti, Robert Bosch LLC | 00035 * Editor: Nikhil Deshpande, Robert Bosch LLC | 00036 */ 00037 00038 #ifndef BMA180_H_ 00039 #define BMA180_H_ 00040 00041 #include "bma180/bma180meas.h" 00042 #include "bma180/bma180err.h" 00043 #include "bma180/bma180_calibrate.h" 00044 #include <list> 00045 #include <libsub.h> // sub20 device 00046 00047 //Define BMA180 specifics 00048 namespace bma180_cmd 00049 { 00050 const char FLAG_R = {0x80}; 00051 //Define BMA180 commands 00052 const char ADDRESS_ACCLXYZ = {0x02}; //read accel data 00053 const char ADDRESS_VER = {0x00}; //read version data 00054 const char ADDRESS_STATREG1 = {0x09}; 00055 const char ADDRESS_STATREG2 = {0x0A}; 00056 const char ADDRESS_STATREG3 = {0x0B}; 00057 const char ADDRESS_STATREG4 = {0x0C}; 00058 const char ADDRESS_CTRLREG0 = {0x0D}; 00059 const char ADDRESS_CTRLREG1 = {0x0E}; 00060 const char ADDRESS_CTRLREG2 = {0x0F}; 00061 const char ADDRESS_SOFTRESET = {0x10}; 00062 const char ADDRESS_EE_OFFSET_Z = {0x5A}; // | 00063 const char ADDRESS_EE_OFFSET_Y = {0x59}; // | 00064 const char ADDRESS_EE_OFFSET_X = {0x58}; // |-> THIS IS THE EEPROM 00065 const char ADDRESS_EE_OFFSET_T = {0x57}; // | 00066 const char ADDRESS_EE_OFFSET_LSB2 = {0x56}; // | 00067 const char ADDRESS_EE_OFFSET_LSB1 = {0x55}; // | 00068 const char ADDRESS_OFFSET_Z = {0x3A}; 00069 const char ADDRESS_OFFSET_Y = {0x39}; 00070 const char ADDRESS_OFFSET_X = {0x38}; 00071 const char ADDRESS_OFFSET_T = {0x37}; 00072 const char ADDRESS_OFFSET_LSB2 = {0x36}; 00073 const char ADDRESS_OFFSET_LSB1 = {0x35}; 00074 const char ADDRESS_EE_GAIN_Z = {0x54}; 00075 //----------------------------------- 00076 const char ADDRESS_BWTCS = {0x20}; 00077 const char ADDRESS_RANGE = {0x35}; 00078 //----------------------------------- 00079 const double dFULLRANGELOOKUP[] = {1.0, 1.5, 2.0, 3.0, 4.0, 8.0, 16.0}; 00080 const char COMMAND_FULLSCALE_G[] = {0x00, 0x01, 0x02, 0x03, 0x04, 0x05, 0x06}; 00081 //----------------------------------- 00082 const double dBWLOOKUP[] = {10, 20, 40, 75, 150, 300, 600, 1200}; 00083 const char COMMAND_BANDWIDTH_HZ[] = {0x00, 0x01, 0x02, 0x03, 0x04, 0x05, 0x06, 0x07}; 00084 //----------------------------------- 00085 const char COMMAND_SOFTRESET = {0xB6}; 00086 const char COMMAND_SET_EEW = {0x01}; 00087 //----------------------------------- 00088 enum eSensorType {eACCEL, eTEMP, eBIAS_X, eBIAS_Y, eBIAS_Z}; 00089 //----------------------------------- 00090 const unsigned short MAX_NUM_SENSORS = 5; //Defined by the Xdimax Unit (which supports 5 sensors per box) 00091 //----------------------------------- 00092 const double dDEFAULT_MAXACC_g = 16.0; 00093 const std::string sSUB20SERIAL("0651"); 00094 const double dDEFAULT_BANDWIDTH_Hz = 300; 00095 const double dDEFAULT_RATE_Hz = 500; 00096 const bool bDEFAULT_CALIBRATE = false; 00097 const double dCALIB_ACCURACY = 0.05; 00098 //----------------------------------- 00099 const unsigned short uSERIALNUMLENGTH = 20; 00100 //----------------------------------- 00101 const short iDEFAULT_CALIBCHIPSELECT= 0; //this is out of range and ensures that no calibration is executed in case of error 00102 } 00103 00104 //Define measurement output 00105 struct OneBma180Meas 00106 { 00107 bool bMeasAvailable; //indicates true if a valid measurement is available 00108 int iNumAccels; 00109 double dAccX[bma180_cmd::MAX_NUM_SENSORS]; 00110 double dAccY[bma180_cmd::MAX_NUM_SENSORS]; 00111 double dAccZ[bma180_cmd::MAX_NUM_SENSORS]; 00112 double dTemp; 00113 ros::Time dtomeas; //time tag measurement immediately in case of other delays 00114 std::string serial_number; 00115 int chip_select[bma180_cmd::MAX_NUM_SENSORS]; 00116 }; 00117 00118 00120 00126 class Bma180 00127 { 00128 public: 00130 00142 Bma180( double max_acceleration_g, double dBandwidth_Hz, bool bCalibrate, double dRate_Hz, std::string sSub20Serial ); 00143 00145 00149 ~Bma180(); 00150 00152 00162 void GetMeasurements( std::list<OneBma180Meas>& ); 00163 00164 private: 00165 00166 // structure for one bma180 configuration 00167 struct OneBma180Config 00168 { 00169 double dFullScaleRange; 00170 double dSensorBandwidth; 00171 bool bConfigured; 00172 unsigned short uiBiasImageX; 00173 unsigned short uiBiasImageY; 00174 unsigned short uiBiasImageZ; 00175 double iNumOfCalibMeas; 00176 }; 00177 00178 // structure for one Sub20 device configuration 00179 struct OneSub20Config 00180 { 00181 std::string strSub20Serial; 00182 sub_handle handle_subdev; 00183 sub_device subdev; 00184 bool bSubSPIConfigured; 00185 OneBma180Config Bma180Cluster[bma180_cmd::MAX_NUM_SENSORS]; 00186 }; 00187 std::list<OneSub20Config> Sub20Device_list; 00188 00189 // internal calibration flags 00190 bool bExecuteCalibration; 00191 bool bCalibrationCompleted; 00192 bma180_calibrate calib_bma180; 00193 00194 // internal selected accelerometer range and bandwidth 00195 char chMaxAccRange_selected; 00196 char chBW_selected; 00197 int iCalib_CS; 00198 00199 // internal Sub20 device data 00200 sub_handle subhndl; //handle for Sub20 device 00201 bool bSubDeviceOpen; //initialization flag 00202 bool bSubDeviceConfigured; //verify if subdevice is configured 00203 std::string serial_number; //Serial number of Sub20 device 00204 00206 double bma180data_to_double( char, char, bma180_cmd::eSensorType, int *, double ); 00207 00209 unsigned short bma180data_to_uint( char, char, bma180_cmd::eSensorType ); 00210 00212 bool read_biassettings( sub_handle, int, unsigned short*, unsigned short*, unsigned short* ); 00213 00215 bool set_biassettings( sub_handle, int, unsigned short, unsigned short, unsigned short, bool ); 00216 00218 bool read_byte_eeprom_sub20( char, char*, unsigned short, sub_handle ); 00219 00221 bool write_bit_eeprom_sub20( char, unsigned short, unsigned short, char, unsigned short, sub_handle ); 00222 00224 void confsens_on_sub20( OneSub20Config*, char, char ); 00225 00227 void set_calibstatus( bool bCalibrate ); 00228 }; 00229 00230 #endif /* MULT_BMA180_H_ */ 00231