Interface to the BNO055 sensor
BNO055.cpp@0:b3e019f0ce58, 2019-01-24 (annotated)
- Committer:
- hzelayasolano22
- Date:
- Thu Jan 24 00:24:49 2019 +0000
- Revision:
- 0:b3e019f0ce58
Library for BNO055 complete!
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
hzelayasolano22 | 0:b3e019f0ce58 | 1 | /* Embedded Systems Final Project: 9-Axis Absolute Orientation imu (BNO055) ndof.cpp |
hzelayasolano22 | 0:b3e019f0ce58 | 2 | */ |
hzelayasolano22 | 0:b3e019f0ce58 | 3 | |
hzelayasolano22 | 0:b3e019f0ce58 | 4 | #include "BNO055.h" |
hzelayasolano22 | 0:b3e019f0ce58 | 5 | #include "mbed.h" |
hzelayasolano22 | 0:b3e019f0ce58 | 6 | |
hzelayasolano22 | 0:b3e019f0ce58 | 7 | #define REG_ADDR_OPR_MODE 0x3D //adresses for registers which are needed for this code |
hzelayasolano22 | 0:b3e019f0ce58 | 8 | #define REG_ADDR_UNIT_SEL 0x3B //address of unit selection |
hzelayasolano22 | 0:b3e019f0ce58 | 9 | |
hzelayasolano22 | 0:b3e019f0ce58 | 10 | BNO055::BNO055(I2C i2c, int addr) : _i2c(i2c) { // for user friendly sensor-address setup and view |
hzelayasolano22 | 0:b3e019f0ce58 | 11 | _addr = addr; |
hzelayasolano22 | 0:b3e019f0ce58 | 12 | } |
hzelayasolano22 | 0:b3e019f0ce58 | 13 | |
hzelayasolano22 | 0:b3e019f0ce58 | 14 | char BNO055::checkID(){//will check if compiler is talking to imu |
hzelayasolano22 | 0:b3e019f0ce58 | 15 | char buff[1]; |
hzelayasolano22 | 0:b3e019f0ce58 | 16 | buff[0] = 0x00; |
hzelayasolano22 | 0:b3e019f0ce58 | 17 | _i2c.write(_addr, buff, 1); |
hzelayasolano22 | 0:b3e019f0ce58 | 18 | _i2c.read(_addr, buff, 1); |
hzelayasolano22 | 0:b3e019f0ce58 | 19 | return buff[0]; |
hzelayasolano22 | 0:b3e019f0ce58 | 20 | } |
hzelayasolano22 | 0:b3e019f0ce58 | 21 | |
hzelayasolano22 | 0:b3e019f0ce58 | 22 | void BNO055::setMode(int modeCode){//sets up the registers |
hzelayasolano22 | 0:b3e019f0ce58 | 23 | char buff[2]; |
hzelayasolano22 | 0:b3e019f0ce58 | 24 | char code; |
hzelayasolano22 | 0:b3e019f0ce58 | 25 | |
hzelayasolano22 | 0:b3e019f0ce58 | 26 | buff[0] = REG_ADDR_OPR_MODE; |
hzelayasolano22 | 0:b3e019f0ce58 | 27 | _i2c.write(_addr, buff, 1); |
hzelayasolano22 | 0:b3e019f0ce58 | 28 | _i2c.read(_addr, buff, 1); |
hzelayasolano22 | 0:b3e019f0ce58 | 29 | code = buff[0]; |
hzelayasolano22 | 0:b3e019f0ce58 | 30 | |
hzelayasolano22 | 0:b3e019f0ce58 | 31 | if(modeCode == BNO055_MODE_NDOF){ |
hzelayasolano22 | 0:b3e019f0ce58 | 32 | code = code & 0xF0; |
hzelayasolano22 | 0:b3e019f0ce58 | 33 | code = code | 0x0C; |
hzelayasolano22 | 0:b3e019f0ce58 | 34 | } |
hzelayasolano22 | 0:b3e019f0ce58 | 35 | buff[0] = REG_ADDR_OPR_MODE; |
hzelayasolano22 | 0:b3e019f0ce58 | 36 | buff[1] = code; |
hzelayasolano22 | 0:b3e019f0ce58 | 37 | _i2c.write(_addr, buff, 2); |
hzelayasolano22 | 0:b3e019f0ce58 | 38 | wait_ms(7); |
hzelayasolano22 | 0:b3e019f0ce58 | 39 | |
hzelayasolano22 | 0:b3e019f0ce58 | 40 | } |
hzelayasolano22 | 0:b3e019f0ce58 | 41 | |
hzelayasolano22 | 0:b3e019f0ce58 | 42 | int BNO055::setAngleUnits(int unitsCode){//will set whether it displays information in degrees or radians |
hzelayasolano22 | 0:b3e019f0ce58 | 43 | _units = unitsCode; |
hzelayasolano22 | 0:b3e019f0ce58 | 44 | char buff[3]; |
hzelayasolano22 | 0:b3e019f0ce58 | 45 | char code; |
hzelayasolano22 | 0:b3e019f0ce58 | 46 | buff[0] = REG_ADDR_UNIT_SEL; |
hzelayasolano22 | 0:b3e019f0ce58 | 47 | _i2c.write(_addr, buff, 1); |
hzelayasolano22 | 0:b3e019f0ce58 | 48 | _i2c.read(_addr, buff, 1); |
hzelayasolano22 | 0:b3e019f0ce58 | 49 | code = buff[0]; |
hzelayasolano22 | 0:b3e019f0ce58 | 50 | |
hzelayasolano22 | 0:b3e019f0ce58 | 51 | if(unitsCode == BNO055_ANGLE_UNITS_DEGREE){//depending on which unit you want to use the right registers and bits will be selected |
hzelayasolano22 | 0:b3e019f0ce58 | 52 | code = code & (0xFF - 0x02 - 0x04); |
hzelayasolano22 | 0:b3e019f0ce58 | 53 | } else if(unitsCode == BNO055_ANGLE_UNITS_RADIAN){ |
hzelayasolano22 | 0:b3e019f0ce58 | 54 | code = code & (0xFF - 0x02 - 0x04); |
hzelayasolano22 | 0:b3e019f0ce58 | 55 | code = code | 0x04; |
hzelayasolano22 | 0:b3e019f0ce58 | 56 | code = code | 0x02; |
hzelayasolano22 | 0:b3e019f0ce58 | 57 | } |
hzelayasolano22 | 0:b3e019f0ce58 | 58 | buff[0] = REG_ADDR_UNIT_SEL; |
hzelayasolano22 | 0:b3e019f0ce58 | 59 | buff[1] = code; |
hzelayasolano22 | 0:b3e019f0ce58 | 60 | _i2c.write(_addr, buff, 2); |
hzelayasolano22 | 0:b3e019f0ce58 | 61 | return unitsCode;//returns int for main to be able to manipulate |
hzelayasolano22 | 0:b3e019f0ce58 | 62 | } |
hzelayasolano22 | 0:b3e019f0ce58 | 63 | |
hzelayasolano22 | 0:b3e019f0ce58 | 64 | |
hzelayasolano22 | 0:b3e019f0ce58 | 65 | |
hzelayasolano22 | 0:b3e019f0ce58 | 66 | float BNO055::readHeading(){//calls register address for heading of imu |
hzelayasolano22 | 0:b3e019f0ce58 | 67 | char buff[2]; |
hzelayasolano22 | 0:b3e019f0ce58 | 68 | int16_t rawHeading; |
hzelayasolano22 | 0:b3e019f0ce58 | 69 | buff[0] = 0x1A; |
hzelayasolano22 | 0:b3e019f0ce58 | 70 | _i2c.write(_addr, buff, 1, true); |
hzelayasolano22 | 0:b3e019f0ce58 | 71 | _i2c.read(_addr, buff, 2); |
hzelayasolano22 | 0:b3e019f0ce58 | 72 | rawHeading = (buff[1]<<8) | buff[0]; |
hzelayasolano22 | 0:b3e019f0ce58 | 73 | float euler; |
hzelayasolano22 | 0:b3e019f0ce58 | 74 | switch (_units) {//uses class int to determine which case to return - depended on what programmer enters in main |
hzelayasolano22 | 0:b3e019f0ce58 | 75 | case BNO055_ANGLE_UNITS_DEGREE: euler = rawHeading/16.0; break; //1 deg = 16LSB from data sheet |
hzelayasolano22 | 0:b3e019f0ce58 | 76 | case BNO055_ANGLE_UNITS_RADIAN: euler = rawHeading/900.0; break; //1 rad = 900LSB |
hzelayasolano22 | 0:b3e019f0ce58 | 77 | } |
hzelayasolano22 | 0:b3e019f0ce58 | 78 | return euler; |
hzelayasolano22 | 0:b3e019f0ce58 | 79 | }//rest of the function follow this same structure |
hzelayasolano22 | 0:b3e019f0ce58 | 80 | |
hzelayasolano22 | 0:b3e019f0ce58 | 81 | float BNO055::readRoll(){//calls register for roll of imu |
hzelayasolano22 | 0:b3e019f0ce58 | 82 | char buff[2]; |
hzelayasolano22 | 0:b3e019f0ce58 | 83 | int16_t rawRoll; |
hzelayasolano22 | 0:b3e019f0ce58 | 84 | buff[0] = 0x1C; |
hzelayasolano22 | 0:b3e019f0ce58 | 85 | _i2c.write(_addr, buff, 1, true); |
hzelayasolano22 | 0:b3e019f0ce58 | 86 | _i2c.read(_addr, buff , 2); |
hzelayasolano22 | 0:b3e019f0ce58 | 87 | rawRoll = (buff[1]<< 8) | buff[0]; |
hzelayasolano22 | 0:b3e019f0ce58 | 88 | float euler; |
hzelayasolano22 | 0:b3e019f0ce58 | 89 | switch(_units){ |
hzelayasolano22 | 0:b3e019f0ce58 | 90 | case BNO055_ANGLE_UNITS_DEGREE: euler = rawRoll/16.0; break; |
hzelayasolano22 | 0:b3e019f0ce58 | 91 | case BNO055_ANGLE_UNITS_RADIAN: euler = rawRoll/900.0; break; |
hzelayasolano22 | 0:b3e019f0ce58 | 92 | } |
hzelayasolano22 | 0:b3e019f0ce58 | 93 | return euler; |
hzelayasolano22 | 0:b3e019f0ce58 | 94 | } |
hzelayasolano22 | 0:b3e019f0ce58 | 95 | |
hzelayasolano22 | 0:b3e019f0ce58 | 96 | float BNO055::readPitch(){ |
hzelayasolano22 | 0:b3e019f0ce58 | 97 | char buff[2]; |
hzelayasolano22 | 0:b3e019f0ce58 | 98 | int16_t rawPitch; |
hzelayasolano22 | 0:b3e019f0ce58 | 99 | buff[0] = 0x1E; |
hzelayasolano22 | 0:b3e019f0ce58 | 100 | _i2c.write(_addr, buff, 1, true); |
hzelayasolano22 | 0:b3e019f0ce58 | 101 | _i2c.read(_addr, buff, 2); |
hzelayasolano22 | 0:b3e019f0ce58 | 102 | rawPitch = (buff[1]<< 8) | buff[0]; |
hzelayasolano22 | 0:b3e019f0ce58 | 103 | float euler; |
hzelayasolano22 | 0:b3e019f0ce58 | 104 | switch(_units){ |
hzelayasolano22 | 0:b3e019f0ce58 | 105 | case BNO055_ANGLE_UNITS_DEGREE: euler = -rawPitch/16.0; break; |
hzelayasolano22 | 0:b3e019f0ce58 | 106 | case BNO055_ANGLE_UNITS_RADIAN: euler = -rawPitch/900.0; break; |
hzelayasolano22 | 0:b3e019f0ce58 | 107 | } |
hzelayasolano22 | 0:b3e019f0ce58 | 108 | return euler; |
hzelayasolano22 | 0:b3e019f0ce58 | 109 | } |