L3GD20.cpp

00001 /*
00002  * mbed library program
00003  *  L3GD20 MEMS motion sensor: 3-axis digital gyroscope, made by STMicroelectronics
00004  *      http://www.st.com/web/catalog/sense_power/FM89/SC1288
00005  *          /PF252443?sc=internet/analog/product/252443.jsp
00006  *  L3G4200 DMEMS motion sensor: three-axis digital output gyroscope, made by STMicroelectronics
00007  *      http://www.st.com/web/catalog/sense_power/FM89/SC1288
00008  *          /PF250373?sc=internet/analog/product/250373.jsp
00009  *
00010  * Copyright (c) 2014,'15,'17 Kenji Arai / JH1PJL
00011  *  http://www.page.sannet.ne.jp/kenjia/index.html
00012  *  http://mbed.org/users/kenjiArai/
00013  *      Created: July       13th, 2014
00014  *      Revised: August     23rd, 2017
00015  */
00016 
00017 #include "L3GD20.h"
00018 
00019 L3GX_GYRO::L3GX_GYRO (PinName p_sda, PinName p_scl,
00020     uint8_t addr, uint8_t data_rate, uint8_t bandwidth, uint8_t fullscale)
00021  : _i2c_p(new I2C(p_sda, p_scl)), _i2c(*_i2c_p)
00022 {
00023     _i2c.frequency(400000);
00024     initialize (addr, data_rate, bandwidth, fullscale);
00025 }
00026 
00027 L3GX_GYRO::L3GX_GYRO (PinName p_sda, PinName p_scl, uint8_t addr)
00028  : _i2c_p(new I2C(p_sda, p_scl)), _i2c(*_i2c_p)
00029 {
00030     _i2c.frequency(400000);
00031     initialize (addr, L3GX_DR_95HZ, L3GX_BW_HI, L3GX_FS_250DPS);
00032 }
00033 
00034 L3GX_GYRO::L3GX_GYRO (I2C& p_i2c,
00035     uint8_t addr, uint8_t data_rate, uint8_t bandwidth, uint8_t fullscale)
00036  : _i2c(p_i2c)
00037 {
00038     _i2c.frequency(400000);
00039     initialize (addr, data_rate, bandwidth, fullscale);
00040 }
00041 
00042 L3GX_GYRO::L3GX_GYRO (I2C& p_i2c, uint8_t addr)
00043  : _i2c(p_i2c)
00044 {
00045     _i2c.frequency(400000);
00046     initialize (addr, L3GX_DR_95HZ, L3GX_BW_HI, L3GX_FS_250DPS);
00047 }
00048 
00049 void L3GX_GYRO::initialize (uint8_t addr, uint8_t data_rate,
00050                      uint8_t bandwidth, uint8_t fullscale)
00051 {
00052     // Check gyro is available of not
00053     gyro_addr = addr;
00054     dt[0] = L3GX_WHO_AM_I;
00055     _i2c.write(gyro_addr, dt, 1, true);
00056     _i2c.read(gyro_addr, dt, 1, false);
00057     if (dt[0] == I_AM_L3G4200D) {
00058         gyro_ready = 1;
00059     } else if (dt[0] == I_AM_L3GD20) {
00060         gyro_ready = 1;
00061     } else {
00062         gyro_ready = 0;
00063         return;     // gyro chip is NOT on I2C line then terminate this part
00064     }
00065     //  Reg.1
00066     dt[0] = L3GX_CTRL_REG1;
00067     dt[1] = 0x0f;
00068     dt[1] |= data_rate << 6;
00069     dt[1] |= bandwidth << 4;
00070     _i2c.write(gyro_addr, dt, 2, false);
00071     //  Reg.3
00072     dt[0] = L3GX_CTRL_REG3;
00073     dt[1] = 0x08;
00074     _i2c.write(gyro_addr, dt, 2, false);
00075     //  Reg.4
00076     dt[0] = L3GX_CTRL_REG4;
00077     switch (fullscale) {
00078         case L3GX_FS_250DPS:
00079             fs_factor = 0.00875;
00080             dt[1] = 0x80;
00081             break;
00082         case L3GX_FS_500DPS:
00083             fs_factor = 0.0175;
00084             dt[1] = 0x90;
00085             break;
00086         case L3GX_FS_2000DPS:
00087             fs_factor = 0.07;
00088             dt[1] = 0xa0;
00089             break;
00090         default:
00091             ;
00092     }
00093     _i2c.write(gyro_addr, dt, 2, false);
00094 }
00095 
00096 void L3GX_GYRO::read_data(float *dt_usr)
00097 {
00098     char data[6];
00099 
00100     if (gyro_ready == 0) {
00101         dt_usr[0] = 0;
00102         dt_usr[1] = 0;
00103         dt_usr[2] = 0;
00104         return;
00105     }
00106     // X,Y & Z
00107     // manual said that
00108     // In order to read multiple bytes, it is necessary to assert the most significant bit
00109     // of the subaddress field.
00110     // In other words, SUB(7) must be equal to ‘1’ while SUB(6-0) represents the address
00111     // of the first register to be read.
00112     dt[0] = L3GX_OUT_X_L | 0x80;
00113     _i2c.write(gyro_addr, dt, 1, true);
00114     _i2c.read(gyro_addr, data, 6, false);
00115     // data normalization
00116     dt_usr[0] = float(short(data[1] << 8 | data[0])) * fs_factor;
00117     dt_usr[1] = float(short(data[3] << 8 | data[2])) * fs_factor;
00118     dt_usr[2] = float(short(data[5] << 8 | data[4])) * fs_factor;
00119 }
00120 
00121 int8_t L3GX_GYRO::read_temp()
00122 {
00123     if (gyro_ready == 1) {
00124         dt[0] = L3GX_OUT_TEMP;
00125         _i2c.write(gyro_addr, dt, 1, true);
00126         _i2c.read(gyro_addr, dt, 1, false);
00127     } else {
00128         dt[0] = 99;
00129     }
00130     return (int8_t)dt[0];
00131 }
00132 
00133 uint8_t L3GX_GYRO::read_id()
00134 {
00135     dt[0] = L3GX_WHO_AM_I;
00136     _i2c.write(gyro_addr, dt, 1, true);
00137     _i2c.read(gyro_addr, dt, 1, false);
00138     return (uint8_t)dt[0];
00139 }
00140 
00141 uint8_t L3GX_GYRO::data_ready()
00142 {
00143     if (gyro_ready == 1) {
00144         dt[0] = L3GX_STATUS_REG;
00145         _i2c.write(gyro_addr, dt, 1, true);
00146         _i2c.read(gyro_addr, dt, 1, false);
00147         if (!(dt[0] & 0x01)) {
00148             return 0;
00149         }
00150     }
00151     return 1;;
00152 }
00153 
00154 void L3GX_GYRO::frequency(int hz)
00155 {
00156     _i2c.frequency(hz);
00157 }
00158 
00159 uint8_t L3GX_GYRO::read_reg(uint8_t addr)
00160 {
00161     if (gyro_ready == 1) {
00162         dt[0] = addr;
00163         _i2c.write(gyro_addr, dt, 1, true);
00164         _i2c.read(gyro_addr, dt, 1, false);
00165     } else {
00166         dt[0] = 0xff;
00167     }
00168     return (uint8_t)dt[0];
00169 }
00170 
00171 void L3GX_GYRO::write_reg(uint8_t addr, uint8_t data)
00172 {
00173     if (gyro_ready == 1) {
00174         dt[0] = addr;
00175         dt[1] = data;
00176         _i2c.write(gyro_addr, dt, 2, false);
00177     }
00178 }
00179 
00180