File content as of revision 0:3bd8aecadafc:

#include "mbed.h"
#include "L3GD20_i2c.hpp"

l3gd20::l3gd20(I2C *_i, bool SA0):ADDR_WHO_AM_I(0x0f), ADDR_CTRL_REG1(0x20), ADDR_CTRL_REG2(0x21), ADDR_CTRL_REG3(0x22)
                            , ADDR_CTRL_REG4(0x23), ADDR_CTRL_REG5(0x24), ADDR_OUT_TEMP(0x26), ADDR_STATUS_REG(0x27)
                            , ADDR_OUT_X_L(0x28), ADDR_OUT_X_H(0x29), ADDR_OUT_Y_L(0x2A), ADDR_OUT_Y_H(0x2B), ADDR_OUT_Z_L(0x2C), ADDR_OUT_Z_H(0x2D)
{
    pre_time_ms = 0;
    i = _i;
    if(SA0) addr = 0xD6;
    else    addr = 0xD4;
    decom = 0.0875;
    write_reg(ADDR_CTRL_REG1, 0x0F);
    angleDeg[0] = angleDeg[1] = angleDeg[2] = 0;
}

char l3gd20::read_reg(char reg)
{
    char recv;
    i->write(addr, &reg, 1, true);
    i->read(addr | 0x01, &recv, 1);
    return recv;
}

void l3gd20::write_reg(char reg, char data)
{
    char send[] = {reg, data};
    i->write(addr, send, 2);
}

bool l3gd20::who_am_i()
{
    return read_reg(ADDR_WHO_AM_I) == 0xD4;
}

void l3gd20::set_range(int dps)
{
    switch(dps)
    {
        case 250:
            decom = 0.00875;
            write_reg(ADDR_CTRL_REG4, 0b00000000);
            break;
            
        case 500:
            decom = 0.0175;
            write_reg(ADDR_CTRL_REG4, 0b00010000);
            break;
            
        case 2000:
            decom = 0.07;
            write_reg(ADDR_CTRL_REG4, 0b00100000);
            break;
            
    }
}

int16_t l3gd20::get_raw_omega(char se)
{
    char addrRegH, addrRegL;
    switch(se)
    {
        case 'x': case 'X':
            addrRegH = ADDR_OUT_X_H;
            addrRegL = ADDR_OUT_X_L;
            break;
            
        case 'y': case 'Y':
            addrRegH = ADDR_OUT_Y_H;
            addrRegL = ADDR_OUT_Y_L;
            break;
            
        case 'z':case 'Z':
            addrRegH = ADDR_OUT_Z_H;
            addrRegL = ADDR_OUT_Z_L;
            break;
    }
    int16_t rawOmegaH = read_reg(addrRegH);
    int16_t rawOmegaL = read_reg(addrRegL);
    
    return  (rawOmegaH << 8) | rawOmegaL;
}

void l3gd20::apply_offset()
{
    int num = 100;
    double accum[3] = {0};
    for(int i = 0; i < num; i++)
    {
        accum[0] += get_raw_omega('X');
        accum[1] += get_raw_omega('Y');
        accum[2] += get_raw_omega('Z');
        wait_ms(1);
    }
    for(int i = 0; i < 3; i++)
    {
        omegaOffset[i] = accum[i] * decom / (double)num;
    }
}

void l3gd20::start()
{
    t.start();
    stpFlg = false;
}

void l3gd20::reset()
{
    t.reset();
    angleDeg[0] = angleDeg[1] = angleDeg[2] = 0;
}

void l3gd20::stop()
{
    t.stop();
    t.reset();
    stpFlg = true;
}

double l3gd20::trapezoid_integr(double data, double ex_data, double period){
    return (data + ex_data) * period / 2.0;
}

void l3gd20::renew_angle()
{
    static double preOmega[3] = {0};
    
    if(stpFlg) return;
    
    double omega[] = 
    {
        get_raw_omega('X') * decom - omegaOffset[0],
        get_raw_omega('Y') * decom - omegaOffset[1],
        get_raw_omega('Z') * decom - omegaOffset[2]
    };
    
    int dt_ms = t.read_ms();
//    t.reset();
    
    for(int i = 0; i < 3; i++)
    {
        angleDeg[i] += trapezoid_integr(omega[i], preOmega[i], (dt_ms - pre_time_ms) / 1000.0);
        preOmega[i] = omega[i];
    }
    pre_time_ms = dt_ms;
    
    if(t.read() > 1800)
    {
        t.reset();
        pre_time_ms = 0;
    }
}

void l3gd20::get_angle_deg(double *a)
{
    for(int i = 0; i < 3; i++)
    {
        a[i] = angleDeg[i];
    }
}

void l3gd20::get_angle_rad(double *a)
{
    for(int i = 0; i < 3; i++)
    {
        a[i] = angleDeg[i] / 180.0 * 3.141593;
    }
}