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:
- 6:451811697299
- Parent:
- 5:81fc00bd76fe
- Child:
- 7:f5c0fe6ed71a
--- a/L3GD20.cpp Tue Feb 24 12:21:41 2015 +0000 +++ b/L3GD20.cpp Tue Feb 24 12:24:29 2015 +0000 @@ -1,5 +1,5 @@ /* - * mbed library program + * 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 @@ -10,7 +10,7 @@ * 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 + * Created: July 13th, 2014 * Revised: Feburary 24th, 2015 * * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, @@ -23,36 +23,41 @@ #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) { +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); +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) { +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; - dt[0] = L3GX_WHO_AM_I; + 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){ + if (dt[0] == I_AM_L3G4200D) { gyro_ready = 1; - } else if (dt[0] == I_AM_L3GD20){ + } else if (dt[0] == I_AM_L3GD20) { gyro_ready = 1; } else { gyro_ready = 0; @@ -64,48 +69,49 @@ dt[1] |= data_rate << 6; dt[1] |= bandwidth << 4; _i2c.write(gyro_addr, dt, 2, false); - // Reg.3 + // Reg.3 dt[0] = L3GX_CTRL_REG3; dt[1] = 0x08; _i2c.write(gyro_addr, dt, 2, false); // Reg.4 dt[0] = L3GX_CTRL_REG4; - switch (fullscale){ - case L3GX_FS_250DPS: - fs_factor = 0.00875; - dt[1] = 0x80; - break; - case L3GX_FS_500DPS: - fs_factor = 0.0175; - dt[1] = 0x90; - break; - case L3GX_FS_2000DPS: - fs_factor = 0.07; - dt[1] = 0xa0; - break; - default: - ; + switch (fullscale) { + case L3GX_FS_250DPS: + fs_factor = 0.00875; + dt[1] = 0x80; + break; + case L3GX_FS_500DPS: + fs_factor = 0.0175; + dt[1] = 0x90; + break; + case L3GX_FS_2000DPS: + fs_factor = 0.07; + dt[1] = 0xa0; + break; + default: + ; } _i2c.write(gyro_addr, dt, 2, false); } -void L3GX_GYRO::read_data(float *dt_usr) { -char data[6]; +void L3GX_GYRO::read_data(float *dt_usr) +{ + char data[6]; - if (gyro_ready == 0){ + if (gyro_ready == 0) { dt_usr[0] = 0; dt_usr[1] = 0; dt_usr[2] = 0; - return; + 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. + // 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. dt[0] = L3GX_OUT_X_L | 0x80; - _i2c.write(gyro_addr, dt, 1, true); + _i2c.write(gyro_addr, dt, 1, true); _i2c.read(gyro_addr, data, 6, false); // data normalization dt_usr[0] = float(short(data[1] << 8 | data[0])) * fs_factor; @@ -113,9 +119,10 @@ dt_usr[2] = float(short(data[5] << 8 | data[4])) * fs_factor; } -int8_t L3GX_GYRO::read_temp() { - if (gyro_ready == 1){ - dt[0] = L3GX_OUT_TEMP; +int8_t L3GX_GYRO::read_temp() +{ + if (gyro_ready == 1) { + dt[0] = L3GX_OUT_TEMP; _i2c.write(gyro_addr, dt, 1, true); _i2c.read(gyro_addr, dt, 1, false); } else { @@ -124,32 +131,36 @@ return (int8_t)dt[0]; } -uint8_t L3GX_GYRO::read_id() { - dt[0] = L3GX_WHO_AM_I; +uint8_t L3GX_GYRO::read_id() +{ + 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){ - dt[0] = L3GX_STATUS_REG; + +uint8_t L3GX_GYRO::data_ready() +{ + if (gyro_ready == 1) { + dt[0] = L3GX_STATUS_REG; _i2c.write(gyro_addr, dt, 1, true); _i2c.read(gyro_addr, dt, 1, false); - if (!(dt[0] & 0x01)){ + if (!(dt[0] & 0x01)) { return 0; } } return 1;; } -void L3GX_GYRO::frequency(int hz) { +void L3GX_GYRO::frequency(int hz) +{ _i2c.frequency(hz); } -uint8_t L3GX_GYRO::read_reg(uint8_t addr) { - if (gyro_ready == 1){ - dt[0] = addr; +uint8_t L3GX_GYRO::read_reg(uint8_t addr) +{ + if (gyro_ready == 1) { + dt[0] = addr; _i2c.write(gyro_addr, dt, 1, true); _i2c.read(gyro_addr, dt, 1, false); } else { @@ -158,10 +169,11 @@ return (uint8_t)dt[0]; } -void L3GX_GYRO::write_reg(uint8_t addr, uint8_t data) { - if (gyro_ready == 1){ +void L3GX_GYRO::write_reg(uint8_t addr, uint8_t data) +{ + if (gyro_ready == 1) { dt[0] = addr; - dt[1] = data; + dt[1] = data; _i2c.write(gyro_addr, dt, 2, false); } }