L3GD20 & L3G4200D / STMicroelectronics / MEMS motion sensor, three-axis gyroscope library
Dependents: GR-PEACH_test_wo_rtos GR-PEACH_test_on_rtos_works_well
Diff: L3GD20.cpp
- Revision:
- 5:81fc00bd76fe
- Parent:
- 4:7d0cec583aa3
- Child:
- 6:451811697299
diff -r 7d0cec583aa3 -r 81fc00bd76fe L3GD20.cpp --- a/L3GD20.cpp Sat Dec 27 07:43:49 2014 +0000 +++ b/L3GD20.cpp Tue Feb 24 12:21:41 2015 +0000 @@ -7,11 +7,11 @@ * http://www.st.com/web/catalog/sense_power/FM89/SC1288 * /PF250373?sc=internet/analog/product/250373.jsp * - * Copyright (c) 2014 Kenji Arai / JH1PJL + * Copyright (c) 2014,'15 Kenji Arai / JH1PJL * http://www.page.sannet.ne.jp/kenjia/index.html * http://mbed.org/users/kenjiArai/ * Created: July 13th, 2014 - * Revised: December 27th, 2014 + * Revised: Feburary 24th, 2015 * * 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 @@ -23,68 +23,79 @@ #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) { + uint8_t addr, uint8_t data_rate, uint8_t bandwidth, uint8_t fullscale) : _i2c(p_sda, p_scl) { + _i2c.frequency(400000); initialize (addr, data_rate, bandwidth, fullscale); } +L3GX_GYRO::L3GX_GYRO (PinName p_sda, PinName p_scl, uint8_t addr) : _i2c(p_sda, p_scl) { + _i2c.frequency(400000); + initialize (addr, L3GX_DR_95HZ, L3GX_BW_HI, L3GX_FS_250DPS); +} + 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); + 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); } +L3GX_GYRO::L3GX_GYRO (I2C& p_i2c, uint8_t addr) : _i2c(p_i2c) { + _i2c.frequency(400000); + initialize (addr, L3GX_DR_95HZ, L3GX_BW_HI, L3GX_FS_250DPS); +} + 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){ + dt[0] = L3GX_WHO_AM_I; + _i2c.write(gyro_addr, dt, 1, true); + _i2c.read(gyro_addr, dt, 1, false); + if (dt[0] == I_AM_L3G4200D){ gyro_ready = 1; - } else if (dbf[0] == I_AM_L3GD20){ + } else if (dt[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); + dt[0] = L3GX_CTRL_REG1; + dt[1] = 0x0f; + dt[1] |= data_rate << 6; + dt[1] |= bandwidth << 4; + _i2c.write(gyro_addr, dt, 2, false); // Reg.3 - dbf[0] = L3GX_CTRL_REG3; - dbf[1] = 0x08; - i2c.write(gyro_addr, dbf, 2); + dt[0] = L3GX_CTRL_REG3; + dt[1] = 0x08; + _i2c.write(gyro_addr, dt, 2, false); // Reg.4 - dbf[0] = L3GX_CTRL_REG4; + dt[0] = L3GX_CTRL_REG4; switch (fullscale){ case L3GX_FS_250DPS: fs_factor = 0.00875; - dbf[1] = 0x80; + dt[1] = 0x80; break; case L3GX_FS_500DPS: fs_factor = 0.0175; - dbf[1] = 0x90; + dt[1] = 0x90; break; case L3GX_FS_2000DPS: fs_factor = 0.07; - dbf[1] = 0xa0; + dt[1] = 0xa0; break; default: ; } - i2c.write(gyro_addr, dbf, 2); + _i2c.write(gyro_addr, dt, 2, false); } -void L3GX_GYRO::read_data(float *dt) { +void L3GX_GYRO::read_data(float *dt_usr) { char data[6]; if (gyro_ready == 0){ - dt[0] = 0; - dt[1] = 0; - dt[2] = 0; + dt_usr[0] = 0; + dt_usr[1] = 0; + dt_usr[2] = 0; return; } // X,Y & Z @@ -93,39 +104,39 @@ // 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); + dt[0] = L3GX_OUT_X_L | 0x80; + _i2c.write(gyro_addr, dt, 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; + dt_usr[0] = float(short(data[1] << 8 | data[0])) * fs_factor; + dt_usr[1] = float(short(data[3] << 8 | data[2])) * fs_factor; + dt_usr[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); + dt[0] = L3GX_OUT_TEMP; + _i2c.write(gyro_addr, dt, 1, true); + _i2c.read(gyro_addr, dt, 1, false); } else { - dbf[0] = 99; + dt[0] = 99; } - return (int8_t)dbf[0]; + return (int8_t)dt[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]; + dt[0] = L3GX_WHO_AM_I; + _i2c.write(gyro_addr, dt, 1, true); + _i2c.read(gyro_addr, dt, 1, false); + return (uint8_t)dt[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)){ + dt[0] = L3GX_STATUS_REG; + _i2c.write(gyro_addr, dt, 1, true); + _i2c.read(gyro_addr, dt, 1, false); + if (!(dt[0] & 0x01)){ return 0; } } @@ -133,24 +144,24 @@ } void L3GX_GYRO::frequency(int hz) { - i2c.frequency(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); + dt[0] = addr; + _i2c.write(gyro_addr, dt, 1, true); + _i2c.read(gyro_addr, dt, 1, false); } else { - dbf[0] = 0xff; + dt[0] = 0xff; } - return (uint8_t)dbf[0]; + return (uint8_t)dt[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); + dt[0] = addr; + dt[1] = data; + _i2c.write(gyro_addr, dt, 2, false); } }