LIS3DH / STMicroelectronics / MEMS motion sensor, 3-axis accelerometer library
Embed:
(wiki syntax)
Show/hide line numbers
LIS3DH.cpp
00001 /* 00002 * mbed library program 00003 * LIS3DH MEMS motion sensor: 3-axis "nano" accelerometer, made by STMicroelectronics 00004 * http://www.st-japan.co.jp/web/jp/catalog/sense_power/FM89/SC444/PF250725 00005 * 00006 * Copyright (c) 2014,'15,'17 Kenji Arai / JH1PJL 00007 * http://www.page.sannet.ne.jp/kenjia/index.html 00008 * http://mbed.org/users/kenjiArai/ 00009 * Created: July 14th, 2014 00010 * Revised: August 23rd, 2017 00011 */ 00012 00013 #include "LIS3DH.h" 00014 00015 LIS3DH::LIS3DH (PinName p_sda, PinName p_scl, 00016 uint8_t addr, uint8_t data_rate, uint8_t fullscale) 00017 : _i2c_p(new I2C(p_sda, p_scl)), _i2c(*_i2c_p) 00018 { 00019 _i2c.frequency(400000); 00020 initialize (addr, data_rate, fullscale); 00021 } 00022 00023 LIS3DH::LIS3DH (PinName p_sda, PinName p_scl, uint8_t addr) 00024 : _i2c_p(new I2C(p_sda, p_scl)), _i2c(*_i2c_p) 00025 { 00026 _i2c.frequency(400000); 00027 initialize (addr, LIS3DH_DR_NR_LP_50HZ, LIS3DH_FS_8G); 00028 } 00029 00030 LIS3DH::LIS3DH (I2C& p_i2c, 00031 uint8_t addr, uint8_t data_rate, uint8_t fullscale) 00032 : _i2c(p_i2c) 00033 { 00034 _i2c.frequency(400000); 00035 initialize (addr, data_rate, fullscale); 00036 } 00037 00038 LIS3DH::LIS3DH (I2C& p_i2c, uint8_t addr) 00039 : _i2c(p_i2c) 00040 { 00041 _i2c.frequency(400000); 00042 initialize (addr, LIS3DH_DR_NR_LP_50HZ, LIS3DH_FS_8G); 00043 } 00044 00045 void LIS3DH::initialize (uint8_t addr, uint8_t data_rate, uint8_t fullscale) 00046 { 00047 // Check acc is available of not 00048 acc_addr = addr; 00049 dt[0] = LIS3DH_WHO_AM_I; 00050 _i2c.write(acc_addr, dt, 1, true); 00051 _i2c.read(acc_addr, dt, 1, false); 00052 if (dt[0] == I_AM_LIS3DH) { 00053 acc_ready = 1; 00054 } else { 00055 acc_ready = 0; 00056 return; // acc chip is NOT on I2C line then terminate 00057 } 00058 // Reg.1 00059 dt[0] = LIS3DH_CTRL_REG1; 00060 dt[1] = 0x07; 00061 dt[1] |= data_rate << 4; 00062 _i2c.write(acc_addr, dt, 2, false); 00063 // Reg.4 00064 dt[0] = LIS3DH_CTRL_REG4; 00065 dt[1] = 0x08; // High resolution 00066 dt[1] |= fullscale << 4; 00067 _i2c.write(acc_addr, dt, 2, false); 00068 switch (fullscale) { 00069 case LIS3DH_FS_2G: 00070 fs_factor = LIS3DH_SENSITIVITY_2G; 00071 break; 00072 case LIS3DH_FS_4G: 00073 fs_factor = LIS3DH_SENSITIVITY_4G; 00074 break; 00075 case LIS3DH_FS_8G: 00076 fs_factor = LIS3DH_SENSITIVITY_8G; 00077 break; 00078 case LIS3DH_FS_16G: 00079 fs_factor = LIS3DH_SENSITIVITY_16G; 00080 break; 00081 default: 00082 ; 00083 } 00084 } 00085 00086 void LIS3DH::read_reg_data(char *data) 00087 { 00088 // X,Y & Z 00089 // manual said that 00090 // In order to read multiple bytes, it is necessary to assert the most significant bit 00091 // of the subaddress field. 00092 // In other words, SUB(7) must be equal to ‘1’ while SUB(6-0) represents the address 00093 // of the first register to be read. 00094 dt[0] = LIS3DH_OUT_X_L | 0x80; 00095 _i2c.write(acc_addr, dt, 1, true); 00096 _i2c.read(acc_addr, data, 6, false); 00097 } 00098 00099 void LIS3DH::read_mg_data(float *dt_usr) 00100 { 00101 char data[6]; 00102 00103 if (acc_ready == 0) { 00104 dt_usr[0] = 0; 00105 dt_usr[1] = 0; 00106 dt_usr[2] = 0; 00107 return; 00108 } 00109 read_reg_data(data); 00110 // change data type 00111 #if OLD_REV // Fixed bugs -> (1) unit is not mg but g (2) shift right 4bit = /16 00112 dt_usr[0] = float(short((data[1] << 8) | data[0])) * fs_factor / 15; 00113 dt_usr[1] = float(short((data[3] << 8) | data[2])) * fs_factor / 15; 00114 dt_usr[2] = float(short((data[5] << 8) | data[4])) * fs_factor / 15; 00115 #else 00116 dt_usr[0] = float(short((data[1] << 8) | data[0]) >> 4) * fs_factor; 00117 dt_usr[1] = float(short((data[3] << 8) | data[2]) >> 4) * fs_factor; 00118 dt_usr[2] = float(short((data[5] << 8) | data[4]) >> 4) * fs_factor; 00119 #endif 00120 } 00121 00122 void LIS3DH::read_data(float *dt_usr) 00123 { 00124 char data[6]; 00125 00126 if (acc_ready == 0) { 00127 dt_usr[0] = 0; 00128 dt_usr[1] = 0; 00129 dt_usr[2] = 0; 00130 return; 00131 } 00132 read_reg_data(data); 00133 // change data type 00134 #if OLD_REV // Fixed bugs -> shift right 4bit = /16 (not /15) 00135 dt_usr[0] = float(short((data[1] << 8) | data[0])) * fs_factor / 15 * GRAVITY; 00136 dt_usr[1] = float(short((data[3] << 8) | data[2])) * fs_factor / 15 * GRAVITY; 00137 dt_usr[2] = float(short((data[5] << 8) | data[4])) * fs_factor / 15 * GRAVITY; 00138 #else 00139 dt_usr[0] = float(short((data[1] << 8) | data[0]) >> 4) * fs_factor * GRAVITY; 00140 dt_usr[1] = float(short((data[3] << 8) | data[2]) >> 4) * fs_factor * GRAVITY; 00141 dt_usr[2] = float(short((data[5] << 8) | data[4]) >> 4) * fs_factor * GRAVITY; 00142 #endif 00143 } 00144 00145 uint8_t LIS3DH::read_id() 00146 { 00147 dt[0] = LIS3DH_WHO_AM_I; 00148 _i2c.write(acc_addr, dt, 1, true); 00149 _i2c.read(acc_addr, dt, 1, false); 00150 return (uint8_t)dt[0]; 00151 } 00152 00153 uint8_t LIS3DH::data_ready() 00154 { 00155 if (acc_ready == 1) { 00156 dt[0] = LIS3DH_STATUS_REG_AUX; 00157 _i2c.write(acc_addr, dt, 1, true); 00158 _i2c.read(acc_addr, dt, 1, false); 00159 if (!(dt[0] & 0x01)) { 00160 return 0; 00161 } 00162 } 00163 return 1; 00164 } 00165 00166 void LIS3DH::frequency(int hz) 00167 { 00168 _i2c.frequency(hz); 00169 } 00170 00171 uint8_t LIS3DH::read_reg(uint8_t addr) 00172 { 00173 if (acc_ready == 1) { 00174 dt[0] = addr; 00175 _i2c.write(acc_addr, dt, 1, true); 00176 _i2c.read(acc_addr, dt, 1, false); 00177 } else { 00178 dt[0] = 0xff; 00179 } 00180 return (uint8_t)dt[0]; 00181 } 00182 00183 void LIS3DH::write_reg(uint8_t addr, uint8_t data) 00184 { 00185 if (acc_ready == 1) { 00186 dt[0] = addr; 00187 dt[1] = data; 00188 _i2c.write(acc_addr, dt, 2, false); 00189 } 00190 } 00191 00192
Generated on Fri Jul 15 2022 18:59:24 by 1.7.2