00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032 #ifndef __BMA180__
00033 #define __BMA180__
00034
00035 #include "bma180/bma180meas.h"
00036 #include "bma180/bma180err.h"
00037 #include "bma180/bma180_calibrate.h"
00038 #include <list>
00039 #include <libsub.h>
00040
00041
00042 namespace bma180_cmd {
00043 const char chFLAG_R = {0x80};
00044
00045 const char chADR_ACCLXYZ = {0x02};
00046 const char chADR_VER = {0x00};
00047 const char chADR_STATREG1 = {0x09};
00048 const char chADR_STATREG2 = {0x0A};
00049 const char chADR_STATREG3 = {0x0B};
00050 const char chADR_STATREG4 = {0x0C};
00051 const char chADR_CTRLREG0 = {0x0D};
00052 const char chADR_CTRLREG1 = {0x0E};
00053 const char chADR_CTRLREG2 = {0x0F};
00054 const char chADR_SOFTRESET = {0x10};
00055 const char chADR_EE_OFFSET_Z = {0x5A};
00056 const char chADR_EE_OFFSET_Y = {0x59};
00057 const char chADR_EE_OFFSET_X = {0x58};
00058 const char chADR_EE_OFFSET_T = {0x57};
00059 const char chADR_EE_OFFSET_LSB2 = {0x56};
00060 const char chADR_EE_OFFSET_LSB1 = {0x55};
00061 const char chADR_OFFSET_Z = {0x3A};
00062 const char chADR_OFFSET_Y = {0x39};
00063 const char chADR_OFFSET_X = {0x38};
00064 const char chADR_OFFSET_T = {0x37};
00065 const char chADR_OFFSET_LSB2 = {0x36};
00066 const char chADR_OFFSET_LSB1 = {0x35};
00067 const char chADR_EE_GAIN_Z = {0x54};
00068
00069 const char chADR_BWTCS = {0x20};
00070 const char chADR_RANGE = {0x35};
00071
00072 const double dFULLRANGELOOKUP[] = {1.0, 1.5, 2.0, 3.0, 4.0, 8.0, 16.0};
00073 const char chCMD_FULLSCALE_G[]= {0x00, 0x01, 0x02, 0x03, 0x04, 0x05, 0x06};
00074
00075 const double dBWLOOKUP[] = {10, 20, 40, 75, 150, 300, 600, 1200};
00076 const char chCMD_BANDWIDTH_HZ[]= {0x00, 0x01, 0x02, 0x03, 0x04, 0x05, 0x06, 0x07};
00077
00078 const char chCMD_SOFTRESET = {0xB6};
00079 const char chCMD_SET_EEW = {0x01};
00080
00081 enum eSensorType {eACCEL, eTEMP, eBIAS_X, eBIAS_Y, eBIAS_Z};
00082
00083 const unsigned short iMAXNUM_OF_SENSORS = 5;
00084
00085 const double dDEFAULT_MAXACC_g = 16;
00086 const double dDEFAULT_BANDWIDTH_Hz = 300;
00087 const double dDEFAULT_RATE_Hz = 500;
00088 const bool bDEFAULT_CALIBRATE = false;
00089 const double dCALIB_ACCURACY = 0.05;
00090
00091 const unsigned short uSERIALNUMLENGTH = 20;
00092
00093 const short iDEFAULT_CALIBCHIPSELECT= 0;
00094 }
00095
00096
00097 struct OneBma180Meas {
00098 bool bMeasAvailable;
00099 double dAccX;
00100 double dAccY;
00101 double dAccZ;
00102 double dTemp;
00103 ros::Time dtomeas;
00104 std::string strSerial;
00105 int iChipSelect;
00106 };
00107
00108
00110
00116 class Bma180 {
00117 public:
00119
00129 Bma180(double, double, bool);
00130
00132
00136 ~Bma180();
00137
00139
00149 void GetMeasurements( std::list<OneBma180Meas>& );
00150
00151 private:
00152
00153 struct OneBma180Config {
00154 double dFullScaleRange;
00155 double dSensorBandwidth;
00156 bool bConfigured;
00157 unsigned short uiBiasImageX;
00158 unsigned short uiBiasImageY;
00159 unsigned short uiBiasImageZ;
00160 double iNumOfCalibMeas;
00161 };
00162
00163 struct OneSub20Config {
00164 std::string strSub20Serial;
00165 sub_handle handle_subdev;
00166 sub_device subdev;
00167 bool bSubDevConfigured;
00168 OneBma180Config Bma180Cluster[bma180_cmd::iMAXNUM_OF_SENSORS];
00169 };
00170 std::list<OneSub20Config> Sub20Device_list;
00171
00172
00173 bool bExecuteCalibration;
00174 bool bCalibrationCompleted;
00175 bma180_calibrate calib_bma180;
00176
00177
00178 char chMaxAccRange_selected;
00179 char chBW_selected;
00180 int iCalib_CS;
00181
00182
00183 sub_handle fd;
00184 bool bSubDeviceOpen;
00185 bool bSubDeviceConfigured;
00186 std::string strSerial;
00187
00189 double bma180data_to_double(char, char, bma180_cmd::eSensorType, double);
00190
00192 unsigned short bma180data_to_uint(char, char, bma180_cmd::eSensorType );
00193
00195 bool read_biassettings(sub_handle, int, unsigned short*, unsigned short*, unsigned short*);
00196
00198 bool set_biassettings(sub_handle, int, unsigned short, unsigned short, unsigned short, bool);
00199
00201 bool read_byte_eeprom_sub20(char, char*, unsigned short, sub_handle);
00202
00204 bool write_bit_eeprom_sub20(char, unsigned short, unsigned short, char, unsigned short, sub_handle);
00205
00207 void confsens_on_sub20(OneSub20Config*, char, char);
00208
00210 void set_calibstatus(bool bCalibrate);
00211 };
00212
00213 #endif