Kenji Arai / L3GD20

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@7:f5c0fe6ed71a, 2017-08-23 (annotated)

Committer:
kenjiArai
Date:
Wed Aug 23 09:25:40 2017 +0000
Revision:
7:f5c0fe6ed71a
Parent:
6:451811697299 
countermeasure for  NonCopyable

Who changed what in which revision?

UserRevisionLine numberNew 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 7:f5c0fe6ed71a 10 * Copyright (c) 2014,'15,'17 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 7:f5c0fe6ed71a 13 * Created: July 13th, 2014
kenjiArai 7:f5c0fe6ed71a 14 * Revised: August 23rd, 2017
kenjiArai 0:de66621e5370 15 */
kenjiArai 0:de66621e5370 16
kenjiArai 0:de66621e5370 17 #include "L3GD20.h"
kenjiArai 0:de66621e5370 18
kenjiArai 0:de66621e5370 19 L3GX_GYRO::L3GX_GYRO (PinName p_sda, PinName p_scl,
kenjiArai 7:f5c0fe6ed71a 20 uint8_t addr, uint8_t data_rate, uint8_t bandwidth, uint8_t fullscale)
kenjiArai 7:f5c0fe6ed71a 21 : _i2c_p(new I2C(p_sda, p_scl)), _i2c(*_i2c_p)
kenjiArai 6:451811697299 22 {
kenjiArai 5:81fc00bd76fe 23 _i2c.frequency(400000);
kenjiArai 0:de66621e5370 24 initialize (addr, data_rate, bandwidth, fullscale);
kenjiArai 0:de66621e5370 25 }
kenjiArai 0:de66621e5370 26
kenjiArai 7:f5c0fe6ed71a 27 L3GX_GYRO::L3GX_GYRO (PinName p_sda, PinName p_scl, uint8_t addr)
kenjiArai 7:f5c0fe6ed71a 28 : _i2c_p(new I2C(p_sda, p_scl)), _i2c(*_i2c_p)
kenjiArai 6:451811697299 29 {
kenjiArai 5:81fc00bd76fe 30 _i2c.frequency(400000);
kenjiArai 5:81fc00bd76fe 31 initialize (addr, L3GX_DR_95HZ, L3GX_BW_HI, L3GX_FS_250DPS);
kenjiArai 5:81fc00bd76fe 32 }
kenjiArai 5:81fc00bd76fe 33
kenjiArai 0:de66621e5370 34 L3GX_GYRO::L3GX_GYRO (I2C& p_i2c,
kenjiArai 7:f5c0fe6ed71a 35 uint8_t addr, uint8_t data_rate, uint8_t bandwidth, uint8_t fullscale)
kenjiArai 7:f5c0fe6ed71a 36 : _i2c(p_i2c)
kenjiArai 6:451811697299 37 {
kenjiArai 6:451811697299 38 _i2c.frequency(400000);
kenjiArai 0:de66621e5370 39 initialize (addr, data_rate, bandwidth, fullscale);
kenjiArai 0:de66621e5370 40 }
kenjiArai 0:de66621e5370 41
kenjiArai 7:f5c0fe6ed71a 42 L3GX_GYRO::L3GX_GYRO (I2C& p_i2c, uint8_t addr)
kenjiArai 7:f5c0fe6ed71a 43 : _i2c(p_i2c)
kenjiArai 6:451811697299 44 {
kenjiArai 6:451811697299 45 _i2c.frequency(400000);
kenjiArai 5:81fc00bd76fe 46 initialize (addr, L3GX_DR_95HZ, L3GX_BW_HI, L3GX_FS_250DPS);
kenjiArai 5:81fc00bd76fe 47 }
kenjiArai 5:81fc00bd76fe 48
kenjiArai 7:f5c0fe6ed71a 49 void L3GX_GYRO::initialize (uint8_t addr, uint8_t data_rate,
kenjiArai 7:f5c0fe6ed71a 50 uint8_t bandwidth, uint8_t fullscale)
kenjiArai 6:451811697299 51 {
kenjiArai 0:de66621e5370 52 // Check gyro is available of not
kenjiArai 0:de66621e5370 53 gyro_addr = addr;
kenjiArai 6:451811697299 54 dt[0] = L3GX_WHO_AM_I;
kenjiArai 5:81fc00bd76fe 55 _i2c.write(gyro_addr, dt, 1, true);
kenjiArai 5:81fc00bd76fe 56 _i2c.read(gyro_addr, dt, 1, false);
kenjiArai 6:451811697299 57 if (dt[0] == I_AM_L3G4200D) {
kenjiArai 0:de66621e5370 58 gyro_ready = 1;
kenjiArai 6:451811697299 59 } else if (dt[0] == I_AM_L3GD20) {
kenjiArai 0:de66621e5370 60 gyro_ready = 1;
kenjiArai 0:de66621e5370 61 } else {
kenjiArai 0:de66621e5370 62 gyro_ready = 0;
kenjiArai 0:de66621e5370 63 return; // gyro chip is NOT on I2C line then terminate this part
kenjiArai 0:de66621e5370 64 }
kenjiArai 0:de66621e5370 65 // Reg.1
kenjiArai 5:81fc00bd76fe 66 dt[0] = L3GX_CTRL_REG1;
kenjiArai 5:81fc00bd76fe 67 dt[1] = 0x0f;
kenjiArai 5:81fc00bd76fe 68 dt[1] |= data_rate << 6;
kenjiArai 5:81fc00bd76fe 69 dt[1] |= bandwidth << 4;
kenjiArai 5:81fc00bd76fe 70 _i2c.write(gyro_addr, dt, 2, false);
kenjiArai 6:451811697299 71 // Reg.3
kenjiArai 5:81fc00bd76fe 72 dt[0] = L3GX_CTRL_REG3;
kenjiArai 5:81fc00bd76fe 73 dt[1] = 0x08;
kenjiArai 5:81fc00bd76fe 74 _i2c.write(gyro_addr, dt, 2, false);
kenjiArai 0:de66621e5370 75 // Reg.4
kenjiArai 5:81fc00bd76fe 76 dt[0] = L3GX_CTRL_REG4;
kenjiArai 6:451811697299 77 switch (fullscale) {
kenjiArai 6:451811697299 78 case L3GX_FS_250DPS:
kenjiArai 6:451811697299 79 fs_factor = 0.00875;
kenjiArai 6:451811697299 80 dt[1] = 0x80;
kenjiArai 6:451811697299 81 break;
kenjiArai 6:451811697299 82 case L3GX_FS_500DPS:
kenjiArai 6:451811697299 83 fs_factor = 0.0175;
kenjiArai 6:451811697299 84 dt[1] = 0x90;
kenjiArai 6:451811697299 85 break;
kenjiArai 6:451811697299 86 case L3GX_FS_2000DPS:
kenjiArai 6:451811697299 87 fs_factor = 0.07;
kenjiArai 6:451811697299 88 dt[1] = 0xa0;
kenjiArai 6:451811697299 89 break;
kenjiArai 6:451811697299 90 default:
kenjiArai 6:451811697299 91 ;
kenjiArai 0:de66621e5370 92 }
kenjiArai 5:81fc00bd76fe 93 _i2c.write(gyro_addr, dt, 2, false);
kenjiArai 0:de66621e5370 94 }
kenjiArai 0:de66621e5370 95
kenjiArai 6:451811697299 96 void L3GX_GYRO::read_data(float *dt_usr)
kenjiArai 6:451811697299 97 {
kenjiArai 6:451811697299 98 char data[6];
kenjiArai 0:de66621e5370 99
kenjiArai 6:451811697299 100 if (gyro_ready == 0) {
kenjiArai 5:81fc00bd76fe 101 dt_usr[0] = 0;
kenjiArai 5:81fc00bd76fe 102 dt_usr[1] = 0;
kenjiArai 5:81fc00bd76fe 103 dt_usr[2] = 0;
kenjiArai 6:451811697299 104 return;
kenjiArai 0:de66621e5370 105 }
kenjiArai 0:de66621e5370 106 // X,Y & Z
kenjiArai 6:451811697299 107 // manual said that
kenjiArai 6:451811697299 108 // In order to read multiple bytes, it is necessary to assert the most significant bit
kenjiArai 6:451811697299 109 // of the subaddress field.
kenjiArai 6:451811697299 110 // In other words, SUB(7) must be equal to ‘1’ while SUB(6-0) represents the address
kenjiArai 6:451811697299 111 // of the first register to be read.
kenjiArai 5:81fc00bd76fe 112 dt[0] = L3GX_OUT_X_L | 0x80;
kenjiArai 6:451811697299 113 _i2c.write(gyro_addr, dt, 1, true);
kenjiArai 5:81fc00bd76fe 114 _i2c.read(gyro_addr, data, 6, false);
kenjiArai 0:de66621e5370 115 // data normalization
kenjiArai 5:81fc00bd76fe 116 dt_usr[0] = float(short(data[1] << 8 | data[0])) * fs_factor;
kenjiArai 5:81fc00bd76fe 117 dt_usr[1] = float(short(data[3] << 8 | data[2])) * fs_factor;
kenjiArai 5:81fc00bd76fe 118 dt_usr[2] = float(short(data[5] << 8 | data[4])) * fs_factor;
kenjiArai 0:de66621e5370 119 }
kenjiArai 0:de66621e5370 120
kenjiArai 6:451811697299 121 int8_t L3GX_GYRO::read_temp()
kenjiArai 6:451811697299 122 {
kenjiArai 6:451811697299 123 if (gyro_ready == 1) {
kenjiArai 6:451811697299 124 dt[0] = L3GX_OUT_TEMP;
kenjiArai 5:81fc00bd76fe 125 _i2c.write(gyro_addr, dt, 1, true);
kenjiArai 5:81fc00bd76fe 126 _i2c.read(gyro_addr, dt, 1, false);
kenjiArai 0:de66621e5370 127 } else {
kenjiArai 5:81fc00bd76fe 128 dt[0] = 99;
kenjiArai 0:de66621e5370 129 }
kenjiArai 5:81fc00bd76fe 130 return (int8_t)dt[0];
kenjiArai 0:de66621e5370 131 }
kenjiArai 0:de66621e5370 132
kenjiArai 6:451811697299 133 uint8_t L3GX_GYRO::read_id()
kenjiArai 6:451811697299 134 {
kenjiArai 6:451811697299 135 dt[0] = L3GX_WHO_AM_I;
kenjiArai 5:81fc00bd76fe 136 _i2c.write(gyro_addr, dt, 1, true);
kenjiArai 5:81fc00bd76fe 137 _i2c.read(gyro_addr, dt, 1, false);
kenjiArai 5:81fc00bd76fe 138 return (uint8_t)dt[0];
kenjiArai 0:de66621e5370 139 }
kenjiArai 6:451811697299 140
kenjiArai 6:451811697299 141 uint8_t L3GX_GYRO::data_ready()
kenjiArai 6:451811697299 142 {
kenjiArai 6:451811697299 143 if (gyro_ready == 1) {
kenjiArai 6:451811697299 144 dt[0] = L3GX_STATUS_REG;
kenjiArai 5:81fc00bd76fe 145 _i2c.write(gyro_addr, dt, 1, true);
kenjiArai 5:81fc00bd76fe 146 _i2c.read(gyro_addr, dt, 1, false);
kenjiArai 6:451811697299 147 if (!(dt[0] & 0x01)) {
kenjiArai 0:de66621e5370 148 return 0;
kenjiArai 0:de66621e5370 149 }
kenjiArai 0:de66621e5370 150 }
kenjiArai 0:de66621e5370 151 return 1;;
kenjiArai 0:de66621e5370 152 }
kenjiArai 0:de66621e5370 153
kenjiArai 6:451811697299 154 void L3GX_GYRO::frequency(int hz)
kenjiArai 6:451811697299 155 {
kenjiArai 5:81fc00bd76fe 156 _i2c.frequency(hz);
kenjiArai 1:9475fd0e35ff 157 }
kenjiArai 1:9475fd0e35ff 158
kenjiArai 6:451811697299 159 uint8_t L3GX_GYRO::read_reg(uint8_t addr)
kenjiArai 6:451811697299 160 {
kenjiArai 6:451811697299 161 if (gyro_ready == 1) {
kenjiArai 6:451811697299 162 dt[0] = addr;
kenjiArai 5:81fc00bd76fe 163 _i2c.write(gyro_addr, dt, 1, true);
kenjiArai 5:81fc00bd76fe 164 _i2c.read(gyro_addr, dt, 1, false);
kenjiArai 0:de66621e5370 165 } else {
kenjiArai 5:81fc00bd76fe 166 dt[0] = 0xff;
kenjiArai 0:de66621e5370 167 }
kenjiArai 5:81fc00bd76fe 168 return (uint8_t)dt[0];
kenjiArai 0:de66621e5370 169 }
kenjiArai 0:de66621e5370 170
kenjiArai 6:451811697299 171 void L3GX_GYRO::write_reg(uint8_t addr, uint8_t data)
kenjiArai 6:451811697299 172 {
kenjiArai 6:451811697299 173 if (gyro_ready == 1) {
kenjiArai 5:81fc00bd76fe 174 dt[0] = addr;
kenjiArai 6:451811697299 175 dt[1] = data;
kenjiArai 5:81fc00bd76fe 176 _i2c.write(gyro_addr, dt, 2, false);
kenjiArai 0:de66621e5370 177 }
kenjiArai 0:de66621e5370 178 }
kenjiArai 7:f5c0fe6ed71a 179
kenjiArai 7:f5c0fe6ed71a 180