File content as of revision 1:6de1be86a23f:

#include "mbed.h"
#include "BNO055_lib.h"

BNO055_lib::BNO055_lib(I2C &user_i2c, AD0 ad0){
    slave = ad0;
    i2c = &user_i2c;
    
    i2c -> frequency(400000);
    
    setAccRange(_16G);
    setOperationMode(NDOF);
}

int BNO055_lib::connectCheck(){
    cmd[0] = BNO055_CHIP_ID;
    i2c -> write(slave << 1, cmd, 1);
    i2c -> read(slave << 1 | 1, buff, 1);
    
    if(buff[0] == 0xA0){
        return 1;
    }
    else{
        return 0;
    }
}

void BNO055_lib::setAccRange(ACC_RANGE range){
    cmd[0] = BNO055_PAGE_ID;
    cmd[1] = 0x01;
    i2c -> write(slave << 1, cmd, 2);
    
    cmd[0] = BNO055_ACC_CONFIG;
    cmd[1] = (char)range | 0x0C;
    i2c -> write(slave << 1, cmd, 2);
    i2c -> write(slave << 1, cmd, 1);
    i2c -> read(slave << 1 | 1, buff, 1);
    
    cmd[0] = BNO055_PAGE_ID;
    cmd[1] = 0x00;
    i2c -> write(slave << 1, cmd, 2);
}

void BNO055_lib::setOperationMode(OPERATION_MODE mode){
    wait(0.05f);
    cmd[0] = BNO055_OPR_MODE;
    cmd[1] = (char)mode;
    i2c -> write(slave << 1, cmd, 2);
    wait(0.05f);
}

void BNO055_lib::setAxis(AXIS x, AXIS y, AXIS z){
    if((x != y) && (y != z) && (z != x)){
        cmd[0] = BNO055_AXIS_MAP_CONFIG;
        cmd[1] = ((char)z << 4) | ((char)y << 2) | (char)x;
        i2c -> write(slave << 1, cmd, 2);
    }
}

void BNO055_lib::setAxisPM(int x_pm, int y_pm, int z_pm){
    cmd[0] = BNO055_AXIS_MAP_SIGN;
    cmd[1] = 0x00;
    if(x_pm < 0){
        cmd[1] = cmd[1] | 0x04;
    }
    if(y_pm < 0){
        cmd[1] = cmd[1] | 0x02;
    }
    if(z_pm < 0){
        cmd[1] = cmd[1] | 0x01;
    }
    i2c -> write(slave << 1, cmd, 2);
}

void BNO055_lib::getAcc(float *acc){
    cmd[0] = BNO055_ACC_DATA;
    i2c -> write(slave << 1, cmd, 1);
    i2c -> read(slave << 1 | 1, buff, 6);
    data[0] = (short)buff[1] << 8 | (short)buff[0];
    data[1] = (short)buff[3] << 8 | (short)buff[2];
    data[2] = (short)buff[5] << 8 | (short)buff[4];
    
    acc[0] = (float)data[0] / 100.0;
    acc[1] = (float)data[1] / 100.0;
    acc[2] = (float)data[2] / 100.0;
}

void BNO055_lib::getAcc(double *acc){
    cmd[0] = BNO055_ACC_DATA;
    i2c -> write(slave << 1, cmd, 1);
    i2c -> read(slave << 1 | 1, buff, 6);
    data[0] = (short)buff[1] << 8 | (short)buff[0];
    data[1] = (short)buff[3] << 8 | (short)buff[2];
    data[2] = (short)buff[5] << 8 | (short)buff[4];
    
    acc[0] = (double)data[0] / 100.0;
    acc[1] = (double)data[1] / 100.0;
    acc[2] = (double)data[2] / 100.0;
}

void BNO055_lib::getMag(float *mag){
    cmd[0] = BNO055_MAG_DATA;
    i2c -> write(slave << 1, cmd, 1);
    i2c -> read(slave << 1 | 1, buff, 6);
    data[0] = (short)buff[1] << 8 | (short)buff[0];
    data[1] = (short)buff[3] << 8 | (short)buff[2];
    data[2] = (short)buff[5] << 8 | (short)buff[4];
    
    mag[0] = (float)data[0] / 16.0;
    mag[1] = (float)data[1] / 16.0;
    mag[2] = (float)data[2] / 16.0;
}

void BNO055_lib::getMag(double *mag){
    cmd[0] = BNO055_MAG_DATA;
    i2c -> write(slave << 1, cmd, 1);
    i2c -> read(slave << 1 | 1, buff, 6);
    data[0] = (short)buff[1] << 8 | (short)buff[0];
    data[1] = (short)buff[3] << 8 | (short)buff[2];
    data[2] = (short)buff[5] << 8 | (short)buff[4];
    
    mag[0] = (double)data[0] / 16.0;
    mag[1] = (double)data[1] / 16.0;
    mag[2] = (double)data[2] / 16.0;
}

void BNO055_lib::getGyro(float *gyro){
    cmd[0] = BNO055_GYR_DATA;
    i2c -> write(slave << 1, cmd, 1);
    i2c -> read(slave << 1 | 1, buff, 6);
    data[0] = (short)buff[1] << 8 | (short)buff[0];
    data[1] = (short)buff[3] << 8 | (short)buff[2];
    data[2] = (short)buff[5] << 8 | (short)buff[4];
    
    gyro[0] = (float)data[0] / 16.0;
    gyro[1] = (float)data[1] / 16.0;
    gyro[2] = (float)data[2] / 16.0;
}

void BNO055_lib::getGyro(double *gyro){
    cmd[0] = BNO055_GYR_DATA;
    i2c -> write(slave << 1, cmd, 1);
    i2c -> read(slave << 1 | 1, buff, 6);
    data[0] = (short)buff[1] << 8 | (short)buff[0];
    data[1] = (short)buff[3] << 8 | (short)buff[2];
    data[2] = (short)buff[5] << 8 | (short)buff[4];
    
    gyro[0] = (double)data[0] / 16.0;
    gyro[1] = (double)data[1] / 16.0;
    gyro[2] = (double)data[2] / 16.0;
}

void BNO055_lib::getAMG(float *amg){
    cmd[0] = BNO055_ACC_DATA;
    i2c -> write(slave << 1, cmd, 1);
    i2c -> read(slave << 1 | 1, buff, 18);
    
    data[0] = (short)buff[1] << 8 | (short)buff[0];
    data[1] = (short)buff[3] << 8 | (short)buff[2];
    data[2] = (short)buff[5] << 8 | (short)buff[4];
    amg[0] = (float)data[0] / 100.0;
    amg[1] = (float)data[1] / 100.0;
    amg[2] = (float)data[2] / 100.0;
    
    data[0] = (short)buff[7] << 8 | (short)buff[6];
    data[1] = (short)buff[9] << 8 | (short)buff[8];
    data[2] = (short)buff[11] << 8 | (short)buff[10];
    amg[3] = (float)data[0] / 16.0;
    amg[4] = (float)data[1] / 16.0;
    amg[5] = (float)data[2] / 16.0;
    
    data[0] = (short)buff[13] << 8 | (short)buff[12];
    data[1] = (short)buff[15] << 8 | (short)buff[14];
    data[2] = (short)buff[17] << 8 | (short)buff[16];
    amg[6] = (float)data[0] / 16.0;
    amg[7] = (float)data[1] / 16.0;
    amg[8] = (float)data[2] / 16.0;
}

void BNO055_lib::getAMG(double *amg){
    cmd[0] = BNO055_ACC_DATA;
    i2c -> write(slave << 1, cmd, 1);
    i2c -> read(slave << 1 | 1, buff, 18);
    
    data[0] = (short)buff[1] << 8 | (short)buff[0];
    data[1] = (short)buff[3] << 8 | (short)buff[2];
    data[2] = (short)buff[5] << 8 | (short)buff[4];
    amg[0] = (double)data[0] / 100.0;
    amg[1] = (double)data[1] / 100.0;
    amg[2] = (double)data[2] / 100.0;
    
    data[0] = (short)buff[7] << 8 | (short)buff[6];
    data[1] = (short)buff[9] << 8 | (short)buff[8];
    data[2] = (short)buff[11] << 8 | (short)buff[10];
    amg[3] = (double)data[0] / 16.0;
    amg[4] = (double)data[1] / 16.0;
    amg[5] = (double)data[2] / 16.0;
    
    data[0] = (short)buff[13] << 8 | (short)buff[12];
    data[1] = (short)buff[15] << 8 | (short)buff[14];
    data[2] = (short)buff[17] << 8 | (short)buff[16];
    amg[6] = (double)data[0] / 16.0;
    amg[7] = (double)data[1] / 16.0;
    amg[8] = (double)data[2] / 16.0;
}
    
void BNO055_lib::getEuler(float *euler){
    cmd[0] = BNO055_EUL_DATA;
    i2c -> write(slave << 1, cmd, 1);
    i2c -> read(slave << 1 | 1, buff, 6);
    data[0] = (short)buff[1] << 8 | (short)buff[0];
    data[1] = (short)buff[3] << 8 | (short)buff[2];
    data[2] = (short)buff[5] << 8 | (short)buff[4];
    
    euler[0] = (float)data[0] / 16.0;
    euler[1] = (float)data[1] / 16.0;
    euler[2] = (float)data[2] / 16.0;
}

void BNO055_lib::getEuler(double *euler){
    cmd[0] = BNO055_EUL_DATA;
    i2c -> write(slave << 1, cmd, 1);
    i2c -> read(slave << 1 | 1, buff, 6);
    data[0] = (short)buff[1] << 8 | (short)buff[0];
    data[1] = (short)buff[3] << 8 | (short)buff[2];
    data[2] = (short)buff[5] << 8 | (short)buff[4];
    
    euler[0] = (double)data[0] / 16.0;
    euler[1] = (double)data[1] / 16.0;
    euler[2] = (double)data[2] / 16.0;
}

void BNO055_lib::getQuart(float *quart){
    cmd[0] = BNO055_QUA_DATA;
    i2c -> write(slave << 1, cmd, 1);
    i2c -> read(slave << 1 | 1, buff, 8);
    data[0] = (short)buff[1] << 8 | (short)buff[0];
    data[1] = (short)buff[3] << 8 | (short)buff[2];
    data[2] = (short)buff[5] << 8 | (short)buff[4];
    data[3] = (short)buff[7] << 8 | (short)buff[6];
    
    quart[0] = (float)(data[0] / 16384.0);
    quart[1] = (float)(data[1] / 16384.0);
    quart[2] = (float)(data[2] / 16384.0);
    quart[3] = (float)(data[3] / 16384.0);
}

void BNO055_lib::getQuart(double *quart){
    cmd[0] = BNO055_QUA_DATA;
    i2c -> write(slave << 1, cmd, 1);
    i2c -> read(slave << 1 | 1, buff, 8);
    data[0] = (short)buff[1] << 8 | (short)buff[0];
    data[1] = (short)buff[3] << 8 | (short)buff[2];
    data[2] = (short)buff[5] << 8 | (short)buff[4];
    data[3] = (short)buff[7] << 8 | (short)buff[6];
    
    quart[0] = (double)(data[0] / 16384.0);
    quart[1] = (double)(data[1] / 16384.0);
    quart[2] = (double)(data[2] / 16384.0);
    quart[3] = (double)(data[3] / 16384.0);
}

void BNO055_lib::getEulerFromQ(float *euler){
    getQuart(data_d);
    /***************
    data_d[0] : w
    data_d[1] : x
    data_d[2] : y
    data_d[3] : z
    ***************/
    
    yy = data_d[2] * data_d[2];
    
    t0 = 2.0 * (data_d[0] * data_d[3] + data_d[1] * data_d[2]);
    t1 = 1.0 - 2.0 * (yy + data_d[3] * data_d[3]);
    euler[0] = (float)atan2(t0, t1) * 57.2957795131;
    
    t0 = 2.0 * (data_d[0] * data_d[1] + data_d[2] * data_d[3]);
    t1 = 1.0 - 2.0 * (data_d[1] * data_d[1] + yy);
    euler[1] = (float)atan2(t0, t1) * 57.2957795131;
    
    t0 = 2.0 * (data_d[0] * data_d[2] - data_d[3] * data_d[1]);
    t0 = (t0 > 1.0) ? 1.0 : t0;
    t0 = (t0 < -1.0) ? -1.0 : t0;
    euler[2] = (float)asin(t0) * 57.2957795131;
}

void BNO055_lib::getEulerFromQ(double *euler){
    getQuart(data_d);
    /***************
    data_d[0] : w
    data_d[1] : x
    data_d[2] : y
    data_d[3] : z
    ***************/
    
    yy = data_d[2] * data_d[2];
    
    t0 = 2.0 * (data_d[0] * data_d[3] + data_d[1] * data_d[2]);
    t1 = 1.0 - 2.0 * (yy + data_d[3] * data_d[3]);
    euler[0] = atan2(t0, t1) * 57.2957795131;
    
    t0 = 2.0 * (data_d[0] * data_d[1] + data_d[2] * data_d[3]);
    t1 = 1.0 - 2.0 * (data_d[1] * data_d[1] + yy);
    euler[1] = atan2(t0, t1) * 57.2957795131;
    
    t0 = 2.0 * (data_d[0] * data_d[2] - data_d[3] * data_d[1]);
    t0 = (t0 > 1.0) ? 1.0 : t0;
    t0 = (t0 < -1.0) ? -1.0 : t0;
    euler[2] = asin(t0) * 57.2957795131;
}

void BNO055_lib::getLinearAcc(float *lia){
    cmd[0] = BNO055_LIA_DATA;
    i2c -> write(slave << 1, cmd, 1);
    i2c -> read(slave << 1 | 1, buff, 6);
    data[0] = (short)buff[1] << 8 | (short)buff[0];
    data[1] = (short)buff[3] << 8 | (short)buff[2];
    data[2] = (short)buff[5] << 8 | (short)buff[4];
    
    lia[0] = (float)data[0] / 100.0;
    lia[1] = (float)data[1] / 100.0;
    lia[2] = (float)data[2] / 100.0;
}

void BNO055_lib::getLinearAcc(double *lia){
    cmd[0] = BNO055_LIA_DATA;
    i2c -> write(slave << 1, cmd, 1);
    i2c -> read(slave << 1 | 1, buff, 6);
    data[0] = (short)buff[1] << 8 | (short)buff[0];
    data[1] = (short)buff[3] << 8 | (short)buff[2];
    data[2] = (short)buff[5] << 8 | (short)buff[4];
    
    lia[0] = (double)data[0] / 100.0;
    lia[1] = (double)data[1] / 100.0;
    lia[2] = (double)data[2] / 100.0;
}

void BNO055_lib::getGravity(float *grv){
    cmd[0] = BNO055_GRV_DATA;
    i2c -> write(slave << 1, cmd, 1);
    i2c -> read(slave << 1 | 1, buff, 6);
    data[0] = (short)buff[1] << 8 | (short)buff[0];
    data[1] = (short)buff[3] << 8 | (short)buff[2];
    data[2] = (short)buff[5] << 8 | (short)buff[4];
    
    grv[0] = (float)data[0] / 100.0;
    grv[1] = (float)data[1] / 100.0;
    grv[2] = (float)data[2] / 100.0;
}

void BNO055_lib::getGravity(double *grv){
    cmd[0] = BNO055_GRV_DATA;
    i2c -> write(slave << 1, cmd, 1);
    i2c -> read(slave << 1 | 1, buff, 6);
    data[0] = (short)buff[1] << 8 | (short)buff[0];
    data[1] = (short)buff[3] << 8 | (short)buff[2];
    data[2] = (short)buff[5] << 8 | (short)buff[4];
    
    grv[0] = (double)data[0] / 100.0;
    grv[1] = (double)data[1] / 100.0;
    grv[2] = (double)data[2] / 100.0;
}