|
| 1 | +/* |
| 2 | + * mbed library program |
| 3 | + * LIS3DH MEMS motion sensor: 3-axis "nano" accelerometer, made by STMicroelectronics |
| 4 | + * http://www.st-japan.co.jp/web/jp/catalog/sense_power/FM89/SC444/PF250725 |
| 5 | + * |
| 6 | + * Copyright (c) 2014,'15,'17 Kenji Arai / JH1PJL |
| 7 | + * http://www.page.sannet.ne.jp/kenjia/index.html |
| 8 | + * http://mbed.org/users/kenjiArai/ |
| 9 | + * Created: July 14th, 2014 |
| 10 | + * Revised: August 23rd, 2017 |
| 11 | + */ |
| 12 | + |
| 13 | +#include "LIS3DH.h" |
| 14 | + |
| 15 | +LIS3DH::LIS3DH (PinName p_sda, PinName p_scl, |
| 16 | + uint8_t addr, uint8_t data_rate, uint8_t fullscale) |
| 17 | + : _i2c_p(new I2C(p_sda, p_scl)), _i2c(*_i2c_p) |
| 18 | +{ |
| 19 | + _i2c.frequency(400000); |
| 20 | + initialize (addr, data_rate, fullscale); |
| 21 | +} |
| 22 | + |
| 23 | +LIS3DH::LIS3DH (PinName p_sda, PinName p_scl, uint8_t addr) |
| 24 | + : _i2c_p(new I2C(p_sda, p_scl)), _i2c(*_i2c_p) |
| 25 | +{ |
| 26 | + _i2c.frequency(400000); |
| 27 | + initialize (addr, LIS3DH_DR_NR_LP_50HZ, LIS3DH_FS_8G); |
| 28 | +} |
| 29 | + |
| 30 | +LIS3DH::LIS3DH (I2C& p_i2c, |
| 31 | + uint8_t addr, uint8_t data_rate, uint8_t fullscale) |
| 32 | + : _i2c(p_i2c) |
| 33 | +{ |
| 34 | + _i2c.frequency(400000); |
| 35 | + initialize (addr, data_rate, fullscale); |
| 36 | +} |
| 37 | + |
| 38 | +LIS3DH::LIS3DH (I2C& p_i2c, uint8_t addr) |
| 39 | + : _i2c(p_i2c) |
| 40 | +{ |
| 41 | + _i2c.frequency(400000); |
| 42 | + initialize (addr, LIS3DH_DR_NR_LP_50HZ, LIS3DH_FS_8G); |
| 43 | +} |
| 44 | + |
| 45 | +void LIS3DH::initialize (uint8_t addr, uint8_t data_rate, uint8_t fullscale) |
| 46 | +{ |
| 47 | + // Check acc is available of not |
| 48 | + acc_addr = addr; |
| 49 | + dt[0] = LIS3DH_WHO_AM_I; |
| 50 | + _i2c.write(acc_addr, dt, 1, true); |
| 51 | + _i2c.read(acc_addr, dt, 1, false); |
| 52 | + if (dt[0] == I_AM_LIS3DH) { |
| 53 | + acc_ready = 1; |
| 54 | + } else { |
| 55 | + acc_ready = 0; |
| 56 | + return; // acc chip is NOT on I2C line then terminate |
| 57 | + } |
| 58 | + // Reg.1 |
| 59 | + dt[0] = LIS3DH_CTRL_REG1; |
| 60 | + dt[1] = 0x07; |
| 61 | + dt[1] |= data_rate << 4; |
| 62 | + _i2c.write(acc_addr, dt, 2, false); |
| 63 | + // Reg.4 |
| 64 | + dt[0] = LIS3DH_CTRL_REG4; |
| 65 | + dt[1] = 0x08; // High resolution |
| 66 | + dt[1] |= fullscale << 4; |
| 67 | + _i2c.write(acc_addr, dt, 2, false); |
| 68 | + switch (fullscale) { |
| 69 | + case LIS3DH_FS_2G: |
| 70 | + fs_factor = LIS3DH_SENSITIVITY_2G; |
| 71 | + break; |
| 72 | + case LIS3DH_FS_4G: |
| 73 | + fs_factor = LIS3DH_SENSITIVITY_4G; |
| 74 | + break; |
| 75 | + case LIS3DH_FS_8G: |
| 76 | + fs_factor = LIS3DH_SENSITIVITY_8G; |
| 77 | + break; |
| 78 | + case LIS3DH_FS_16G: |
| 79 | + fs_factor = LIS3DH_SENSITIVITY_16G; |
| 80 | + break; |
| 81 | + default: |
| 82 | + ; |
| 83 | + } |
| 84 | +} |
| 85 | + |
| 86 | +void LIS3DH::read_reg_data(char *data) |
| 87 | +{ |
| 88 | + // X,Y & Z |
| 89 | + // manual said that |
| 90 | + // In order to read multiple bytes, it is necessary to assert the most significant bit |
| 91 | + // of the subaddress field. |
| 92 | + // In other words, SUB(7) must be equal to ‘1’ while SUB(6-0) represents the address |
| 93 | + // of the first register to be read. |
| 94 | + dt[0] = LIS3DH_OUT_X_L | 0x80; |
| 95 | + _i2c.write(acc_addr, dt, 1, true); |
| 96 | + _i2c.read(acc_addr, data, 6, false); |
| 97 | +} |
| 98 | + |
| 99 | +void LIS3DH::read_mg_data(float *dt_usr) |
| 100 | +{ |
| 101 | + char data[6]; |
| 102 | + |
| 103 | + if (acc_ready == 0) { |
| 104 | + dt_usr[0] = 0; |
| 105 | + dt_usr[1] = 0; |
| 106 | + dt_usr[2] = 0; |
| 107 | + return; |
| 108 | + } |
| 109 | + read_reg_data(data); |
| 110 | + // change data type |
| 111 | +#if OLD_REV // Fixed bugs -> (1) unit is not mg but g (2) shift right 4bit = /16 |
| 112 | + dt_usr[0] = float(short((data[1] << 8) | data[0])) * fs_factor / 15; |
| 113 | + dt_usr[1] = float(short((data[3] << 8) | data[2])) * fs_factor / 15; |
| 114 | + dt_usr[2] = float(short((data[5] << 8) | data[4])) * fs_factor / 15; |
| 115 | +#else |
| 116 | + dt_usr[0] = float(short((data[1] << 8) | data[0]) >> 4) * fs_factor; |
| 117 | + dt_usr[1] = float(short((data[3] << 8) | data[2]) >> 4) * fs_factor; |
| 118 | + dt_usr[2] = float(short((data[5] << 8) | data[4]) >> 4) * fs_factor; |
| 119 | +#endif |
| 120 | +} |
| 121 | + |
| 122 | +void LIS3DH::read_data(float *dt_usr) |
| 123 | +{ |
| 124 | + char data[6]; |
| 125 | + |
| 126 | + if (acc_ready == 0) { |
| 127 | + dt_usr[0] = 0; |
| 128 | + dt_usr[1] = 0; |
| 129 | + dt_usr[2] = 0; |
| 130 | + return; |
| 131 | + } |
| 132 | + read_reg_data(data); |
| 133 | + // change data type |
| 134 | +#if OLD_REV // Fixed bugs -> shift right 4bit = /16 (not /15) |
| 135 | + dt_usr[0] = float(short((data[1] << 8) | data[0])) * fs_factor / 15 * GRAVITY; |
| 136 | + dt_usr[1] = float(short((data[3] << 8) | data[2])) * fs_factor / 15 * GRAVITY; |
| 137 | + dt_usr[2] = float(short((data[5] << 8) | data[4])) * fs_factor / 15 * GRAVITY; |
| 138 | +#else |
| 139 | + dt_usr[0] = float(short((data[1] << 8) | data[0]) >> 4) * fs_factor * GRAVITY; |
| 140 | + dt_usr[1] = float(short((data[3] << 8) | data[2]) >> 4) * fs_factor * GRAVITY; |
| 141 | + dt_usr[2] = float(short((data[5] << 8) | data[4]) >> 4) * fs_factor * GRAVITY; |
| 142 | +#endif |
| 143 | +} |
| 144 | + |
| 145 | +uint8_t LIS3DH::read_id() |
| 146 | +{ |
| 147 | + dt[0] = LIS3DH_WHO_AM_I; |
| 148 | + _i2c.write(acc_addr, dt, 1, true); |
| 149 | + _i2c.read(acc_addr, dt, 1, false); |
| 150 | + return (uint8_t)dt[0]; |
| 151 | +} |
| 152 | + |
| 153 | +uint8_t LIS3DH::data_ready() |
| 154 | +{ |
| 155 | + if (acc_ready == 1) { |
| 156 | + dt[0] = LIS3DH_STATUS_REG_AUX; |
| 157 | + _i2c.write(acc_addr, dt, 1, true); |
| 158 | + _i2c.read(acc_addr, dt, 1, false); |
| 159 | + if (!(dt[0] & 0x01)) { |
| 160 | + return 0; |
| 161 | + } |
| 162 | + } |
| 163 | + return 1; |
| 164 | +} |
| 165 | + |
| 166 | +void LIS3DH::frequency(int hz) |
| 167 | +{ |
| 168 | + _i2c.frequency(hz); |
| 169 | +} |
| 170 | + |
| 171 | +uint8_t LIS3DH::read_reg(uint8_t addr) |
| 172 | +{ |
| 173 | + if (acc_ready == 1) { |
| 174 | + dt[0] = addr; |
| 175 | + _i2c.write(acc_addr, dt, 1, true); |
| 176 | + _i2c.read(acc_addr, dt, 1, false); |
| 177 | + } else { |
| 178 | + dt[0] = 0xff; |
| 179 | + } |
| 180 | + return (uint8_t)dt[0]; |
| 181 | +} |
| 182 | + |
| 183 | +void LIS3DH::write_reg(uint8_t addr, uint8_t data) |
| 184 | +{ |
| 185 | + if (acc_ready == 1) { |
| 186 | + dt[0] = addr; |
| 187 | + dt[1] = data; |
| 188 | + _i2c.write(acc_addr, dt, 2, false); |
| 189 | + } |
| 190 | +} |
| 191 | + |
| 192 | + |
0 commit comments