$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 const char chFLAG_R = {0x80}; 00050 //Define BMA180 commands 00051 const char chADR_ACCLXYZ = {0x02}; //read accel data 00052 const char chADR_VER = {0x00}; //read version data 00053 const char chADR_STATREG1 = {0x09}; 00054 const char chADR_STATREG2 = {0x0A}; 00055 const char chADR_STATREG3 = {0x0B}; 00056 const char chADR_STATREG4 = {0x0C}; 00057 const char chADR_CTRLREG0 = {0x0D}; 00058 const char chADR_CTRLREG1 = {0x0E}; 00059 const char chADR_CTRLREG2 = {0x0F}; 00060 const char chADR_SOFTRESET = {0x10}; 00061 const char chADR_EE_OFFSET_Z = {0x5A}; // | 00062 const char chADR_EE_OFFSET_Y = {0x59}; // | 00063 const char chADR_EE_OFFSET_X = {0x58}; // |-> THIS IS THE EEPROM 00064 const char chADR_EE_OFFSET_T = {0x57}; // | 00065 const char chADR_EE_OFFSET_LSB2 = {0x56}; // | 00066 const char chADR_EE_OFFSET_LSB1 = {0x55}; // | 00067 const char chADR_OFFSET_Z = {0x3A}; 00068 const char chADR_OFFSET_Y = {0x39}; 00069 const char chADR_OFFSET_X = {0x38}; 00070 const char chADR_OFFSET_T = {0x37}; 00071 const char chADR_OFFSET_LSB2 = {0x36}; 00072 const char chADR_OFFSET_LSB1 = {0x35}; 00073 const char chADR_EE_GAIN_Z = {0x54}; 00074 //----------------------------------- 00075 const char chADR_BWTCS = {0x20}; 00076 const char chADR_RANGE = {0x35}; 00077 //----------------------------------- 00078 const double dFULLRANGELOOKUP[] = {1.0, 1.5, 2.0, 3.0, 4.0, 8.0, 16.0}; 00079 const char chCMD_FULLSCALE_G[]= {0x00, 0x01, 0x02, 0x03, 0x04, 0x05, 0x06}; 00080 //----------------------------------- 00081 const double dBWLOOKUP[] = {10, 20, 40, 75, 150, 300, 600, 1200}; 00082 const char chCMD_BANDWIDTH_HZ[]= {0x00, 0x01, 0x02, 0x03, 0x04, 0x05, 0x06, 0x07}; 00083 //----------------------------------- 00084 const char chCMD_SOFTRESET = {0xB6}; 00085 const char chCMD_SET_EEW = {0x01}; 00086 //----------------------------------- 00087 enum eSensorType {eACCEL, eTEMP, eBIAS_X, eBIAS_Y, eBIAS_Z}; 00088 //----------------------------------- 00089 const unsigned short iMAXNUM_OF_SENSORS = 5; //Defined by the Xdimax Unit (which supports 5 sensors per box) 00090 //----------------------------------- 00091 const double dDEFAULT_MAXACC_g = 16; 00092 const std::string sSUB20SERIAL("0651"); 00093 const double dDEFAULT_BANDWIDTH_Hz = 300; 00094 const double dDEFAULT_RATE_Hz = 500; 00095 const bool bDEFAULT_CALIBRATE = false; 00096 const double dCALIB_ACCURACY = 0.05; 00097 //----------------------------------- 00098 const unsigned short uSERIALNUMLENGTH = 20; 00099 //----------------------------------- 00100 const short iDEFAULT_CALIBCHIPSELECT= 0; //this is out of range and ensures that no calibration is executed in case of error 00101 } 00102 00103 //Define measurement output 00104 struct OneBma180Meas { 00105 bool bMeasAvailable; //indicates true if a valid measurement is available 00106 int iNumAccels; 00107 double dAccX[bma180_cmd::iMAXNUM_OF_SENSORS]; 00108 double dAccY[bma180_cmd::iMAXNUM_OF_SENSORS]; 00109 double dAccZ[bma180_cmd::iMAXNUM_OF_SENSORS]; 00110 double dTemp; 00111 ros::Time dtomeas; //time tag measurement immediately in case of other delays 00112 std::string strSerial; 00113 int iChipSelect[bma180_cmd::iMAXNUM_OF_SENSORS]; 00114 }; 00115 00116 00118 00124 class Bma180 { 00125 public: 00127 00139 Bma180(double, double, bool, double, std::string); 00140 00142 00146 ~Bma180(); 00147 00149 00159 void GetMeasurements( std::list<OneBma180Meas>& ); 00160 00161 private: 00162 // structure for one bma180 configuration 00163 struct OneBma180Config { 00164 double dFullScaleRange; 00165 double dSensorBandwidth; 00166 bool bConfigured; 00167 unsigned short uiBiasImageX; 00168 unsigned short uiBiasImageY; 00169 unsigned short uiBiasImageZ; 00170 double iNumOfCalibMeas; 00171 }; 00172 // structure for one Sub20 device configuration 00173 struct OneSub20Config { 00174 std::string strSub20Serial; 00175 sub_handle handle_subdev; 00176 sub_device subdev; 00177 bool bSubSPIConfigured; 00178 OneBma180Config Bma180Cluster[bma180_cmd::iMAXNUM_OF_SENSORS]; 00179 }; 00180 std::list<OneSub20Config> Sub20Device_list; 00181 00182 // internal calibration flags 00183 bool bExecuteCalibration; 00184 bool bCalibrationCompleted; 00185 bma180_calibrate calib_bma180; 00186 00187 // internal selected accelerometer range and bandwidth 00188 char chMaxAccRange_selected; 00189 char chBW_selected; 00190 int iCalib_CS; 00191 00192 // internal Sub20 device data 00193 sub_handle subhndl; //handle for subdevice 00194 bool bSubDeviceOpen; //initialization flag 00195 bool bSubDeviceConfigured; //verify if subdevice is configured 00196 std::string strSerial; //Serial number of SUB20 device 00197 00199 double bma180data_to_double(char, char, bma180_cmd::eSensorType, int *, double); 00200 00202 unsigned short bma180data_to_uint(char, char, bma180_cmd::eSensorType ); 00203 00205 bool read_biassettings(sub_handle, int, unsigned short*, unsigned short*, unsigned short*); 00206 00208 bool set_biassettings(sub_handle, int, unsigned short, unsigned short, unsigned short, bool); 00209 00211 bool read_byte_eeprom_sub20(char, char*, unsigned short, sub_handle); 00212 00214 bool write_bit_eeprom_sub20(char, unsigned short, unsigned short, char, unsigned short, sub_handle); 00215 00217 void confsens_on_sub20(OneSub20Config*, char, char); 00218 00220 void set_calibstatus(bool bCalibrate); 00221 }; 00222 00223 #endif /* MULT_BMA180_H_ */ 00224