File content as of revision 1:635b9b4215b1:

#include "mbed.h"

#define I2C_ADDRESS     0xD4


#define ADDR_WHO_AM_I   0x0f
#define ADDR_CTRL_REG1  0x20
#define ADDR_CTRL_REG2  0x21
#define ADDR_CTRL_REG3  0x22
#define ADDR_CTRL_REG4  0x23
#define ADDR_CTRL_REG5  0x24
#define ADDR_OUT_TEMP   0x26
#define ADDR_STATUS_REG 0x27
#define ADDR_OUT_X_L    0x28
#define ADDR_OUT_X_H    0x29
#define ADDR_OUT_Y_L    0x2A
#define ADDR_OUT_Y_H    0x2B
#define ADDR_OUT_Z_L    0x2C
#define ADDR_OUT_Z_H    0x2D

I2C i2c(PB_9, PB_8);

DigitalOut myled(LED1);

Serial pc(USBTX, USBRX);

double omega_offset[3] = {0, 0, 0};

char getData(char addr){
    char readData;
    i2c.write(I2C_ADDRESS, &addr, 1, true);
    i2c.read(I2C_ADDRESS | 0x01, &readData, 1);
    return readData;
}

void setData(char addr, char data){
    char sendData[] = {addr, data};
//    i2c.write(I2C_ADDRESS, &addr, 1, true);
//    i2c.write(I2C_ADDRESS, &data, 1);
    i2c.write(I2C_ADDRESS, sendData, 2);
    return;
}

double get_angular_velocity(char axis){
    int omega_l, omega_h;
    int16_t omega;
    switch(axis){
        case 'X': case 'x':
            omega_l = getData(ADDR_OUT_X_L);
            omega_h = getData(ADDR_OUT_X_H);
            break;
        case 'Y': case 'y':
            omega_l = getData(ADDR_OUT_Y_L);
            omega_h = getData(ADDR_OUT_Y_H);
            break;
        case 'Z': case 'z':
            omega_l = getData(ADDR_OUT_Z_L);
            omega_h = getData(ADDR_OUT_Z_H);
            break;
        default : return 0.0;
    }
    omega = omega_h << 8 | omega_l;
    return omega * 0.00875; // +- 250 dps mode
    //return omega * 0.01750; // +- 500 dps mode
    //return omega * 0.070; // +- 2000 dps mode
    
}

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

Timer gyro_timer;

void getAngle(double *result, bool reset){
    static double angle_list[3] = {0, 0, 0};
    static double ex_omega[3] = {0, 0, 0};
    int read_period_us;
    double omega[3] = {0, 0, 0};
    
    omega[0] = get_angular_velocity('X') - omega_offset[0];
    omega[1] = get_angular_velocity('Y') - omega_offset[1];
    omega[2] = get_angular_velocity('Z') - omega_offset[2];
    
    read_period_us = gyro_timer.read_us();
    gyro_timer.reset();
    
    for(int axis = 0; axis < 3; axis++){
        angle_list[axis] += trapezoid_integr(omega[axis], ex_omega[axis], read_period_us / 1000000.0);
        ex_omega[axis] = omega[axis];
        if(reset)angle_list[axis] = 0;
        result[axis] = angle_list[axis];
    }
    return;
}

int getTemp(){
    return (int)getData(ADDR_OUT_TEMP) * -1 + 40; // 40 : Set manually
}


int main(){
    double angle[3] = {0, 0, 0};
    double accum[3] = {0, 0, 0};
    
    i2c.frequency(400000);
    
    pc.baud(115200);
    
    pc.printf("Hello\r\n");
    
    pc.printf("whoAmI = 0x%.2x\r\n", getData(ADDR_WHO_AM_I));
    
    setData(ADDR_CTRL_REG1, 0x0f);
    
    setData(ADDR_CTRL_REG4, 0b00000000); // +- 250 dps mode
    //setData(ADDR_CTRL_REG4, 0b00010000); // +- 500 dps mode
    //setData(ADDR_CTRL_REG4, 0b00100000); // +- 2000 dps mode
    
    gyro_timer.start();
    
    for(int count = 0; count < 1000; count++){
        accum[0] += get_angular_velocity('X');
        accum[1] += get_angular_velocity('Y');
        accum[2] += get_angular_velocity('Z');
        wait_ms(2);
    }
    
    for(int axis = 0; axis < 3; axis++){
        omega_offset[axis] = accum[axis] / 1000.0;
    }
    
    pc.printf("accum(%f, %f, %f)\r\n", accum[0], accum[1], accum[2]);
    pc.printf("offset(%f, %f, %f)\r\n", omega_offset[0], omega_offset[1], omega_offset[2]);
    
    while(1){
        
        wait_ms(1);
        
        //pc.printf("whoAmI = 0x%.2x\r\n", getData(ADDR_WHO_AM_I));
        
        //pc.printf("status = 0x%.2x\r\n", getData(ADDR_STATUS_REG));
        
        getAngle(angle, false);
        pc.printf("(%f, %f, %f)\r\n", angle[0], angle[1], angle[2]);
        
        pc.printf("temp = %d ℃\r\n", getTemp());
        
        //pc.printf("(%f, %f, %f)\r\n", get_angular_velocity('X'), get_angular_velocity('Y'), get_angular_velocity('Z'));
        
        
        //get_angular_velocity_Z();
        
        
        //pc.printf("CTRL_REG1 = 0x%.2x\r\n", getData(ADDR_CTRL_REG1));
        //pc.printf("CTRL_REG2 = 0x%.2x\r\n", getData(ADDR_CTRL_REG2));
        //pc.printf("CTRL_REG3 = 0x%.2x\r\n", getData(ADDR_CTRL_REG3));
        //pc.printf("CTRL_REG4 = 0x%.2x\r\n", getData(ADDR_CTRL_REG4));
        //pc.printf("CTRL_REG5 = 0x%.2x\r\n", getData(ADDR_CTRL_REG5));
        
    }
}