Revision 0:d884c7453c85, committed 2013-05-21

Comitter:

jose_claudiojr

Date:

Tue May 21 13:50:20 2013 +0000

Commit message:

Example of reading an gyroscope sensor (ITG3205)

Changed in this revision

diff -r 000000000000 -r d884c7453c85 Gyroscope.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Gyroscope.cpp	Tue May 21 13:50:20 2013 +0000
@@ -0,0 +1,107 @@
+#include "Gyroscope.h"
+
+Gyroscope::Gyroscope(ITG3205* gyroHardware, float sensitivity, float dataRate)
+{
+    this->gyroHardware = gyroHardware;
+    this->sensitivity = sensitivity;
+    this->dataRate = dataRate;
+    
+    updateZeroRates();
+}
+
+Gyroscope::~Gyroscope(void)
+{
+}
+
+void Gyroscope::updateZeroRates()
+{
+    update(10, 10);
+    
+    zeroRateX = rawX;
+    zeroRateY = rawY;
+    zeroRateZ = rawZ;
+    
+    resetAngles();
+}
+
+void Gyroscope::update()
+{    
+    update(1, 1);
+}
+
+void Gyroscope::update(int samplesSize, int sampleDataRate)
+{
+    //int axes[3] = {0, 0, 0};
+    
+    rawX = 0;
+    rawY = 0;
+    rawZ = 0;
+    
+    for (int i = 0; i < samplesSize; i++)
+    {
+        //(gyroHardware)->getAxes(axes);
+        while(!gyroHardware->isRawReady());
+        
+        rawX += ( gyroHardware->getGyroX() / samplesSize);
+        rawY += ( gyroHardware->getGyroY() / samplesSize);
+        rawZ += ( gyroHardware->getGyroZ() / samplesSize);
+        
+        wait_us(sampleDataRate);
+    }
+    
+    angleX += getDegreesX() * dataRate;
+    angleY += getDegreesY() * dataRate;
+    angleZ += getDegreesZ() * dataRate;
+}
+
+void Gyroscope::resetAngles()
+{
+    angleX = 0;
+    angleY = 0;
+    angleZ = 0;
+}
+
+float Gyroscope::getDegreesX()
+{
+    return (rawX - zeroRateX) / sensitivity;
+}
+
+float Gyroscope::getDegreesY()
+{
+    return (rawY - zeroRateY) / sensitivity;
+}
+
+float Gyroscope::getDegreesZ()
+{
+    return (rawZ - zeroRateZ) / sensitivity;
+}
+
+float Gyroscope::getRadiansX()
+{
+    return getDegreesX() / 180.0 * PI;
+}
+
+float Gyroscope::getRadiansY()
+{
+    return getDegreesY() / 180.0 * PI;
+}
+
+float Gyroscope::getRadiansZ()
+{
+    return getDegreesZ() / 180.0 * PI;
+}
+
+float Gyroscope::getAngleX()
+{
+    return angleX;
+}
+
+float Gyroscope::getAngleY()
+{
+    return angleY;
+}
+
+float Gyroscope::getAngleZ()
+{
+    return angleZ;
+}

diff -r 000000000000 -r d884c7453c85 Gyroscope.h
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Gyroscope.h	Tue May 21 13:50:20 2013 +0000
@@ -0,0 +1,50 @@
+#ifndef GYROSCOPE_H
+#define GYROSCOPE_H
+
+#include "ITG3205.h"
+
+#define PI  3.14159265
+
+class Gyroscope
+{
+    public:
+        Gyroscope(ITG3205* gyroHardware, float sensitivity, float dataRate);
+        ~Gyroscope(void);
+        
+        void updateZeroRates();
+        
+        void update();
+        void update(int samplesSize, int sampleDataRate);
+        
+        void resetAngles();
+        
+        float getRadiansX();
+        float getRadiansY();
+        float getRadiansZ();
+        
+        float getDegreesX();
+        float getDegreesY();
+        float getDegreesZ();
+        
+        float getAngleX();
+        float getAngleY();
+        float getAngleZ();
+    
+    private:
+        ITG3205* gyroHardware;
+    
+        float sensitivity;
+        float dataRate;
+        
+        float zeroRateX, zeroRateY, zeroRateZ;
+        
+        float rawX;
+        float rawY;
+        float rawZ;
+        
+        float angleX;
+        float angleY;
+        float angleZ;
+};
+
+#endif /* GYROSCOPE_H */

diff -r 000000000000 -r d884c7453c85 ITG3205.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/ITG3205.cpp	Tue May 21 13:50:20 2013 +0000
@@ -0,0 +1,200 @@
+
+#include "ITG3205.h"
+
+ITG3205::ITG3205(PinName sda, PinName scl) : i2c(sda, scl) 
+{
+    char rx;
+    //400kHz, fast mode.
+    i2c.frequency(400000);
+    //===========================================================================================================
+    // Read chip_id
+    //===========================================================================================================
+    rx = Read(ITG3205_WHO_AM_I);
+    if (rx != 0x68 && rx!= 0x69)                      // Data sheet say's "The slave address of the ITG-3205
+        printf("\ninvalid chip id %d\r\n", rx);       // devices is b110100X". This could be either, 0x68 or 0x69
+    //===========================================================================================================
+    // Let's reset the chip to it's default configuration
+    //===========================================================================================================
+    // The PWR_MGM (0x3E) is composed of six parameters, to reset is necessary to set the H_RESET (bit 7)
+    //  PWR_MGM Register |===============================================|
+    //    bits           |    7      6      5       4       3    2  1  0 |
+    //                   | H_RESET SLEEP STBY_XG STBY_YG STBY_ZG CLK_SEL |
+    //                   |    1      0      0       0       0    0  0  0 |
+    //                   |===============================================|
+    Write(ITG3205_PWRM, 0x80);
+    //==========================================================================================================
+    // Let's define the low-pass filter's bandwidth to 42Hz (DLPF_CFG<2:0> -> 011) with internal sample rate of 
+    // 1kHz, to reduce noise in the signal, and also, let's define the gyro's sensors (FS_SELT<4:3> -> 11) to 
+    // FullScale Range (+/- 2000º/sec).
+    // To set the DLPF_CFG bit, we will have to acces the DLP_FS register (0x16) 
+    //==========================================================================================================
+    // The DLP_FS is composed of the FS_SEL (Bit 4 and 3) and the DLPF_CFG  (Bit 1 and 0)
+    //  DPL_FS Register |================================|
+    //    bits          | 7   6   5  |4    3| |2   1  0| |
+    //                  | X   X   X  |FS_SEL| |DLPF_CFG| |
+    //                  | 0   0   0   1    1   0   1  1  | -> (0x1B) Sets the gyro to FullScale Range (+/- 2000º/sec) and Low-Pass Filter 
+    //                  |================================|           with bandwidth of 42Hz and internal sample rate of 1kHz.
+    Write(ITG3205_DLPFS, 0x1B);
+    //Let's check
+    rx = Read(ITG3205_DLPFS);                                       // procedure: readed value: i.e XXX1 1110 -> The bits that we want to check are the bits 0,1,2,3 and 4, so we have to eliminate the other bits      
+    rx &= (~0xE0); //Mask                                           //                        (AND) 0001 1111 -> (~DLPFSMask)
+    if(rx != 0x1B)                                                  //                              ---------
+        printf("DPL_FS register has not been written: %d\r\n", rx); //                              0001 1110 -> Final value, compare with the wanted value (this one doesn't match)
+    //==========================================================================================================
+    // Now let's define the sample rate to 5msec. 
+    // To do this, a formula is applied: Fsample = Finternal/(x+1) on wich Fsample is the inverse o our sample rate, 
+    // and Finternal is equal to 1kHz, as defined in the low-pass filter's bandwidth.
+    // The SMPLRT register (0x15) controls the sample rate, so we must set this register to 4, as the result of x in the
+    // equation.
+    //==========================================================================================================
+    /*
+    Write(ITG3205_SMPLRT, 0x04);
+    //Let's check
+    rx = Read(ITG3205_SMPLRT);                                     
+    if(rx != 0x04)                                                 
+        printf("SMPLRT register has not been written: %d\r\n", rx); 
+    */
+    //==========================================================================================================
+    // Generate interrupt when device is ready or raw data ready
+    //==========================================================================================================
+    // The PWR_MGM is composed of six parameters, to reset is necessary to set the H_RESET (bit 7)
+    //  PWR_MGM Register |==========================================================================|
+    //    bits           |   7     6        5                4         3      2       1       0     |
+    //                   | ACTL  OPEN  LATCH_INT_EN  INT_ANYRD_2CLEAR  0  ITG_RDY_EN  0  RAW_RDY_EN |
+    //                   |   0     0        0                0         0      1       0       1     | -> 0x05
+    //                   |==========================================================================|
+    //Write(ITG3205_INT_CFG, 0x05);  
+    Write(ITG3205_INT_CFG, 0x00);  
+    //==========================================================================================================
+    // Back the PWR_MGM to what it was
+    //==========================================================================================================
+    Write(ITG3205_PWRM, 0x00);
+}
+
+
+
+void ITG3205::Write(char reg, char data)
+{
+    char c_data[2];
+    c_data[0] = reg;
+    c_data[1] = data;
+    i2c.write((ITG3205_ADDR << 1), c_data, 2); 
+}
+
+char ITG3205::Read(char data)
+{
+    char tx = data;
+    char rx;
+    
+    i2c.write((ITG3205_ADDR << 1) & 0xFE, &tx, 1); // 0xFE ensure that the MSB bit is being set to zero (RW=0 -> Writing)
+    i2c.read((ITG3205_ADDR << 1) | 0x01, &rx, 1);  // 0x01 ensure that the MSB bit is being set to one  (RW=1 -> Reading)
+                                             // The read/write method of I2C does this automatically, so it's useless to set manually    
+    return rx;
+}
+/*
+int ITG3205::getInternalSampleRate(void){
+
+    char tx = ITG3205_DLPFS;
+    char rx;
+    
+    i2c_.write((ITG3200_I2C_ADDRESS << 1) & 0xFE, &tx, 1);
+    
+    i2c_.read((ITG3200_I2C_ADDRESS << 1) | 0x01, &rx, 1);
+    
+    //DLPF_CFG == 0 -> sample rate = 8kHz.
+    if(rx == 0){
+        return 8;
+    } 
+    //DLPF_CFG = 1..7 -> sample rate = 1kHz.
+    else if(rx >= 1 && rx <= 7){
+        return 1;
+    }
+    //DLPF_CFG = anything else -> something's wrong!
+    else{
+        return -1;
+    }
+    
+}
+*/
+bool ITG3205::isRawReady()
+{
+    if(Read(ITG3205_INT_STATUS) == 0x01)
+        return true;
+    else
+        return false;
+        
+}
+
+float ITG3205::getGyroX(void)
+{
+    char lsb_byte = 0;
+    signed short msb_byte;
+    float acc;
+    
+    lsb_byte = Read(ITG3205_XMSB);
+    msb_byte = lsb_byte << 8;
+    msb_byte |= Read(ITG3205_XLSB);
+    acc = (float)msb_byte;
+    return acc;
+    /*
+    char tx = GYRO_XOUT_H_REG;
+    char rx[2];
+    
+    i2c.write((ITG3205_ADDR << 1) & 0xFE, &tx, 1);
+    i2c.read((ITG3205_ADDR << 1) | 0x01, rx, 2);
+    
+    int16_t output = ((int) rx[0] << 8) | ((int) rx[1]);
+    
+    return output;
+    */
+}
+
+float ITG3205::getGyroY(void)
+{
+    char lsb_byte = 0;
+    signed short msb_byte;
+    float acc;
+    
+    lsb_byte = Read(ITG3205_YMSB);
+    msb_byte = lsb_byte << 8;
+    msb_byte |= Read(ITG3205_YLSB);
+    acc = (float)msb_byte;
+    return acc;
+    /*
+    char tx = GYRO_YOUT_H_REG;
+    char rx[2];
+    
+    i2c.write((ITG3205_ADDR << 1) & 0xFE, &tx, 1);
+    
+    i2c.read((ITG3205_ADDR << 1) | 0x01, rx, 2);
+    
+    int16_t output = ((int) rx[0] << 8) | ((int) rx[1]);
+    
+    return output;
+    */
+}
+
+float ITG3205::getGyroZ(void)
+{
+    char lsb_byte = 0;
+    signed short msb_byte;
+    float acc;
+    
+    lsb_byte = Read(ITG3205_ZMSB);
+    msb_byte = lsb_byte << 8;
+    msb_byte |= Read(ITG3205_ZLSB);
+    acc = (float)msb_byte;
+    return acc;
+    /*
+    char tx = GYRO_ZOUT_H_REG;
+    char rx[2];
+    
+    i2c.write((ITG3205_ADDR << 1) & 0xFE, &tx, 1);
+    
+    i2c.read((ITG3205_ADDR << 1) | 0x01, rx, 2);
+    
+    int16_t output = ((int) rx[0] << 8) | ((int) rx[1]);
+    
+    return output;
+    */
+}
\ No newline at end of file

diff -r 000000000000 -r d884c7453c85 ITG3205.h
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/ITG3205.h	Tue May 21 13:50:20 2013 +0000
@@ -0,0 +1,37 @@
+#ifndef ITG3205_H
+#define ITG3205_H
+
+#include "mbed.h"
+
+#define ITG3205_ADDR       0x68
+#define ITG3205_WHO_AM_I   0x00
+#define ITG3205_SMPLRT     0x15
+#define ITG3205_DLPFS      0x16
+#define ITG3205_INT_CFG    0x17
+#define ITG3205_INT_STATUS 0x1A
+#define ITG3205_PWRM       0x3E
+
+#define ITG3205_XMSB       0x1D
+#define ITG3205_XLSB       0x1E
+#define ITG3205_YMSB       0x1F
+#define ITG3205_YLSB       0x20
+#define ITG3205_ZMSB       0x21
+#define ITG3205_ZLSB       0x22
+
+class ITG3205 
+{
+
+public:
+
+    ITG3205(PinName sda, PinName scl);
+    void Write(char reg, char data);
+    char Read(char data);
+    bool isRawReady();
+    float getGyroX(void);
+    float getGyroY(void);
+    float getGyroZ(void);
+private:
+    I2C i2c;
+};
+
+#endif /* ITG3205_H */

diff -r 000000000000 -r d884c7453c85 main.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Tue May 21 13:50:20 2013 +0000
@@ -0,0 +1,43 @@
+#include "mbed.h"
+#include "ITG3205.h"
+#include "Gyroscope.h"
+
+#define SDA      p9
+#define SCL      p10
+
+ITG3205 *itg3205;
+Gyroscope *gyroscope;
+Serial pc(USBTX, USBRX);
+
+float gyroX, gyroY, AngleX, AngleY;
+Ticker updater;
+
+void update()
+{
+    pc.printf("x: %f \t\t y: %f \t\t angle x: %f \t\t angle y: %f  \r\n", gyroX, gyroY, AngleX, AngleY);
+}
+int main()
+{
+    itg3205 = new ITG3205(SDA, SCL);
+    gyroscope = new Gyroscope(itg3205, 14.375, 0.005);
+    wait(1);
+    gyroscope->updateZeroRates();
+    pc.baud(115200);
+    updater.attach(&update, 0.2);
+    while(1) 
+    {
+
+        wait_ms(5);
+        gyroscope->update();
+        
+        gyroX = gyroscope->getDegreesX();
+        gyroY = gyroscope->getDegreesY() * -1;
+        AngleX = gyroscope->getAngleX();
+        AngleY = gyroscope->getAngleY();
+
+        //printf("x: %f \t\t y: %f \t\t angle x: %f \t\t angle y: %f  \r\n", gyroX, gyroY, AngleX, AngleY);
+        //printf("%f,%f,%f,%f\n", gyroX, gyroY, AngleX, AngleY);
+        
+    }
+
+}

diff -r 000000000000 -r d884c7453c85 mbed.bld
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/mbed.bld	Tue May 21 13:50:20 2013 +0000
@@ -0,0 +1,1 @@
+http://mbed.org/users/mbed_official/code/mbed/builds/cd19af002ccc
\ No newline at end of file