File content as of revision 0:3df2d4d2d393:

#include "mbed.h"
#include "test_BMX055.h"

test_BMX055::test_BMX055(PinName sda,PinName scl):i2c(sda ,scl){
    
    //I2C initialization
    i2c.frequency(400000); //400 kHz
    
    currentGyroRange = 0;
    currentAcceleroRange=0;
    gravity=9.80665;
    RAD_TO_DEG=57.3;
    }

test_BMX055::~test_BMX055(){
    }

void test_BMX055::i2c_mem_write(int device_address, int mem_address, int mem_data){
    
    int device_address_temp = device_address<<1;
    device_address_temp = device_address_temp & 0xfe;

    i2c.start();
    i2c.write(device_address_temp);
    i2c.write(mem_address);
    i2c.write(mem_data); 
    i2c.stop();   
    return;
    
    }
    
//i2c read function
int test_BMX055::i2c_mem_read(int device_address, int mem_address)
{   
    int device_address_temp = device_address<<1;
    int device_address_temp_w = device_address_temp & 0xfe; 
    int device_address_temp_r = device_address_temp | 0x01;

    i2c.start();
    i2c.write(device_address_temp_w);
    i2c.write(mem_address);  
    i2c.start();
    i2c.write(device_address_temp_r);    
    int data = i2c.read(0);
    i2c.stop();   
    return data;
}

void test_BMX055::test_BMX055::set_gyro_range(int range){
    
    currentGyroRange=range;
    
    if(range==0){
        i2c_mem_write(GYRO_ADDRESS, GYRO_RANGE_SET, 0x00);
        gyro_scale_factor=0.061f;
    }else if(range==1){
        i2c_mem_write(GYRO_ADDRESS, GYRO_RANGE_SET, 0x01);
        gyro_scale_factor=0.0305f;
    }else if(range==2){
        i2c_mem_write(GYRO_ADDRESS, GYRO_RANGE_SET, 0x02);
        gyro_scale_factor=0.0153f;
    }else if(range==3){
        i2c_mem_write(GYRO_ADDRESS, GYRO_RANGE_SET, 0x03);
        gyro_scale_factor=0.0076f;
    }else if(range==4){
        i2c_mem_write(GYRO_ADDRESS, GYRO_RANGE_SET, 0x04);
        gyro_scale_factor=0.0038f;
    }else{}
 
    }
    
float test_BMX055::get_gyro_x_data()
{
    float x_rate;
    
    //read RATE_X_LSB registor (0x02)
    int x_temp_L = i2c_mem_read(GYRO_ADDRESS, GYRO_X_L);
    //read RATE_X_MSB registor
    int x_temp_H = i2c_mem_read(GYRO_ADDRESS, GYRO_X_H);
    
    //calculate X angular ratio
    int x_data = x_temp_L + 256 * x_temp_H;
    if(x_data > 32767)
    {
        x_data = -1 * (65536 - x_data);    
    }
    
    x_rate=(float(x_data) * gyro_scale_factor)/RAD_TO_DEG; // rad/sec
    
    return x_rate;
}

    
float test_BMX055::get_gyro_y_data()
{
    float y_rate;
    
    //read RATE_Y_LSB registor (0x04)
    int y_temp_L = i2c_mem_read(GYRO_ADDRESS, GYRO_Y_L);
    //read RATE_Y_MSB registor
    int y_temp_H = i2c_mem_read(GYRO_ADDRESS, GYRO_Y_H);
    
    //calculate Y angular ratio
    int y_data = y_temp_L + 256 * y_temp_H;
    if(y_data > 32767)
    {
        y_data = -1 * (65536 - y_data);    
    }

    y_rate=(float(y_data) * gyro_scale_factor)/RAD_TO_DEG; // deg/sec
    
    return y_rate;
}

float test_BMX055::get_gyro_z_data()
{
    float z_rate;
    
    //read RATE_Z_LSB registor (0x04)
    int z_temp_L = i2c_mem_read(GYRO_ADDRESS, GYRO_Z_L);
    //read RATE_Z_MSB registor
    int z_temp_H = i2c_mem_read(GYRO_ADDRESS, GYRO_Z_H);
    
    //calculate Z angular ratio
    int z_data = z_temp_L + 256 * z_temp_H;
    if(z_data > 32767)
    {
        z_data = -1 * (65536 - z_data);    
    }

    z_data = -1 * z_data;

    z_rate=(float(z_data) * gyro_scale_factor)/RAD_TO_DEG; // deg/sec
    
    return z_rate;
}

void test_BMX055::test_BMX055::set_acc_range(int range){
    
    currentAcceleroRange=range;
    
    if(range==0){// +-2G
        i2c_mem_write(ACC_ADDRESS, ACC_RANGE_SET, 0x03);
        acc_scale_factor=1024;
    }else if(range==1){//+-4G
        i2c_mem_write(ACC_ADDRESS, ACC_RANGE_SET, 0x05);
        acc_scale_factor=512;
    }else if(range==2){
        i2c_mem_write(ACC_ADDRESS, ACC_RANGE_SET, 0x08);
        acc_scale_factor=256;
    }else if(range==3){
        i2c_mem_write(ACC_ADDRESS, ACC_RANGE_SET, 0x0c);
        acc_scale_factor=128;
    }else{}
 
}

float test_BMX055::get_acc_x_data()
{
    float x_acc;
    
    //read ACC_X_LSB registor (0x02)
    int x_temp_L = i2c_mem_read(ACC_ADDRESS, ACC_X_L);
    x_temp_L = x_temp_L >> 4;
    x_temp_L = x_temp_L & 0x0f;
    
    //read ACC_X_MSB registor (0x03)
    int x_temp_H = i2c_mem_read(ACC_ADDRESS, ACC_X_H);
    
    //calculate Z acceleration
    int x_data = x_temp_L + 16 * x_temp_H;
    if(x_data > 2047)
    {
        x_data = -1 * (4096 - x_data);    
    }
    
    x_acc=float(x_data)*gravity/acc_scale_factor;
    
    return x_acc;
}

float test_BMX055::get_acc_y_data()
{
    float y_acc;
    
    //read ACC_X_LSB registor (0x04)
    int y_temp_L = i2c_mem_read(ACC_ADDRESS, ACC_Y_L);
    y_temp_L = y_temp_L >> 4;
    y_temp_L = y_temp_L & 0x0f;
    
    //read ACC_X_MSB registor (0x05)
    int y_temp_H = i2c_mem_read(ACC_ADDRESS, ACC_Y_H);
    
    //calculate Z acceleration
    int y_data = y_temp_L + 16 * y_temp_H;
    if(y_data > 2047)
    {
        y_data = -1 * (4096 - y_data);    
    }
    
    y_acc=float(y_data)*gravity/acc_scale_factor;
    
    return y_acc;
}
    
float test_BMX055::get_acc_z_data()
{
    float z_acc;
    
    //read ACC_Z_LSB registor (0x06)
    int z_temp_L = i2c_mem_read(ACC_ADDRESS, ACC_Z_L);
    z_temp_L = z_temp_L >> 4;
    z_temp_L = z_temp_L & 0x0f;
    
    //read ACC_Z_MSB registor (0x07)
    int z_temp_H = i2c_mem_read(ACC_ADDRESS, ACC_Z_H);
    
    //calculate Z acceleration
    int z_data = z_temp_L + 16 * z_temp_H;
    if(z_data > 2047)
    {
        z_data = (4096 - z_data);    
    }
    
    z_acc=float(z_data)*gravity/acc_scale_factor;
    
    return z_acc;
}

void test_BMX055::initiate_mag()
{
    i2c_mem_write(MAG_ADDRESS, 0x4b, 0x83);
    wait_ms(10);
    i2c_mem_write(MAG_ADDRESS, 0x4b, 0x01);
    wait_ms(10);
    i2c_mem_write(MAG_ADDRESS, 0x4c, 0x00);
    wait_ms(10);
    i2c_mem_write(MAG_ADDRESS, 0x4c, 0x00);
    wait_ms(10);
    i2c_mem_write(MAG_ADDRESS, 0x4e, 0x84);
    wait_ms(10);    
    i2c_mem_write(MAG_ADDRESS, 0x51, 0x04);
    wait_ms(10);
    i2c_mem_write(MAG_ADDRESS, 0x52, 0x16);
    wait_ms(10);
    
        
    
}
    
float test_BMX055::get_mag_x_data()
{
    float x_mag;
    
    //read MAG_X_LSB registor (0x42)
    int x_temp_L = i2c_mem_read(MAG_ADDRESS, MAG_X_L);
    x_temp_L = x_temp_L >> 3;
    x_temp_L = x_temp_L & 0x0f;
    
    //read MAG_X_MSB registor (0x43)
    int x_temp_H = i2c_mem_read(MAG_ADDRESS, MAG_X_H);
    
    //calculate Z acceleration
    int x_data = x_temp_L +  32* x_temp_H;
    if(x_data > 4095)
    {
        x_data = -1 * (8192 - x_data);    
    }
    
    x_mag=float(x_data);
    
    return x_mag;
    
}

float test_BMX055::get_mag_y_data()
{
    float y_mag;
    
    //read MAG_Y_LSB registor (0x44)
    int y_temp_L = i2c_mem_read(MAG_ADDRESS, MAG_Y_L);
    y_temp_L = y_temp_L >> 3;
    y_temp_L = y_temp_L & 0x0f;
    
    //read MAG_Y_MSB registor (0x45)
    int y_temp_H = i2c_mem_read(MAG_ADDRESS, MAG_Y_H);
    
    //calculate Z acceleration
    int y_data = y_temp_L +  32* y_temp_H;
    if(y_data > 4095)
    {
        y_data = -1 * (8192 - y_data);    
    }
    
    y_mag= - 1 * float(y_data);
    
    return y_mag;
    
}

float test_BMX055::get_mag_z_data()
{
    float z_mag;
    
    //read MAG_Z_LSB registor (0x46)
    int z_temp_L = i2c_mem_read(MAG_ADDRESS, MAG_Z_L);
    z_temp_L = z_temp_L >> 1;
    z_temp_L = z_temp_L & 0x0f;
    
    //read MAG_Z_MSB registor (0x47)
    int z_temp_H = i2c_mem_read(MAG_ADDRESS, MAG_Z_H);
    
    //calculate Z acceleration
    int z_data = z_temp_L +  128* z_temp_H;
    if(z_data > 16383)
    {
        z_data = -1 * (32768 - z_data);    
    }
    
    z_mag= - 1* float(z_data);
    
    return z_mag;
    
}