Diff: L3GD20.cpp

Revision:

0:de66621e5370

Child:

1:9475fd0e35ff

diff -r 000000000000 -r de66621e5370 L3GD20.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/L3GD20.cpp	Fri Aug 29 13:26:33 2014 +0000
@@ -0,0 +1,151 @@
+/*
+ * 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
+ *  L3G4200 DMEMS motion sensor: three-axis digital output gyroscope, made by STMicroelectronics
+ *      http://www.st.com/web/catalog/sense_power/FM89/SC1288
+ *          /PF250373?sc=internet/analog/product/250373.jsp
+ *
+ * Copyright (c) 2014 Kenji Arai / JH1PJL
+ *  http://www.page.sannet.ne.jp/kenjia/index.html
+ *  http://mbed.org/users/kenjiArai/
+ *      Created: July      13th, 2014 
+ *      Revised: August    29th, 2014
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED,
+ * INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE
+ * AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM,
+ * DAMAGES OR OTHER  LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
+ */
+
+#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) {
+    initialize (addr, data_rate, bandwidth, fullscale);
+}
+
+L3GX_GYRO::L3GX_GYRO (I2C& p_i2c,
+    uint8_t addr, uint8_t data_rate, uint8_t bandwidth, uint8_t fullscale) : i2c(p_i2c) { 
+    initialize (addr, data_rate, bandwidth, 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;
+    dbf[0] = L3GX_WHO_AM_I; 
+    i2c.write(gyro_addr, dbf, 1); 
+    i2c.read(gyro_addr, dbf, 1);
+    if (dbf[0] == I_AM_L3G4200D){
+        gyro_ready = 1;
+    } else if (dbf[0] == I_AM_L3GD20){
+        gyro_ready = 1;
+    } else {
+        gyro_ready = 0;
+        return;     // gyro chip is NOT on I2C line then terminate this part
+    }
+    //  Reg.1
+    dbf[0] = L3GX_CTRL_REG1;
+    dbf[1] = 0x0f;
+    dbf[1] |= data_rate << 6;
+    dbf[1] |= bandwidth << 4;
+    i2c.write(gyro_addr, dbf, 2);
+    //  Reg.3 
+    dbf[0] = L3GX_CTRL_REG3;
+    dbf[1] = 0x08;
+    i2c.write(gyro_addr, dbf, 2);
+    //  Reg.4
+    dbf[0] = L3GX_CTRL_REG4;
+    switch (fullscale){
+    case L3GX_FS_250DPS:
+        fs_factor = 0.00875;
+        dbf[1] = 0x80;
+        break;
+    case L3GX_FS_500DPS:
+        fs_factor = 0.0175;
+        dbf[1] = 0x90;
+        break;
+    case L3GX_FS_2000DPS:
+        fs_factor = 0.07;
+        dbf[1] = 0xa0;
+        break;
+    default:
+        ;
+    }
+    i2c.write(gyro_addr, dbf, 2);
+}
+
+void L3GX_GYRO::read_data(float *dt) {
+char data[6];
+
+    if (gyro_ready == 0){
+        dt[0] = 0;
+        dt[1] = 0;
+        dt[2] = 0;
+        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.
+    dbf[0] = L3GX_OUT_X_L | 0x80;
+    i2c.write(gyro_addr, dbf, 1, true); 
+    i2c.read(gyro_addr, data, 6, false);
+    // data normalization
+    dt[0] = float(short(data[1] << 8 | data[0])) * fs_factor;
+    dt[1] = float(short(data[3] << 8 | data[2])) * fs_factor;
+    dt[2] = float(short(data[5] << 8 | data[4])) * fs_factor;   
+}
+
+int8_t L3GX_GYRO::read_temp() {
+    if (gyro_ready == 1){
+        dbf[0] = L3GX_OUT_TEMP; 
+        i2c.write(gyro_addr, dbf, 1);
+        i2c.read(gyro_addr, dbf, 1);
+    } else {
+        dbf[0] = 99;
+    }
+    return (int8_t)dbf[0];
+}
+
+uint8_t L3GX_GYRO::read_id() {
+    dbf[0] = L3GX_WHO_AM_I; 
+    i2c.write(gyro_addr, dbf, 1); 
+    i2c.read(gyro_addr, dbf, 1);
+    return (uint8_t)dbf[0];
+}
+    
+uint8_t L3GX_GYRO::data_ready() {
+    if (gyro_ready == 1){
+        dbf[0] = L3GX_STATUS_REG; 
+        i2c.write(gyro_addr, dbf, 1); 
+        i2c.read(gyro_addr, dbf, 1);
+        if (!(dbf[0] & 0x01)){
+            return 0;
+        }
+    }
+    return 1;;
+}
+
+uint8_t L3GX_GYRO::read_reg(uint8_t addr) {
+    if (gyro_ready == 1){
+        dbf[0] = addr; 
+        i2c.write(gyro_addr, dbf, 1); 
+        i2c.read(gyro_addr, dbf, 1);
+    } else {
+        dbf[0] = 0xff;
+    }
+    return (uint8_t)dbf[0];
+}
+
+void L3GX_GYRO::write_reg(uint8_t addr, uint8_t data) {
+    if (gyro_ready == 1){
+        dbf[0] = addr;
+        dbf[1] = data; 
+        i2c.write(gyro_addr, dbf, 2); 
+    }
+}