BNO library commit with fork
Dependents: ES456_Labs MadPulseCntrl MadPulseIMU MadPulse_Controller_ros
Fork of BNO055_fusion by
Diff: BNO055.cpp
- Revision:
- 5:a44318a8bad4
- Parent:
- 4:9e6fead1e93e
- Child:
- 6:ae4be69ac7bb
diff -r 9e6fead1e93e -r a44318a8bad4 BNO055.cpp --- 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