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@2:8073008f3036, 2014-12-07 (annotated)

Committer:
kenjiArai
Date:
Sun Dec 07 08:26:58 2014 +0000
Revision:
2:8073008f3036
Parent:
1:9475fd0e35ff
Child:
3:56bb957380c8
bug fixed in read_data()

Who changed what in which revision?

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