huiying jiang

Important changes to forums and questions

All forums and questions are now archived. To start a new conversation or read the latest updates go to forums.mbed.com.

6 years, 6 months ago.

Angle calculation using L3G4200D Gyroscope sensor

I have an L3G4200D gyroscope sensor communicate to LPC1768 through I2C. I have the output which gives me the angular rate of the axis but I don't know how to convert the angular rate to angle. I know it can be achieve by using integration. Can anyone help me with the code?

Convert

#include "mbed.h"

// Pin 9 is SDA & Pin 10 is SCL // 
I2C i2c(p9, p10);
Serial pc(USBTX, USBRX);
Timer t;

// SAD 1101 00xb, 1101 0010 //
// x = 1 when SDO is connected to Vs, x = 0 when SDO is connected to Gnd //
// b = 1 when read, b = 0 when write // 
const int Addr = 0xD2;

// Register Address 1000 1111. MSB = 1 means read and MSB = 0 means write// 
const int regAddr = 0x8F;

// Register Address of CTRL_REG1, 0x20 // 
const int CTRL_REG1 = 0x20; 
// const int CTRL_REG1 = 0xA0;

// Reigister Address of CTRL_REG4, 0x23 //
const int CTRL_REG4 = 0x23; 

// Register Address of Z-axis, 0x2C & 0x2D //
const int OUT_Z_H = 0xAD; // High Register // 
const int OUT_Z_L = 0xAC; // Low Register //

// Pointer to read data // 
char data[1] = {0};
// char data_ctrl[1] = {0}; 
char data_H[1] = {0};
char data_L[1] = {0};

float angle = 0.00000f;
float dt = 0.00125; 

int main() {
    
    // Maximum clock frequency is 400KHz // 
    i2c.frequency(400000);
    
    // Master send slave address and register address // 
    // Master read data from slave //
    // Default is 1101 0011 // 
    char WHO_AM_I[1] = {regAddr};
    i2c.write(Addr, WHO_AM_I, 1);
    i2c.read(Addr, data, 1);
    pc.printf("SAD is 0x%X\n", data[0]);
    
    //Configuring CTRL_REG1 //
    char ctrl_reg1[2] = {CTRL_REG1,0xFF};
    i2c.write(Addr, ctrl_reg1, 2);
    
    // 500dps, sensitivity is 17.50mdps // 
    char ctrl_reg4[2] = {CTRL_REG4, 0x20};
    i2c.write(Addr, ctrl_reg4, 2); 

    while(1) {
        
        t.reset();
        t.start();
        // Reading gyro's z-axis register //
        char zAxis_H[1] = {OUT_Z_H}; 
        i2c.write(Addr, zAxis_H, 1);
        i2c.read(Addr, data_H, 1);
        
        char zAxis_L[1] = {OUT_Z_L};
        i2c.write(Addr, zAxis_L, 1);
        i2c.read(Addr, data_L, 1);
        
        t.stop();
        
        int16_t combineValue = (((int)data_H[0]) << 8 | data_L[0]);
        int dAngle = combineValue * 0.0175;
        angle += dAngle*t.read();
        
        pc.printf("The angle is %.5f degrees, angular rate is %d dps\n", angle, dAngle);
    }
}
Be the first to answer this question.