Diff: BNO055.cpp

Revision:

5:a44318a8bad4

Parent:

4:9e6fead1e93e

Child:

6:ae4be69ac7bb

--- a/BNO055.cpp	Thu Apr 16 10:47:40 2015 +0000
+++ b/BNO055.cpp	Wed Aug 17 20:30:46 2016 +0000
@@ -1,483 +1,321 @@
-/*
- * mbed library program
- *  BNO055 Intelligent 9-axis absolute orientation sensor
- *  by Bosch Sensortec
- *
- * Copyright (c) 2015 Kenji Arai / JH1PJL
- *  http://www.page.sannet.ne.jp/kenjia/index.html
- *  http://mbed.org/users/kenjiArai/
- *      Created: March     30th, 2015
- *      Revised: April     16th, 2015
- *
- * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED,
- * INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE
- * AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM,
- * DAMAGES OR OTHER  LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
- * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
- */
-
+#include "BNO055.h"
 #include "mbed.h"
-#include "BNO055.h"
 
-BNO055::BNO055 (PinName p_sda, PinName p_scl, PinName p_reset, uint8_t addr, uint8_t mode):
-    _i2c(p_sda, p_scl), _res(p_reset)
-{
-    chip_addr = addr;
-    chip_mode = mode;
-    initialize ();
+BNO055::BNO055(PinName SDA, PinName SCL) : _i2c(SDA,SCL){
+    //Set I2C fast and bring reset line high
+    _i2c.frequency(400000);
+    address = BNOAddress;
+    accel_scale = 0.001f;
+    rate_scale = 1.0f/16.0f;
+    angle_scale = 1.0f/16.0f;
+    temp_scale = 1;
+    }
+    
+void BNO055::reset(){
+//Perform a power-on-reset
+    readchar(BNO055_SYS_TRIGGER_ADDR);
+    rx = rx | 0x20;
+    writechar(BNO055_SYS_TRIGGER_ADDR,rx);
+//Wait for the system to come back up again (datasheet says 650ms)
+    wait_ms(675);
 }
-
-BNO055::BNO055 (PinName p_sda, PinName p_scl, PinName p_reset) :
-    _i2c(p_sda, p_scl), _res(p_reset)
-{
-    chip_addr = BNO055_G_CHIP_ADDR;
-    chip_mode = MODE_NDOF;
-    initialize ();
-}
-
-BNO055::BNO055 (I2C& p_i2c, PinName p_reset, uint8_t addr, uint8_t mode) :
-    _i2c(p_i2c), _res(p_reset)
-{
-    chip_addr = addr;
-    chip_mode = mode;
-    initialize ();
+    
+bool BNO055::check(){
+//Check we have communication link with the chip
+    readchar(BNO055_CHIP_ID_ADDR);
+    if (rx != 0xA0) return false;
+//Grab the chip ID and software versions
+    tx[0] = BNO055_CHIP_ID_ADDR;
+    _i2c.write(address,tx,1,true);  
+    _i2c.read(address+1,rawdata,7,false); 
+    ID.id = rawdata[0];
+    ID.accel = rawdata[1];
+    ID.mag = rawdata[2];
+    ID.gyro = rawdata[3];
+    ID.sw[0] = rawdata[4];
+    ID.sw[1] = rawdata[5];
+    ID.bootload = rawdata[6];
+    setpage(1);
+    tx[0] = BNO055_UNIQUE_ID_ADDR;
+    _i2c.write(address,tx,1,true);  
+    _i2c.read(address+1,ID.serial,16,false); 
+    setpage(0);
+    return true;
+    }
+    
+void BNO055::SetExternalCrystal(bool yn){
+// Read the current status from the device
+    readchar(BNO055_SYS_TRIGGER_ADDR); 
+    if (yn) rx = rx | 0x80;
+    else rx = rx & 0x7F;
+    writechar(BNO055_SYS_TRIGGER_ADDR,rx); 
 }
 
-BNO055::BNO055 (I2C& p_i2c, PinName p_reset) :
-    _i2c(p_i2c), _res(p_reset)
-{
-    chip_addr = BNO055_G_CHIP_ADDR;
-    chip_mode = MODE_NDOF;
-    initialize ();
-}
-
-/////////////// Read data & normalize /////////////////////
-void BNO055::get_Euler_Angles(BNO055_EULER_TypeDef *el)
-{
-    uint8_t deg_or_rad;
-    int16_t h,p,r;
-
-    select_page(0);
-    dt[0] = BNO055_UNIT_SEL;
-    _i2c.write(chip_addr, dt, 1, true);
-    _i2c.read(chip_addr, dt, 1, false);
-    if (dt[0] & 0x04) {
-        deg_or_rad = 1; // Radian
-    } else {
-        deg_or_rad = 0; // Degree
-    }
-    dt[0] = BNO055_EULER_H_LSB;
-    _i2c.write(chip_addr, dt, 1, true);
-    _i2c.read(chip_addr, dt, 6, false);
-    h = dt[1] << 8 | dt[0];
-    p = dt[3] << 8 | dt[2];
-    r = dt[5] << 8 | dt[4];
-    if (deg_or_rad) {
-        el->h = (double)h / 900;
-        el->p = (double)p / 900;
-        el->r = (double)r / 900;
-    } else {
-        el->h = (double)h / 16;
-        el->p = (double)p / 16;
-        el->r = (double)r / 16;
-    }
-}
-
-void BNO055::get_quaternion(BNO055_QUATERNION_TypeDef *qua)
-{
-    select_page(0);
-    dt[0] = BNO055_QUATERNION_W_LSB;
-    _i2c.write(chip_addr, dt, 1, true);
-    _i2c.read(chip_addr, dt, 8, false);
-    qua->w = dt[1] << 8 | dt[0];
-    qua->x = dt[3] << 8 | dt[2];
-    qua->y = dt[5] << 8 | dt[4];
-    qua->z = dt[7] << 8 | dt[6];
+void BNO055::set_accel_units(char units){
+    readchar(BNO055_UNIT_SEL_ADDR);
+    if(units == MPERSPERS){
+        rx = rx & 0xFE;
+        accel_scale = 0.01f;
+        }
+    else {
+        rx = rx | units;
+        accel_scale = 0.001f;
+        }
+    writechar(BNO055_UNIT_SEL_ADDR,rx);
 }
 
-void BNO055::get_linear_accel(BNO055_LIN_ACC_TypeDef *la)
-{
-    uint8_t ms2_or_mg;
-    int16_t x,y,z;
+void BNO055::set_anglerate_units(char units){
+    readchar(BNO055_UNIT_SEL_ADDR);
+    if (units == DEG_PER_SEC){
+        rx = rx & 0xFD;
+        rate_scale = 1.0f/16.0f;
+        }
+    else {
+        rx = rx | units;
+        rate_scale = 1.0f/900.0f;
+        }
+    writechar(BNO055_UNIT_SEL_ADDR,rx);
+}    
+
+void BNO055::set_angle_units(char units){
+    readchar(BNO055_UNIT_SEL_ADDR);
+    if (units == DEGREES){
+        rx = rx & 0xFB;
+        angle_scale = 1.0f/16.0f;
+        }
+    else {
+        rx = rx | units;
+        angle_scale = 1.0f/900.0f;
+        }
+    writechar(BNO055_UNIT_SEL_ADDR,rx);
+}    
+
+void BNO055::set_temp_units(char units){
+    readchar(BNO055_UNIT_SEL_ADDR);
+    if (units == CENTIGRADE){
+        rx = rx & 0xEF;
+        temp_scale = 1;
+        }
+    else {
+        rx = rx | units;
+        temp_scale = 2;
+        }
+    writechar(BNO055_UNIT_SEL_ADDR,rx);
+}    
 
-    select_page(0);
-    dt[0] = BNO055_UNIT_SEL;
-    _i2c.write(chip_addr, dt, 1, true);
-    _i2c.read(chip_addr, dt, 1, false);
-    if (dt[0] & 0x01) {
-        ms2_or_mg = 1; // mg
-    } else {
-        ms2_or_mg = 0; // m/s*s
-    }
-    dt[0] = BNO055_LINEAR_ACC_X_LSB;
-    _i2c.write(chip_addr, dt, 1, true);
-    _i2c.read(chip_addr, dt, 6, false);
-    x = dt[1] << 8 | dt[0];
-    y = dt[3] << 8 | dt[2];
-    z = dt[5] << 8 | dt[4];
-    if (ms2_or_mg) {
-        la->x = (double)x;
-        la->y = (double)y;
-        la->z = (double)z;
-    } else {
-        la->x = (double)x / 100;
-        la->y = (double)y / 100;
-        la->z = (double)z / 100;
+void BNO055::set_orientation(int position){
+    switch(position){
+        case 0:
+            writechar(BNO055_AXIS_MAP_CONFIG_ADDR,0x21);
+            writechar(BNO055_AXIS_MAP_SIGN_ADDR,0x04);
+            break;
+        case 1: //(Default)
+            writechar(BNO055_AXIS_MAP_CONFIG_ADDR,0x24);
+            writechar(BNO055_AXIS_MAP_SIGN_ADDR,0x00);
+            break;
+        case 2:
+            writechar(BNO055_AXIS_MAP_CONFIG_ADDR,0x24);
+            writechar(BNO055_AXIS_MAP_SIGN_ADDR,0x06);
+            break;  
+        case 3:
+            writechar(BNO055_AXIS_MAP_CONFIG_ADDR,0x21);
+            writechar(BNO055_AXIS_MAP_SIGN_ADDR,0x02);            
+            break;
+        case 4:
+            writechar(BNO055_AXIS_MAP_CONFIG_ADDR,0x24);
+            writechar(BNO055_AXIS_MAP_SIGN_ADDR,0x03);            
+            break;
+        case 5:
+            writechar(BNO055_AXIS_MAP_CONFIG_ADDR,0x21);
+            writechar(BNO055_AXIS_MAP_SIGN_ADDR,0x01);            
+            break;
+        case 6:
+            writechar(BNO055_AXIS_MAP_CONFIG_ADDR,0x21);
+            writechar(BNO055_AXIS_MAP_SIGN_ADDR,0x07);            
+            break;
+        case 7:
+            writechar(BNO055_AXIS_MAP_CONFIG_ADDR,0x24);
+            writechar(BNO055_AXIS_MAP_SIGN_ADDR,0x05);            
+            break;
+        default:
+            writechar(BNO055_AXIS_MAP_CONFIG_ADDR,0x24);
+            writechar(BNO055_AXIS_MAP_SIGN_ADDR,0x00);
+            break;                 
     }
-}
-
-void BNO055::get_gravity(BNO055_GRAVITY_TypeDef *gr)
-{
-    uint8_t ms2_or_mg;
-    int16_t x,y,z;
+}        
 
-    select_page(0);
-    dt[0] = BNO055_UNIT_SEL;
-    _i2c.write(chip_addr, dt, 1, true);
-    _i2c.read(chip_addr, dt, 1, false);
-    if (dt[0] & 0x01) {
-        ms2_or_mg = 1; // mg
-    } else {
-        ms2_or_mg = 0; // m/s*s
-    }
-    dt[0] = BNO055_GRAVITY_X_LSB;
-    _i2c.write(chip_addr, dt, 1, true);
-    _i2c.read(chip_addr, dt, 6, false);
-    x = dt[1] << 8 | dt[0];
-    y = dt[3] << 8 | dt[2];
-    z = dt[5] << 8 | dt[4];
-    if (ms2_or_mg) {
-        gr->x = (double)x;
-        gr->y = (double)y;
-        gr->z = (double)z;
-    } else {
-        gr->x = (double)x / 100;
-        gr->y = (double)y / 100;
-        gr->z = (double)z / 100;
-    }
+void BNO055::setmode(char omode){
+    writechar(BNO055_OPR_MODE_ADDR,omode);
+    op_mode = omode;
 }
 
-void BNO055::get_chip_temperature(BNO055_TEMPERATURE_TypeDef *tmp)
-{
-    uint8_t c_or_f;
-
-    select_page(0);
-    dt[0] = BNO055_UNIT_SEL;
-    _i2c.write(chip_addr, dt, 1, true);
-    _i2c.read(chip_addr, dt, 1, false);
-    if (dt[0] & 0x10) {
-        c_or_f = 1; // Fahrenheit
-    } else {
-        c_or_f = 0; // degrees Celsius
-    }
-    dt[0] = BNO055_TEMP_SOURCE;
-    dt[1] = 0;
-    _i2c.write(chip_addr, dt, 2, false);
-    wait_ms(1); // Do I need to wait?
-    dt[0] = BNO055_TEMP;
-    _i2c.write(chip_addr, dt, 1, true);
-    _i2c.read(chip_addr, dt, 1, false);
-    if (c_or_f) {
-        tmp->acc_chip = (int8_t)dt[0] * 2;
-    } else {
-        tmp->acc_chip = (int8_t)dt[0];
-    }
-    dt[0] = BNO055_TEMP_SOURCE;
-    dt[1] = 1;
-    _i2c.write(chip_addr, dt, 2, false);
-    wait_ms(1); // Do I need to wait?
-    dt[0] = BNO055_TEMP;
-    _i2c.write(chip_addr, dt, 1, true);
-    _i2c.read(chip_addr, dt, 1, false);
-    if (c_or_f) {
-        tmp->gyr_chip = (int8_t)dt[0] * 2;
-    } else {
-        tmp->gyr_chip = (int8_t)dt[0];
-    }
+void BNO055::setpowermode(char pmode){
+    writechar(BNO055_PWR_MODE_ADDR,pmode);
+    pwr_mode = pmode;
 }
 
-/////////////// Initialize ////////////////////////////////
-void BNO055::initialize (void)
-{
-#if defined(TARGET_STM32L152RE)
-    _i2c.frequency(100000);
-#else
-    _i2c.frequency(400000);
-#endif
-    page_flag = 0xff;
-    select_page(0);
-    // Check Acc & Mag & Gyro are available of not
-    check_id();
-    // Set initial data
-    set_initial_dt_to_regs();
-    // Unit selection
-    unit_selection();
-    // Set fusion mode
-    change_fusion_mode(chip_mode);
-}
-
-void BNO055::unit_selection(void)
-{
-    select_page(0);
-    dt[0] = BNO055_UNIT_SEL;
-    dt[1] = UNIT_ORI_WIN + UNIT_ACC_MSS + UNIT_GYR_DPS + UNIT_EULER_DEG + UNIT_TEMP_C;
-    _i2c.write(chip_addr, dt, 2, false);
+void BNO055::setoutputformat(char format){
+    writechar(BNO055_UNIT_SEL_ADDR, format);
+    _format = format;   
 }
 
-uint8_t BNO055::select_page(uint8_t page)
-{
-    if (page != page_flag){
-        dt[0] = BNO055_PAGE_ID;
-        if (page == 1) {
-            dt[1] = 1;  // select page 1
-        } else {
-            dt[1] = 0;  // select page 0
-        }
-        _i2c.write(chip_addr, dt, 2, false);
-        dt[0] = BNO055_PAGE_ID;
-        _i2c.write(chip_addr, dt, 1, true);
-        _i2c.read(chip_addr, dt, 1, false);
-        page_flag = dt[0];
-    }
-    return page_flag;
+void BNO055::get_accel(void){
+    tx[0] = BNO055_ACCEL_DATA_X_LSB_ADDR;
+    _i2c.write(address,tx,1,true);  
+    _i2c.read(address+1,rawdata,6,0); 
+    accel.rawx = (rawdata[1] << 8 | rawdata[0]);
+    accel.rawy = (rawdata[3] << 8 | rawdata[2]);
+    accel.rawz = (rawdata[5] << 8 | rawdata[4]);
+    accel.x = float(accel.rawx)*accel_scale;
+    accel.y = float(accel.rawy)*accel_scale*(-1.0);
+    accel.z = float(accel.rawz)*accel_scale*(-1.0);
+}
+    
+void BNO055::get_gyro(void){
+    tx[0] = BNO055_GYRO_DATA_X_LSB_ADDR;
+    _i2c.write(address,tx,1,true);  
+    _i2c.read(address+1,rawdata,6,0); 
+    gyro.rawx = (rawdata[1] << 8 | rawdata[0]);
+    gyro.rawy = (rawdata[3] << 8 | rawdata[2]);
+    gyro.rawz = (rawdata[5] << 8 | rawdata[4]);
+    gyro.x = float(gyro.rawx)*rate_scale;
+    gyro.y = float(gyro.rawy)*rate_scale*(-1.0);
+    gyro.z = float(gyro.rawz)*rate_scale*(-1.0);
 }
 
-uint8_t BNO055::reset(void)
-{
-     _res = 0;
-     wait_ms(1);   // Reset 1mS
-     _res = 1;
-     wait(0.7);  // Need to wait at least 650mS
-#if defined(TARGET_STM32L152RE)
-    _i2c.frequency(400000);
-#else
-    _i2c.frequency(400000);
-#endif
-    _i2c.stop();
-    page_flag = 0xff;
-    select_page(0);
-    check_id();
-    if (chip_id != I_AM_BNO055_CHIP){
-        return 1;
-    } else {
-        initialize();
-        return 0;
-    }
-}
-
-////// Set initialize data to related registers ///////////
-void BNO055::set_initial_dt_to_regs(void)
-{
-    // select_page(0);
-    // current setting is only used default values
+void BNO055::get_mag(void){
+    tx[0] = BNO055_MAG_DATA_X_LSB_ADDR;
+    _i2c.write(address,tx,1,true);  
+    _i2c.read(address+1,rawdata,6,0); 
+    mag.rawx = (rawdata[1] << 8 | rawdata[0]);
+    mag.rawy = (rawdata[3] << 8 | rawdata[2]);
+    mag.rawz = (rawdata[5] << 8 | rawdata[4]);
+    mag.x = float(mag.rawx);
+    mag.y = float(mag.rawy);
+    mag.z = float(mag.rawz);
 }
 
-/////////////// Check Who am I? ///////////////////////////
-void BNO055::check_id(void)
-{
-    select_page(0);
-    // ID
-    dt[0] = BNO055_CHIP_ID;
-    _i2c.write(chip_addr, dt, 1, true);
-    _i2c.read(chip_addr, dt, 7, false);
-    chip_id = dt[0];
-    if (chip_id == I_AM_BNO055_CHIP) {
-        ready_flag = 1;
-    } else {
-        ready_flag = 0;
-    }
-    acc_id = dt[1];
-    if (acc_id == I_AM_BNO055_ACC) {
-        ready_flag |= 2;
-    }
-    mag_id = dt[2];
-    if (mag_id == I_AM_BNO055_MAG) {
-        ready_flag |= 4;
-    }
-    gyr_id = dt[3];
-    if (mag_id == I_AM_BNO055_MAG) {
-        ready_flag |= 8;
-    }
-    bootldr_rev_id = dt[5]<< 8 | dt[4];
-    sw_rev_id = dt[6];
+void BNO055::get_lia(void){
+    tx[0] = BNO055_LINEAR_ACCEL_DATA_X_LSB_ADDR;
+    _i2c.write(address,tx,1,true);  
+    _i2c.read(address+1,rawdata,6,0); 
+    lia.rawx = (rawdata[1] << 8 | rawdata[0]);
+    lia.rawy = (rawdata[3] << 8 | rawdata[2]);
+    lia.rawz = (rawdata[5] << 8 | rawdata[4]);
+    lia.x = float(lia.rawx)*accel_scale;
+    // Multiply by -1 to rotate from z up to z down
+    lia.y = float(lia.rawy)*accel_scale*(-1.0);
+    lia.z = float(lia.rawz)*accel_scale*(-1.0);
 }
 
-void BNO055::read_id_inf(BNO055_ID_INF_TypeDef *id)
-{
-    id->chip_id = chip_id;
-    id->acc_id = acc_id;
-    id->mag_id = mag_id;
-    id->gyr_id = gyr_id;
-    id->bootldr_rev_id = bootldr_rev_id;
-    id->sw_rev_id = sw_rev_id;
+void BNO055::get_grv(void){
+    tx[0] = BNO055_GRAVITY_DATA_X_LSB_ADDR;
+    _i2c.write(address,tx,1,true);  
+    _i2c.read(address+1,rawdata,6,0); 
+    gravity.rawx = (rawdata[1] << 8 | rawdata[0]);
+    gravity.rawy = (rawdata[3] << 8 | rawdata[2]);
+    gravity.rawz = (rawdata[5] << 8 | rawdata[4]);
+    gravity.x = float(gravity.rawx)*accel_scale;
+    gravity.y = float(gravity.rawy)*accel_scale;
+    gravity.z = float(gravity.rawz)*accel_scale;
 }
 
-/////////////// Check chip ready or not  //////////////////
-uint8_t BNO055::chip_ready(void)
-{
-    if (ready_flag == 0x0f) {
-        return 1;
-    }
-    return 0;
-}
-
-/////////////// Read Calibration status  //////////////////
-uint8_t BNO055::read_calib_status(void)
-{
-    select_page(0);
-    dt[0] = BNO055_CALIB_STAT;
-    _i2c.write(chip_addr, dt, 1, true);
-    _i2c.read(chip_addr, dt, 1, false);
-    return dt[0];
+void BNO055::get_quat(void){
+    tx[0] = BNO055_QUATERNION_DATA_W_LSB_ADDR;
+    _i2c.write(address,tx,1,true);  
+    _i2c.read(address+1,rawdata,8,0); 
+    quat.raww = (rawdata[1] << 8 | rawdata[0]);
+    quat.rawx = (rawdata[3] << 8 | rawdata[2]);
+    quat.rawy = (rawdata[5] << 8 | rawdata[4]);
+    quat.rawz = (rawdata[7] << 8 | rawdata[6]);
+    quat.w = float(quat.raww)/16384.0f;
+    quat.x = float(quat.rawx)/16384.0f;
+    quat.y = float(quat.rawy)/16384.0f;
+    quat.z = float(quat.rawz)/16384.0f;
 }
 
-/////////////// Change Fusion mode  ///////////////////////
-void BNO055::change_fusion_mode(uint8_t mode)
-{
-    uint8_t current_mode;
-
-    select_page(0);
-    current_mode = check_operating_mode();
-    switch (mode) {
-        case CONFIGMODE:
-            dt[0] = BNO055_OPR_MODE;
-            dt[1] = mode;
-            _i2c.write(chip_addr, dt, 2, false);
-            wait_ms(19);    // wait 19mS
-            break;
-        case MODE_IMU:
-        case MODE_COMPASS:
-        case MODE_M4G:
-        case MODE_NDOF_FMC_OFF:
-        case MODE_NDOF:
-            if (current_mode != CONFIGMODE) {   // Can we change the mode directry?
-                dt[0] = BNO055_OPR_MODE;
-                dt[1] = CONFIGMODE;
-                _i2c.write(chip_addr, dt, 2, false);
-                wait_ms(19);    // wait 19mS
-            }
-            dt[0] = BNO055_OPR_MODE;
-            dt[1] = mode;
-            _i2c.write(chip_addr, dt, 2, false);
-            wait_ms(7);    // wait 7mS
-            break;
-        default:
-            break;
-    }
+void BNO055::get_angles(void){
+    tx[0] = BNO055_EULER_H_LSB_ADDR;
+    _i2c.write(address,tx,1,true);  
+    _i2c.read(address+1,rawdata,6,0); 
+    euler.rawyaw = (rawdata[1] << 8 | rawdata[0]);
+    euler.rawroll = (rawdata[3] << 8 | rawdata[2]);
+    euler.rawpitch = (rawdata[5] << 8 | rawdata[4]);
+    euler.yaw = float(euler.rawyaw)*angle_scale + 1.57;
+    // Flipped roll and pitch to offset screwy work by others
+    euler.roll = float(euler.rawpitch)*angle_scale*(-1.0);  
+    euler.pitch = float(euler.rawroll)*angle_scale;
 }
 
-uint8_t BNO055::check_operating_mode(void)
-{
-    select_page(0);
-    dt[0] = BNO055_OPR_MODE;
-    _i2c.write(chip_addr, dt, 1, true);
-    _i2c.read(chip_addr, dt, 1, false);
-    return dt[0];
+
+void BNO055::get_temp(void){
+    readchar(BNO055_TEMP_ADDR);
+    temperature = rx / temp_scale;
+}
+
+void BNO055::get_calib(void){
+     readchar(BNO055_CALIB_STAT_ADDR);
+     calib = rx;
+}
+
+void BNO055::read_calibration_data(void){
+    char tempmode = op_mode;
+    setmode(OPERATION_MODE_CONFIG);
+    wait_ms(20);
+    tx[0] = ACCEL_OFFSET_X_LSB_ADDR;
+    _i2c.write(address,tx,1,true);  
+    _i2c.read(address,calibration,22,false); 
+    setmode(tempmode);
+    wait_ms(10);
 }
 
-/////////////// Set Mouting position  /////////////////////
-void BNO055::set_mounting_position(uint8_t position)
-{
-    uint8_t remap_config;
-    uint8_t remap_sign;
-    uint8_t current_mode;
-
-    current_mode = check_operating_mode();
-    change_fusion_mode(CONFIGMODE);
-    switch (position) {
-        case MT_P0:
-            remap_config = 0x21;
-            remap_sign = 0x04;
-            break;
-        case MT_P2:
-            remap_config = 0x24;
-            remap_sign = 0x06;
-            break;
-        case MT_P3:
-            remap_config = 0x21;
-            remap_sign = 0x02;
-            break;
-        case MT_P4:
-            remap_config = 0x24;
-            remap_sign = 0x03;
-            break;
-        case MT_P5:
-            remap_config = 0x21;
-            remap_sign = 0x01;
-            break;
-        case MT_P6:
-            remap_config = 0x21;
-            remap_sign = 0x07;
-            break;
-        case MT_P7:
-            remap_config = 0x24;
-            remap_sign = 0x05;
-            break;
-        case MT_P1:
-        default:
-            remap_config = 0x24;
-            remap_sign = 0x00;
-            break;
-    }
-    dt[0] = BNO055_AXIS_MAP_CONFIG;
-    dt[1] = remap_config;
-    dt[2] = remap_sign;
-    _i2c.write(chip_addr, dt, 3, false);
-    change_fusion_mode(current_mode);
+void BNO055::write_calibration_data(void){
+    char tempmode = op_mode;
+    setmode(OPERATION_MODE_CONFIG);
+    wait_ms(20);
+    tx[0] = ACCEL_OFFSET_X_LSB_ADDR;
+    _i2c.write(address,tx,1,true);  
+    _i2c.write(address,calibration,22,false); 
+    setmode(tempmode);
+    wait_ms(10);
 }
 
-/////////////// I2C Freq. /////////////////////////////////
-void BNO055::frequency(int hz)
-{
-    _i2c.frequency(hz);
-}
-
-/////////////// Read/Write specific register //////////////
-uint8_t BNO055::read_reg0(uint8_t addr)
-{
-    select_page(0);
-    dt[0] = addr;
-    _i2c.write(chip_addr, dt, 1, true);
-    _i2c.read(chip_addr, dt, 1, false);
-    return (uint8_t)dt[0];
-}
-
-uint8_t BNO055::write_reg0(uint8_t addr, uint8_t data)
-{
-    uint8_t current_mode;
-    uint8_t d;
-
-    current_mode = check_operating_mode();
-    change_fusion_mode(CONFIGMODE);
-    dt[0] = addr;
-    dt[1] = data;
-    _i2c.write(chip_addr, dt, 2, false);
-    d = dt[0];
-    change_fusion_mode(current_mode);
-    return d;
-}
-
-uint8_t BNO055::read_reg1(uint8_t addr)
-{
-    select_page(1);
-    dt[0] = addr;
-    _i2c.write(chip_addr, dt, 1, true);
-    _i2c.read(chip_addr, dt, 1, false);
-    return (uint8_t)dt[0];
-}
-
-uint8_t BNO055::write_reg1(uint8_t addr, uint8_t data)
-{
-    uint8_t current_mode;
-    uint8_t d;
-
-    current_mode = check_operating_mode();
-    change_fusion_mode(CONFIGMODE);
-    select_page(1);
-    dt[0] = addr;
-    dt[1] = data;
-    _i2c.write(chip_addr, dt, 2, false);
-    d = dt[0];
-    change_fusion_mode(current_mode);
-    return d;
-}
+void BNO055::set_mapping(char orient){
+    switch (orient){
+        case 0: 
+            writechar(BNO055_AXIS_MAP_CONFIG_ADDR,0x21);
+            writechar(BNO055_AXIS_MAP_SIGN_ADDR,0x04);
+            break;
+        case 1:
+            writechar(BNO055_AXIS_MAP_CONFIG_ADDR,0x24);
+            writechar(BNO055_AXIS_MAP_SIGN_ADDR,0x00);
+            break;
+        case 2:
+            writechar(BNO055_AXIS_MAP_CONFIG_ADDR,0x24);
+            writechar(BNO055_AXIS_MAP_SIGN_ADDR,0x00);
+            break;
+        case 3:
+            writechar(BNO055_AXIS_MAP_CONFIG_ADDR,0x21);
+            writechar(BNO055_AXIS_MAP_SIGN_ADDR,0x02);
+            break;
+        case 4:
+            writechar(BNO055_AXIS_MAP_CONFIG_ADDR,0x24);
+            writechar(BNO055_AXIS_MAP_SIGN_ADDR,0x03);
+            break;
+        case 5:
+            writechar(BNO055_AXIS_MAP_CONFIG_ADDR,0x21);
+            writechar(BNO055_AXIS_MAP_SIGN_ADDR,0x01);
+            break;
+        case 6:
+            writechar(BNO055_AXIS_MAP_CONFIG_ADDR,0x21);
+            writechar(BNO055_AXIS_MAP_SIGN_ADDR,0x07);
+            break;
+        case 7:
+            writechar(BNO055_AXIS_MAP_CONFIG_ADDR,0x24);
+            writechar(BNO055_AXIS_MAP_SIGN_ADDR,0x05);
+            break;
+        default:
+            writechar(BNO055_AXIS_MAP_CONFIG_ADDR,0x24);
+            writechar(BNO055_AXIS_MAP_SIGN_ADDR,0x00);
+        }
+}
\ No newline at end of file