File content as of revision 4:c79a3c86ab36:

#include "mbed.h"

/*Timer*/
Timer t;

/* defines the axis for acc */
#define ACC_NOOF_AXIS       3
#define GYR_NOOF_AXIS       2

/* bmi160 slave address */
#define BMI160_ADDR         ((0x68)<<1)

/*Valor para Transformar de Radiano para Graus*/
#define RAD_DEG           57.29577951

/*Comunicacao LPCXpresso4337*/ 
Serial pc(USBTX, USBRX); // tx, rx

/*Comunicacao I2C*/
I2C i2c(P2_3, P2_4);


/* buffer to store acc samples */
int16_t acc_sample_buffer[ACC_NOOF_AXIS] = {0x5555, 0x5555, 0x5555};
int16_t gyr_sample_buffer[GYR_NOOF_AXIS] = {0x5555, 0x5555};

double acc_result_buffer[ACC_NOOF_AXIS] = {0x5555, 0x5555, 0x5555};
double gyr_result_buffer[GYR_NOOF_AXIS] = {0x5555, 0x5555};

double accel_ang_x, accel_ang_y;
double tiltx, tilty;
double tiltx_prev, tilty_prev;

char i2c_reg_buffer[2] = {0};
 
int main() {
    pc.printf("Teste BMI160\n\r");
    pc.printf("Configurando BMI160...\n\r");
    wait_ms(250);
    
    /*Config Freq. I2C Bus*/
    i2c.frequency(20000);
    
    /*Reset BMI160*/
    i2c_reg_buffer[0] = 0x7E;
    i2c_reg_buffer[1] = 0xB6;    
    i2c.write(BMI160_ADDR, i2c_reg_buffer, sizeof(i2c_reg_buffer), false);
    wait_ms(200);
    pc.printf("BMI160 Resetado\n\r");
    
    /*Habilita o Acelerometro*/
    i2c_reg_buffer[0] = 0x7E;
    i2c_reg_buffer[1] = 0x11; //PMU Normal   
    i2c.write(BMI160_ADDR, i2c_reg_buffer, sizeof(i2c_reg_buffer), false);
    pc.printf("Acc Habilitado\n\r");
    
    /*Habilita o Giroscopio*/
    i2c_reg_buffer[0] = 0x7E;
    i2c_reg_buffer[1] = 0x15;  //PMU Normal 
    i2c.write(BMI160_ADDR, i2c_reg_buffer, sizeof(i2c_reg_buffer), false);
    pc.printf("Gyr Habilitado\n\r");
    
    /*Config o Data Rate ACC em 1600Hz*/
    i2c_reg_buffer[0] = 0x40;
    i2c_reg_buffer[1] = 0x2C;    
    i2c.write(BMI160_ADDR, i2c_reg_buffer, sizeof(i2c_reg_buffer), false);
    pc.printf("Data Rate ACC Selecionado a 1600Hz\n\r");
    
    /*Config o Data Rate GYR em 1600Hz*/
    i2c_reg_buffer[0] = 0x42;
    i2c_reg_buffer[1] = 0x2C;    
    i2c.write(BMI160_ADDR, i2c_reg_buffer, sizeof(i2c_reg_buffer), false);
    pc.printf("Data Rate GYR Selecionado a 1600Hz\n\r");
    
    /*Config o Range GYR em 250º/s*/
    i2c_reg_buffer[0] = 0x43;
    i2c_reg_buffer[1] = 0x03;    
    i2c.write(BMI160_ADDR, i2c_reg_buffer, sizeof(i2c_reg_buffer), false);
    pc.printf("Range GYR Selecionado a 250deg/s\n\r");
    
    wait_ms(2000);
    pc.printf("BMI160 Configurado\n\r");
    
        
    while(1) {
        
        /*Le os Registradores do Acelerometro*/
        i2c_reg_buffer[0] = 0x12;
        i2c.write(BMI160_ADDR, i2c_reg_buffer, 1, true);
        i2c.read(BMI160_ADDR, (char *)&acc_sample_buffer, sizeof(acc_sample_buffer), false);
        
        /*Le os Registradores do Giroscopio*/
        i2c_reg_buffer[0] = 0x0C;
        i2c.write(BMI160_ADDR, i2c_reg_buffer, 1, true);
        i2c.read(BMI160_ADDR, (char *)&gyr_sample_buffer, sizeof(gyr_sample_buffer), false);
        
        /*Ajusta dados brutos Acelerometro em unidades de g */
        acc_result_buffer[0] = (acc_sample_buffer[0]/16384.0);
        acc_result_buffer[1] = (acc_sample_buffer[1]/16384.0);
        acc_result_buffer[2] = (acc_sample_buffer[2]/16384.0);
        
        /*Ajusta dados Brutos do Giroscopio em unidades de deg/s */
        gyr_result_buffer[0] = (gyr_sample_buffer[0]/131.2);
        gyr_result_buffer[1] = (gyr_sample_buffer[1]/131.2);
                
        /*Calcula os Angulos de Inclinacao com valor do Acelerometro*/
        accel_ang_x=atan(acc_result_buffer[0]/sqrt(pow(acc_result_buffer[1],2) + pow(acc_result_buffer[2],2)))*RAD_DEG;
        accel_ang_y=atan(acc_result_buffer[1]/sqrt(pow(acc_result_buffer[0],2) + pow(acc_result_buffer[2],2)))*RAD_DEG;
        
        /*Para o Timer*/
        t.stop();
                
        /*Calcula os Angulos de Rotacao com valor do Giroscopio e aplica filtro complementar realizando a fusao*/
        tiltx = (0.965*(tiltx_prev+(gyr_result_buffer[0]*t.read())))+(0.035*(accel_ang_x));
        tilty = (0.965*(tilty_prev+(gyr_result_buffer[1]*t.read())))+(0.035*(accel_ang_y));
        
        /*Debug para encontrar o tempo do loop*/
        //pc.printf("%f",t.read());
        
        /*Reseta o Timer*/
        t.reset();
        /*Inicia o Timer*/
        t.start();
        
        tiltx_prev = tiltx;
        tilty_prev = tilty;                                 
        
        /*Imprime os dados ACC pre-formatados*/
        pc.printf("%.3f,%.3f\n\r",tiltx, tilty);


         
        wait_ms(1);
    }
}