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@6:451811697299, 2015-02-24 (annotated)
- Committer:
- kenjiArai
- Date:
- Tue Feb 24 12:24:29 2015 +0000
- Revision:
- 6:451811697299
- Parent:
- 5:81fc00bd76fe
- Child:
- 7:f5c0fe6ed71a
changed format
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
kenjiArai | 0:de66621e5370 | 1 | /* |
kenjiArai | 6:451811697299 | 2 | * mbed library program |
kenjiArai | 0:de66621e5370 | 3 | * L3GD20 MEMS motion sensor: 3-axis digital gyroscope, made by STMicroelectronics |
kenjiArai | 0:de66621e5370 | 4 | * http://www.st.com/web/catalog/sense_power/FM89/SC1288 |
kenjiArai | 0:de66621e5370 | 5 | * /PF252443?sc=internet/analog/product/252443.jsp |
kenjiArai | 0:de66621e5370 | 6 | * L3G4200 DMEMS motion sensor: three-axis digital output gyroscope, made by STMicroelectronics |
kenjiArai | 0:de66621e5370 | 7 | * http://www.st.com/web/catalog/sense_power/FM89/SC1288 |
kenjiArai | 0:de66621e5370 | 8 | * /PF250373?sc=internet/analog/product/250373.jsp |
kenjiArai | 0:de66621e5370 | 9 | * |
kenjiArai | 5:81fc00bd76fe | 10 | * Copyright (c) 2014,'15 Kenji Arai / JH1PJL |
kenjiArai | 0:de66621e5370 | 11 | * http://www.page.sannet.ne.jp/kenjia/index.html |
kenjiArai | 0:de66621e5370 | 12 | * http://mbed.org/users/kenjiArai/ |
kenjiArai | 6:451811697299 | 13 | * Created: July 13th, 2014 |
kenjiArai | 5:81fc00bd76fe | 14 | * Revised: Feburary 24th, 2015 |
kenjiArai | 0:de66621e5370 | 15 | * |
kenjiArai | 0:de66621e5370 | 16 | * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, |
kenjiArai | 0:de66621e5370 | 17 | * INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE |
kenjiArai | 0:de66621e5370 | 18 | * AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, |
kenjiArai | 0:de66621e5370 | 19 | * DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, |
kenjiArai | 0:de66621e5370 | 20 | * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. |
kenjiArai | 0:de66621e5370 | 21 | */ |
kenjiArai | 0:de66621e5370 | 22 | |
kenjiArai | 0:de66621e5370 | 23 | #include "L3GD20.h" |
kenjiArai | 0:de66621e5370 | 24 | |
kenjiArai | 0:de66621e5370 | 25 | L3GX_GYRO::L3GX_GYRO (PinName p_sda, PinName p_scl, |
kenjiArai | 6:451811697299 | 26 | uint8_t addr, uint8_t data_rate, uint8_t bandwidth, uint8_t fullscale) : _i2c(p_sda, p_scl) |
kenjiArai | 6:451811697299 | 27 | { |
kenjiArai | 5:81fc00bd76fe | 28 | _i2c.frequency(400000); |
kenjiArai | 0:de66621e5370 | 29 | initialize (addr, data_rate, bandwidth, fullscale); |
kenjiArai | 0:de66621e5370 | 30 | } |
kenjiArai | 0:de66621e5370 | 31 | |
kenjiArai | 6:451811697299 | 32 | L3GX_GYRO::L3GX_GYRO (PinName p_sda, PinName p_scl, uint8_t addr) : _i2c(p_sda, p_scl) |
kenjiArai | 6:451811697299 | 33 | { |
kenjiArai | 5:81fc00bd76fe | 34 | _i2c.frequency(400000); |
kenjiArai | 5:81fc00bd76fe | 35 | initialize (addr, L3GX_DR_95HZ, L3GX_BW_HI, L3GX_FS_250DPS); |
kenjiArai | 5:81fc00bd76fe | 36 | } |
kenjiArai | 5:81fc00bd76fe | 37 | |
kenjiArai | 0:de66621e5370 | 38 | L3GX_GYRO::L3GX_GYRO (I2C& p_i2c, |
kenjiArai | 6:451811697299 | 39 | uint8_t addr, uint8_t data_rate, uint8_t bandwidth, uint8_t fullscale) : _i2c(p_i2c) |
kenjiArai | 6:451811697299 | 40 | { |
kenjiArai | 6:451811697299 | 41 | _i2c.frequency(400000); |
kenjiArai | 0:de66621e5370 | 42 | initialize (addr, data_rate, bandwidth, fullscale); |
kenjiArai | 0:de66621e5370 | 43 | } |
kenjiArai | 0:de66621e5370 | 44 | |
kenjiArai | 6:451811697299 | 45 | L3GX_GYRO::L3GX_GYRO (I2C& p_i2c, uint8_t addr) : _i2c(p_i2c) |
kenjiArai | 6:451811697299 | 46 | { |
kenjiArai | 6:451811697299 | 47 | _i2c.frequency(400000); |
kenjiArai | 5:81fc00bd76fe | 48 | initialize (addr, L3GX_DR_95HZ, L3GX_BW_HI, L3GX_FS_250DPS); |
kenjiArai | 5:81fc00bd76fe | 49 | } |
kenjiArai | 5:81fc00bd76fe | 50 | |
kenjiArai | 6:451811697299 | 51 | void L3GX_GYRO::initialize (uint8_t addr, uint8_t data_rate, uint8_t bandwidth, uint8_t fullscale) |
kenjiArai | 6:451811697299 | 52 | { |
kenjiArai | 0:de66621e5370 | 53 | // Check gyro is available of not |
kenjiArai | 0:de66621e5370 | 54 | gyro_addr = addr; |
kenjiArai | 6:451811697299 | 55 | dt[0] = L3GX_WHO_AM_I; |
kenjiArai | 5:81fc00bd76fe | 56 | _i2c.write(gyro_addr, dt, 1, true); |
kenjiArai | 5:81fc00bd76fe | 57 | _i2c.read(gyro_addr, dt, 1, false); |
kenjiArai | 6:451811697299 | 58 | if (dt[0] == I_AM_L3G4200D) { |
kenjiArai | 0:de66621e5370 | 59 | gyro_ready = 1; |
kenjiArai | 6:451811697299 | 60 | } else if (dt[0] == I_AM_L3GD20) { |
kenjiArai | 0:de66621e5370 | 61 | gyro_ready = 1; |
kenjiArai | 0:de66621e5370 | 62 | } else { |
kenjiArai | 0:de66621e5370 | 63 | gyro_ready = 0; |
kenjiArai | 0:de66621e5370 | 64 | return; // gyro chip is NOT on I2C line then terminate this part |
kenjiArai | 0:de66621e5370 | 65 | } |
kenjiArai | 0:de66621e5370 | 66 | // Reg.1 |
kenjiArai | 5:81fc00bd76fe | 67 | dt[0] = L3GX_CTRL_REG1; |
kenjiArai | 5:81fc00bd76fe | 68 | dt[1] = 0x0f; |
kenjiArai | 5:81fc00bd76fe | 69 | dt[1] |= data_rate << 6; |
kenjiArai | 5:81fc00bd76fe | 70 | dt[1] |= bandwidth << 4; |
kenjiArai | 5:81fc00bd76fe | 71 | _i2c.write(gyro_addr, dt, 2, false); |
kenjiArai | 6:451811697299 | 72 | // Reg.3 |
kenjiArai | 5:81fc00bd76fe | 73 | dt[0] = L3GX_CTRL_REG3; |
kenjiArai | 5:81fc00bd76fe | 74 | dt[1] = 0x08; |
kenjiArai | 5:81fc00bd76fe | 75 | _i2c.write(gyro_addr, dt, 2, false); |
kenjiArai | 0:de66621e5370 | 76 | // Reg.4 |
kenjiArai | 5:81fc00bd76fe | 77 | dt[0] = L3GX_CTRL_REG4; |
kenjiArai | 6:451811697299 | 78 | switch (fullscale) { |
kenjiArai | 6:451811697299 | 79 | case L3GX_FS_250DPS: |
kenjiArai | 6:451811697299 | 80 | fs_factor = 0.00875; |
kenjiArai | 6:451811697299 | 81 | dt[1] = 0x80; |
kenjiArai | 6:451811697299 | 82 | break; |
kenjiArai | 6:451811697299 | 83 | case L3GX_FS_500DPS: |
kenjiArai | 6:451811697299 | 84 | fs_factor = 0.0175; |
kenjiArai | 6:451811697299 | 85 | dt[1] = 0x90; |
kenjiArai | 6:451811697299 | 86 | break; |
kenjiArai | 6:451811697299 | 87 | case L3GX_FS_2000DPS: |
kenjiArai | 6:451811697299 | 88 | fs_factor = 0.07; |
kenjiArai | 6:451811697299 | 89 | dt[1] = 0xa0; |
kenjiArai | 6:451811697299 | 90 | break; |
kenjiArai | 6:451811697299 | 91 | default: |
kenjiArai | 6:451811697299 | 92 | ; |
kenjiArai | 0:de66621e5370 | 93 | } |
kenjiArai | 5:81fc00bd76fe | 94 | _i2c.write(gyro_addr, dt, 2, false); |
kenjiArai | 0:de66621e5370 | 95 | } |
kenjiArai | 0:de66621e5370 | 96 | |
kenjiArai | 6:451811697299 | 97 | void L3GX_GYRO::read_data(float *dt_usr) |
kenjiArai | 6:451811697299 | 98 | { |
kenjiArai | 6:451811697299 | 99 | char data[6]; |
kenjiArai | 0:de66621e5370 | 100 | |
kenjiArai | 6:451811697299 | 101 | if (gyro_ready == 0) { |
kenjiArai | 5:81fc00bd76fe | 102 | dt_usr[0] = 0; |
kenjiArai | 5:81fc00bd76fe | 103 | dt_usr[1] = 0; |
kenjiArai | 5:81fc00bd76fe | 104 | dt_usr[2] = 0; |
kenjiArai | 6:451811697299 | 105 | return; |
kenjiArai | 0:de66621e5370 | 106 | } |
kenjiArai | 0:de66621e5370 | 107 | // X,Y & Z |
kenjiArai | 6:451811697299 | 108 | // manual said that |
kenjiArai | 6:451811697299 | 109 | // In order to read multiple bytes, it is necessary to assert the most significant bit |
kenjiArai | 6:451811697299 | 110 | // of the subaddress field. |
kenjiArai | 6:451811697299 | 111 | // In other words, SUB(7) must be equal to ‘1’ while SUB(6-0) represents the address |
kenjiArai | 6:451811697299 | 112 | // of the first register to be read. |
kenjiArai | 5:81fc00bd76fe | 113 | dt[0] = L3GX_OUT_X_L | 0x80; |
kenjiArai | 6:451811697299 | 114 | _i2c.write(gyro_addr, dt, 1, true); |
kenjiArai | 5:81fc00bd76fe | 115 | _i2c.read(gyro_addr, data, 6, false); |
kenjiArai | 0:de66621e5370 | 116 | // data normalization |
kenjiArai | 5:81fc00bd76fe | 117 | dt_usr[0] = float(short(data[1] << 8 | data[0])) * fs_factor; |
kenjiArai | 5:81fc00bd76fe | 118 | dt_usr[1] = float(short(data[3] << 8 | data[2])) * fs_factor; |
kenjiArai | 5:81fc00bd76fe | 119 | dt_usr[2] = float(short(data[5] << 8 | data[4])) * fs_factor; |
kenjiArai | 0:de66621e5370 | 120 | } |
kenjiArai | 0:de66621e5370 | 121 | |
kenjiArai | 6:451811697299 | 122 | int8_t L3GX_GYRO::read_temp() |
kenjiArai | 6:451811697299 | 123 | { |
kenjiArai | 6:451811697299 | 124 | if (gyro_ready == 1) { |
kenjiArai | 6:451811697299 | 125 | dt[0] = L3GX_OUT_TEMP; |
kenjiArai | 5:81fc00bd76fe | 126 | _i2c.write(gyro_addr, dt, 1, true); |
kenjiArai | 5:81fc00bd76fe | 127 | _i2c.read(gyro_addr, dt, 1, false); |
kenjiArai | 0:de66621e5370 | 128 | } else { |
kenjiArai | 5:81fc00bd76fe | 129 | dt[0] = 99; |
kenjiArai | 0:de66621e5370 | 130 | } |
kenjiArai | 5:81fc00bd76fe | 131 | return (int8_t)dt[0]; |
kenjiArai | 0:de66621e5370 | 132 | } |
kenjiArai | 0:de66621e5370 | 133 | |
kenjiArai | 6:451811697299 | 134 | uint8_t L3GX_GYRO::read_id() |
kenjiArai | 6:451811697299 | 135 | { |
kenjiArai | 6:451811697299 | 136 | dt[0] = L3GX_WHO_AM_I; |
kenjiArai | 5:81fc00bd76fe | 137 | _i2c.write(gyro_addr, dt, 1, true); |
kenjiArai | 5:81fc00bd76fe | 138 | _i2c.read(gyro_addr, dt, 1, false); |
kenjiArai | 5:81fc00bd76fe | 139 | return (uint8_t)dt[0]; |
kenjiArai | 0:de66621e5370 | 140 | } |
kenjiArai | 6:451811697299 | 141 | |
kenjiArai | 6:451811697299 | 142 | uint8_t L3GX_GYRO::data_ready() |
kenjiArai | 6:451811697299 | 143 | { |
kenjiArai | 6:451811697299 | 144 | if (gyro_ready == 1) { |
kenjiArai | 6:451811697299 | 145 | dt[0] = L3GX_STATUS_REG; |
kenjiArai | 5:81fc00bd76fe | 146 | _i2c.write(gyro_addr, dt, 1, true); |
kenjiArai | 5:81fc00bd76fe | 147 | _i2c.read(gyro_addr, dt, 1, false); |
kenjiArai | 6:451811697299 | 148 | if (!(dt[0] & 0x01)) { |
kenjiArai | 0:de66621e5370 | 149 | return 0; |
kenjiArai | 0:de66621e5370 | 150 | } |
kenjiArai | 0:de66621e5370 | 151 | } |
kenjiArai | 0:de66621e5370 | 152 | return 1;; |
kenjiArai | 0:de66621e5370 | 153 | } |
kenjiArai | 0:de66621e5370 | 154 | |
kenjiArai | 6:451811697299 | 155 | void L3GX_GYRO::frequency(int hz) |
kenjiArai | 6:451811697299 | 156 | { |
kenjiArai | 5:81fc00bd76fe | 157 | _i2c.frequency(hz); |
kenjiArai | 1:9475fd0e35ff | 158 | } |
kenjiArai | 1:9475fd0e35ff | 159 | |
kenjiArai | 6:451811697299 | 160 | uint8_t L3GX_GYRO::read_reg(uint8_t addr) |
kenjiArai | 6:451811697299 | 161 | { |
kenjiArai | 6:451811697299 | 162 | if (gyro_ready == 1) { |
kenjiArai | 6:451811697299 | 163 | dt[0] = addr; |
kenjiArai | 5:81fc00bd76fe | 164 | _i2c.write(gyro_addr, dt, 1, true); |
kenjiArai | 5:81fc00bd76fe | 165 | _i2c.read(gyro_addr, dt, 1, false); |
kenjiArai | 0:de66621e5370 | 166 | } else { |
kenjiArai | 5:81fc00bd76fe | 167 | dt[0] = 0xff; |
kenjiArai | 0:de66621e5370 | 168 | } |
kenjiArai | 5:81fc00bd76fe | 169 | return (uint8_t)dt[0]; |
kenjiArai | 0:de66621e5370 | 170 | } |
kenjiArai | 0:de66621e5370 | 171 | |
kenjiArai | 6:451811697299 | 172 | void L3GX_GYRO::write_reg(uint8_t addr, uint8_t data) |
kenjiArai | 6:451811697299 | 173 | { |
kenjiArai | 6:451811697299 | 174 | if (gyro_ready == 1) { |
kenjiArai | 5:81fc00bd76fe | 175 | dt[0] = addr; |
kenjiArai | 6:451811697299 | 176 | dt[1] = data; |
kenjiArai | 5:81fc00bd76fe | 177 | _i2c.write(gyro_addr, dt, 2, false); |
kenjiArai | 0:de66621e5370 | 178 | } |
kenjiArai | 0:de66621e5370 | 179 | } |