L3GD20 & L3G4200D / STMicroelectronics / MEMS motion sensor, three-axis gyroscope library
Dependents: GR-PEACH_test_wo_rtos GR-PEACH_test_on_rtos_works_well
L3GD20.cpp
- Committer:
- kenjiArai
- Date:
- 2014-09-07
- Revision:
- 1:9475fd0e35ff
- Parent:
- 0:de66621e5370
- Child:
- 2:8073008f3036
File content as of revision 1:9475fd0e35ff:
/* * mbed library program * L3GD20 MEMS motion sensor: 3-axis digital gyroscope, made by STMicroelectronics * http://www.st.com/web/catalog/sense_power/FM89/SC1288 * /PF252443?sc=internet/analog/product/252443.jsp * L3G4200 DMEMS motion sensor: three-axis digital output gyroscope, made by STMicroelectronics * http://www.st.com/web/catalog/sense_power/FM89/SC1288 * /PF250373?sc=internet/analog/product/250373.jsp * * Copyright (c) 2014 Kenji Arai / JH1PJL * http://www.page.sannet.ne.jp/kenjia/index.html * http://mbed.org/users/kenjiArai/ * Created: July 13th, 2014 * Revised: September 7th, 2014 * * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, * INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE * AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, * DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ #include "L3GD20.h" L3GX_GYRO::L3GX_GYRO (PinName p_sda, PinName p_scl, uint8_t addr, uint8_t data_rate, uint8_t bandwidth, uint8_t fullscale) : i2c(p_sda, p_scl) { initialize (addr, data_rate, bandwidth, fullscale); } L3GX_GYRO::L3GX_GYRO (I2C& p_i2c, uint8_t addr, uint8_t data_rate, uint8_t bandwidth, uint8_t fullscale) : i2c(p_i2c) { i2c.frequency(400000); initialize (addr, data_rate, bandwidth, fullscale); } void L3GX_GYRO::initialize (uint8_t addr, uint8_t data_rate, uint8_t bandwidth, uint8_t fullscale) { // Check gyro is available of not gyro_addr = addr; dbf[0] = L3GX_WHO_AM_I; i2c.write(gyro_addr, dbf, 1); i2c.read(gyro_addr, dbf, 1); if (dbf[0] == I_AM_L3G4200D){ gyro_ready = 1; } else if (dbf[0] == I_AM_L3GD20){ gyro_ready = 1; } else { gyro_ready = 0; return; // gyro chip is NOT on I2C line then terminate this part } // Reg.1 dbf[0] = L3GX_CTRL_REG1; dbf[1] = 0x0f; dbf[1] |= data_rate << 6; dbf[1] |= bandwidth << 4; i2c.write(gyro_addr, dbf, 2); // Reg.3 dbf[0] = L3GX_CTRL_REG3; dbf[1] = 0x08; i2c.write(gyro_addr, dbf, 2); // Reg.4 dbf[0] = L3GX_CTRL_REG4; switch (fullscale){ case L3GX_FS_250DPS: fs_factor = 0.00875; dbf[1] = 0x80; break; case L3GX_FS_500DPS: fs_factor = 0.0175; dbf[1] = 0x90; break; case L3GX_FS_2000DPS: fs_factor = 0.07; dbf[1] = 0xa0; break; default: ; } i2c.write(gyro_addr, dbf, 2); } void L3GX_GYRO::read_data(float *dt) { char data[6]; if (gyro_ready == 0){ dt[0] = 0; dt[1] = 0; dt[2] = 0; return; } // X,Y & Z // manual said that // In order to read multiple bytes, it is necessary to assert the most significant bit // of the subaddress field. // In other words, SUB(7) must be equal to ‘1’ while SUB(6-0) represents the address // of the first register to be read. dbf[0] = L3GX_OUT_X_L | 0x80; i2c.write(gyro_addr, dbf, 1, true); i2c.read(gyro_addr, data, 6, false); // data normalization dt[0] = float(short(data[1] << 8 | data[0])) * fs_factor; dt[1] = float(short(data[3] << 8 | data[2])) * fs_factor; dt[2] = float(short(data[5] << 8 | data[4])) * fs_factor; } int8_t L3GX_GYRO::read_temp() { if (gyro_ready == 1){ dbf[0] = L3GX_OUT_TEMP; i2c.write(gyro_addr, dbf, 1); i2c.read(gyro_addr, dbf, 1); } else { dbf[0] = 99; } return (int8_t)dbf[0]; } uint8_t L3GX_GYRO::read_id() { dbf[0] = L3GX_WHO_AM_I; i2c.write(gyro_addr, dbf, 1); i2c.read(gyro_addr, dbf, 1); return (uint8_t)dbf[0]; } uint8_t L3GX_GYRO::data_ready() { if (gyro_ready == 1){ dbf[0] = L3GX_STATUS_REG; i2c.write(gyro_addr, dbf, 1); i2c.read(gyro_addr, dbf, 1); if (!(dbf[0] & 0x01)){ return 0; } } return 1;; } void L3GX_GYRO::frequency(int hz) { i2c.frequency(hz); } uint8_t L3GX_GYRO::read_reg(uint8_t addr) { if (gyro_ready == 1){ dbf[0] = addr; i2c.write(gyro_addr, dbf, 1); i2c.read(gyro_addr, dbf, 1); } else { dbf[0] = 0xff; } return (uint8_t)dbf[0]; } void L3GX_GYRO::write_reg(uint8_t addr, uint8_t data) { if (gyro_ready == 1){ dbf[0] = addr; dbf[1] = data; i2c.write(gyro_addr, dbf, 2); } }