File content as of revision 2:f9ddabfe2eb6:

#include "BMI160.h"

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

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

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

/* buffer to store acc samples */
int16_t acc_sample_buffer[ACC_NOOF_AXIS] = {0x5555, 0x5555, 0x5555};
double acc_result_buffer[ACC_NOOF_AXIS] = {0x5555, 0x5555, 0x5555};
double accel_ang_x, accel_ang_y;
 
char i2c_reg_buffer[2] = {0};

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

int currentState;
int previousState;

void BMI160::configureAccelerometer(){
    currentState = 0;
    previousState = -1;

    
    /*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);

    /*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);
    
    /*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);
    
}

void BMI160::readAccelerometer(){

    /*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);
    /*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);
                
    /*Calcula os Angulos de Inclinacao com valor do Acelerometro*/
    accel_ang_y=atan(acc_result_buffer[1]/sqrt(pow(acc_result_buffer[0],2) + pow(acc_result_buffer[2],2)))*RAD_DEG;

}

int BMI160::getAngle(){

        readAccelerometer();
        
        if(currentState == 0 && accel_ang_y < -20){
            currentState = 1;
            previousState = 0;    
        }
        else if(currentState == 0 && accel_ang_y > 20){
            currentState = 2;
            previousState = 0;
        }
        else if(currentState == 1 && accel_ang_y > -20 && accel_ang_y < 20){
            currentState = 0;
            previousState = 1;  
        }  
        else if(currentState == 1 && accel_ang_y > -20 && accel_ang_y < 20){
            currentState = 0;
            previousState = 1;  
        } 
        else if(currentState == 2 && accel_ang_y > -20 && accel_ang_y < 20){
            currentState = 0;
            previousState = 2;
        }
        
        if(currentState == 1 && previousState == 0) 
            return 90;
        else if(currentState == 2 && previousState == 0)
            return 270;
        else if(currentState == 0 && previousState == 1)
            return 0;
        else if(currentState == 0 && previousState == 2)
            return 180;
        else
            return -1;
        
}