BNO055.cpp

00001 /* Embedded Systems Final Project: 9-Axis Absolute Orientation imu (BNO055) ndof.cpp 
00002 */
00003 
00004 #include "BNO055.h"
00005 #include "mbed.h"
00006 
00007 #define REG_ADDR_OPR_MODE   0x3D //adresses for registers which are needed for this code
00008 #define REG_ADDR_UNIT_SEL   0x3B //address of unit selection
00009 
00010 BNO055::BNO055(I2C i2c, int addr) : _i2c(i2c) { // for user friendly sensor-address setup and view 
00011     _addr = addr;
00012 }
00013 
00014 char BNO055::checkID(){//will check if compiler is talking to imu
00015     char buff[1];
00016     buff[0] = 0x00;
00017     _i2c.write(_addr, buff, 1);
00018     _i2c.read(_addr, buff, 1);
00019     return buff[0];
00020 }
00021 
00022 void BNO055::setMode(int modeCode){//sets up the registers
00023     char buff[2];
00024     char code;
00025 
00026     buff[0] = REG_ADDR_OPR_MODE;
00027     _i2c.write(_addr, buff, 1);
00028     _i2c.read(_addr, buff, 1);
00029     code = buff[0];
00030 
00031     if(modeCode == BNO055_MODE_NDOF){
00032         code = code & 0xF0;
00033         code = code | 0x0C;
00034     }
00035     buff[0] = REG_ADDR_OPR_MODE;
00036     buff[1] = code;
00037     _i2c.write(_addr, buff, 2);
00038     wait_ms(7);
00039 
00040 }
00041 
00042 int BNO055::setAngleUnits(int unitsCode){//will set whether it displays information in degrees or radians
00043     _units = unitsCode;
00044     char buff[3];
00045     char code;
00046     buff[0] = REG_ADDR_UNIT_SEL;
00047     _i2c.write(_addr, buff, 1);
00048     _i2c.read(_addr, buff, 1);
00049     code = buff[0];
00050 
00051     if(unitsCode == BNO055_ANGLE_UNITS_DEGREE){//depending on which unit you want to use the right registers and bits will be selected
00052         code = code & (0xFF - 0x02 - 0x04);
00053     } else if(unitsCode == BNO055_ANGLE_UNITS_RADIAN){
00054         code = code & (0xFF - 0x02 - 0x04);
00055         code = code | 0x04;
00056         code = code | 0x02;
00057     }
00058     buff[0] = REG_ADDR_UNIT_SEL;
00059     buff[1] = code;
00060     _i2c.write(_addr, buff, 2);
00061     return unitsCode;//returns int for main to be able to manipulate
00062 }
00063 
00064 
00065 
00066 float BNO055::readHeading(){//calls register address for heading of imu
00067     char buff[2];
00068     int16_t rawHeading;
00069     buff[0] = 0x1A;
00070     _i2c.write(_addr, buff, 1, true);
00071     _i2c.read(_addr, buff, 2);
00072     rawHeading = (buff[1]<<8) | buff[0];
00073     float euler;
00074     switch (_units) {//uses class int to determine which case to return - depended on what programmer enters in main
00075         case BNO055_ANGLE_UNITS_DEGREE: euler = rawHeading/16.0; break;   //1 deg = 16LSB  from data sheet
00076         case BNO055_ANGLE_UNITS_RADIAN: euler = rawHeading/900.0; break;  //1 rad = 900LSB  
00077     }
00078     return euler;
00079     }//rest of the function follow this same structure
00080 
00081 float BNO055::readRoll(){//calls register for roll of imu
00082     char buff[2];
00083     int16_t rawRoll;
00084     buff[0] = 0x1C;
00085     _i2c.write(_addr, buff, 1, true);
00086     _i2c.read(_addr, buff , 2);
00087     rawRoll = (buff[1]<< 8) | buff[0];
00088     float euler;
00089     switch(_units){
00090         case BNO055_ANGLE_UNITS_DEGREE: euler = rawRoll/16.0; break;     
00091         case BNO055_ANGLE_UNITS_RADIAN: euler = rawRoll/900.0; break;   
00092     }
00093     return euler;
00094 }
00095 
00096 float BNO055::readPitch(){
00097     char buff[2];
00098     int16_t rawPitch;
00099     buff[0] = 0x1E;
00100     _i2c.write(_addr, buff, 1, true);
00101     _i2c.read(_addr, buff, 2);
00102     rawPitch = (buff[1]<< 8) | buff[0];
00103     float euler;
00104     switch(_units){
00105         case BNO055_ANGLE_UNITS_DEGREE: euler = -rawPitch/16.0; break;     
00106         case BNO055_ANGLE_UNITS_RADIAN: euler = -rawPitch/900.0; break;  
00107     }
00108     return euler;
00109 }