Dave Turner / BNO055

I2C library for Bosch BNO055 sensor

Dependents:   BNO055_HelloWorld robfish_test_IMU_and_hallsensor SCRIBE_stepper SCRIBE_servo ... more

Based off the Bosch Sensortec driver stored at GitHub. I've broken out the main control functions out as separate functions, save for the interrupt setting up, Configuration for the separate sensors not yet enabled.

BNO055.cpp@0:24f23c36dd24, 2015-05-28 (annotated)

Committer:
StressedDave
Date:
Thu May 28 19:22:25 2015 +0000
Revision:
0:24f23c36dd24
Child:
1:2c3322a8d417
Initial upload

Who changed what in which revision?

UserRevisionLine numberNew contents of line
StressedDave 0:24f23c36dd24 1 #include "BNO055.h"
StressedDave 0:24f23c36dd24 2 #include "mbed.h"
StressedDave 0:24f23c36dd24 3
StressedDave 0:24f23c36dd24 4 BNO055::BNO055(PinName SDA, PinName SCL) : _i2c(SDA,SCL){
StressedDave 0:24f23c36dd24 5 //Set I2C fast and bring reset line high
StressedDave 0:24f23c36dd24 6 _i2c.frequency(400000);
StressedDave 0:24f23c36dd24 7 address = BNOAddress;
StressedDave 0:24f23c36dd24 8 accel_scale = 1.0f;
StressedDave 0:24f23c36dd24 9 rate_scale = 1.0f/16.0f;
StressedDave 0:24f23c36dd24 10 angle_scale = 1.0f/16.0f;
StressedDave 0:24f23c36dd24 11 temp_scale = 1.0f;
StressedDave 0:24f23c36dd24 12 }
StressedDave 0:24f23c36dd24 13
StressedDave 0:24f23c36dd24 14 void BNO055::reset(){
StressedDave 0:24f23c36dd24 15 //Perform a power-on-reset
StressedDave 0:24f23c36dd24 16 readchar(BNO055_SYS_TRIGGER_ADDR);
StressedDave 0:24f23c36dd24 17 rx = rx | 0x20;
StressedDave 0:24f23c36dd24 18 writechar(BNO055_SYS_TRIGGER_ADDR,rx);
StressedDave 0:24f23c36dd24 19 //Wait for the system to come back up again (datasheet says 650ms)
StressedDave 0:24f23c36dd24 20 wait_ms(675);
StressedDave 0:24f23c36dd24 21 }
StressedDave 0:24f23c36dd24 22
StressedDave 0:24f23c36dd24 23 bool BNO055::check(){
StressedDave 0:24f23c36dd24 24 //Check we have communication link with the chip
StressedDave 0:24f23c36dd24 25 readchar(BNO055_CHIP_ID_ADDR);
StressedDave 0:24f23c36dd24 26 if (rx != 0xA0) return false;
StressedDave 0:24f23c36dd24 27 //Grab the chip ID and software versions
StressedDave 0:24f23c36dd24 28 tx[0] = BNO055_CHIP_ID_ADDR;
StressedDave 0:24f23c36dd24 29 _i2c.write(address,tx,1,true);
StressedDave 0:24f23c36dd24 30 _i2c.read(address+1,rawdata,7,false);
StressedDave 0:24f23c36dd24 31 ID.id = rawdata[0];
StressedDave 0:24f23c36dd24 32 ID.accel = rawdata[1];
StressedDave 0:24f23c36dd24 33 ID.mag = rawdata[2];
StressedDave 0:24f23c36dd24 34 ID.gyro = rawdata[3];
StressedDave 0:24f23c36dd24 35 ID.sw[0] = rawdata[4];
StressedDave 0:24f23c36dd24 36 ID.sw[1] = rawdata[5];
StressedDave 0:24f23c36dd24 37 ID.bootload = rawdata[6];
StressedDave 0:24f23c36dd24 38 setpage(1);
StressedDave 0:24f23c36dd24 39 tx[0] = BNO055_UNIQUE_ID_ADDR;
StressedDave 0:24f23c36dd24 40 _i2c.write(address,tx,1,true);
StressedDave 0:24f23c36dd24 41 _i2c.read(address+1,ID.serial,16,false);
StressedDave 0:24f23c36dd24 42 setpage(0);
StressedDave 0:24f23c36dd24 43 return true;
StressedDave 0:24f23c36dd24 44 }
StressedDave 0:24f23c36dd24 45
StressedDave 0:24f23c36dd24 46 void BNO055::SetExternalCrystal(bool yn){
StressedDave 0:24f23c36dd24 47 // Read the current status from the device
StressedDave 0:24f23c36dd24 48 readchar(BNO055_SYS_TRIGGER_ADDR);
StressedDave 0:24f23c36dd24 49 if (yn) rx = rx | 0x80;
StressedDave 0:24f23c36dd24 50 else rx = rx & 0x7F;
StressedDave 0:24f23c36dd24 51 writechar(BNO055_SYS_TRIGGER_ADDR,rx);
StressedDave 0:24f23c36dd24 52 }
StressedDave 0:24f23c36dd24 53
StressedDave 0:24f23c36dd24 54 void BNO055::set_accel_units(char units){
StressedDave 0:24f23c36dd24 55 readchar(BNO055_UNIT_SEL_ADDR);
StressedDave 0:24f23c36dd24 56 if(units == MPERSPERS){
StressedDave 0:24f23c36dd24 57 rx = rx & 0xFE;
StressedDave 0:24f23c36dd24 58 accel_scale = 0.01f;
StressedDave 0:24f23c36dd24 59 }
StressedDave 0:24f23c36dd24 60 else {
StressedDave 0:24f23c36dd24 61 rx = rx | units;
StressedDave 0:24f23c36dd24 62 accel_scale = 1.0f;
StressedDave 0:24f23c36dd24 63 }
StressedDave 0:24f23c36dd24 64 writechar(BNO055_UNIT_SEL_ADDR,rx);
StressedDave 0:24f23c36dd24 65 }
StressedDave 0:24f23c36dd24 66
StressedDave 0:24f23c36dd24 67 void BNO055::set_anglerate_units(char units){
StressedDave 0:24f23c36dd24 68 readchar(BNO055_UNIT_SEL_ADDR);
StressedDave 0:24f23c36dd24 69 if (units == DEG_PER_SEC){
StressedDave 0:24f23c36dd24 70 rx = rx & 0xFD;
StressedDave 0:24f23c36dd24 71 rate_scale = 1.0f/16.0f;
StressedDave 0:24f23c36dd24 72 }
StressedDave 0:24f23c36dd24 73 else {
StressedDave 0:24f23c36dd24 74 rx = rx | units;
StressedDave 0:24f23c36dd24 75 rate_scale = 1.0f/900.0f;
StressedDave 0:24f23c36dd24 76 }
StressedDave 0:24f23c36dd24 77 writechar(BNO055_UNIT_SEL_ADDR,rx);
StressedDave 0:24f23c36dd24 78 }
StressedDave 0:24f23c36dd24 79
StressedDave 0:24f23c36dd24 80 void BNO055::set_angle_units(char units){
StressedDave 0:24f23c36dd24 81 readchar(BNO055_UNIT_SEL_ADDR);
StressedDave 0:24f23c36dd24 82 if (units == DEGREES){
StressedDave 0:24f23c36dd24 83 rx = rx & 0xFB;
StressedDave 0:24f23c36dd24 84 angle_scale = 1.0f/16.0f;
StressedDave 0:24f23c36dd24 85 }
StressedDave 0:24f23c36dd24 86 else {
StressedDave 0:24f23c36dd24 87 rx = rx | units;
StressedDave 0:24f23c36dd24 88 rate_scale = 1.0f/900.0f;
StressedDave 0:24f23c36dd24 89 }
StressedDave 0:24f23c36dd24 90 writechar(BNO055_UNIT_SEL_ADDR,rx);
StressedDave 0:24f23c36dd24 91 }
StressedDave 0:24f23c36dd24 92
StressedDave 0:24f23c36dd24 93 void BNO055::set_temp_units(char units){
StressedDave 0:24f23c36dd24 94 readchar(BNO055_UNIT_SEL_ADDR);
StressedDave 0:24f23c36dd24 95 if (units == CENTIGRADE){
StressedDave 0:24f23c36dd24 96 rx = rx & 0x7F;
StressedDave 0:24f23c36dd24 97 temp_scale = 1.0f;
StressedDave 0:24f23c36dd24 98 }
StressedDave 0:24f23c36dd24 99 else {
StressedDave 0:24f23c36dd24 100 rx = rx | units;
StressedDave 0:24f23c36dd24 101 temp_scale = 2.0f;
StressedDave 0:24f23c36dd24 102 }
StressedDave 0:24f23c36dd24 103 writechar(BNO055_UNIT_SEL_ADDR,rx);
StressedDave 0:24f23c36dd24 104 }
StressedDave 0:24f23c36dd24 105
StressedDave 0:24f23c36dd24 106 void BNO055::setmode(char omode){
StressedDave 0:24f23c36dd24 107 writechar(BNO055_OPR_MODE_ADDR,omode);
StressedDave 0:24f23c36dd24 108 op_mode = omode;
StressedDave 0:24f23c36dd24 109 }
StressedDave 0:24f23c36dd24 110
StressedDave 0:24f23c36dd24 111 void BNO055::setpowermode(char pmode){
StressedDave 0:24f23c36dd24 112 writechar(BNO055_PWR_MODE_ADDR,pmode);
StressedDave 0:24f23c36dd24 113 pwr_mode = pmode;
StressedDave 0:24f23c36dd24 114 }
StressedDave 0:24f23c36dd24 115
StressedDave 0:24f23c36dd24 116 void BNO055::get_accel(void){
StressedDave 0:24f23c36dd24 117 tx[0] = BNO055_ACCEL_DATA_X_LSB_ADDR;
StressedDave 0:24f23c36dd24 118 _i2c.write(address,tx,1,true);
StressedDave 0:24f23c36dd24 119 _i2c.read(address+1,rawdata,6,0);
StressedDave 0:24f23c36dd24 120 accel.rawx = (rawdata[1] << 8 | rawdata[0]);
StressedDave 0:24f23c36dd24 121 accel.rawy = (rawdata[3] << 8 | rawdata[2]);
StressedDave 0:24f23c36dd24 122 accel.rawz = (rawdata[5] << 8 | rawdata[4]);
StressedDave 0:24f23c36dd24 123 accel.x = float(accel.rawx)*accel_scale;
StressedDave 0:24f23c36dd24 124 accel.y = float(accel.rawy)*accel_scale;
StressedDave 0:24f23c36dd24 125 accel.z = float(accel.rawz)*accel_scale;
StressedDave 0:24f23c36dd24 126 }
StressedDave 0:24f23c36dd24 127
StressedDave 0:24f23c36dd24 128 void BNO055::get_gyro(void){
StressedDave 0:24f23c36dd24 129 tx[0] = BNO055_GYRO_DATA_X_LSB_ADDR;
StressedDave 0:24f23c36dd24 130 _i2c.write(address,tx,1,true);
StressedDave 0:24f23c36dd24 131 _i2c.read(address+1,rawdata,6,0);
StressedDave 0:24f23c36dd24 132 gyro.rawx = (rawdata[1] << 8 | rawdata[0]);
StressedDave 0:24f23c36dd24 133 gyro.rawy = (rawdata[3] << 8 | rawdata[2]);
StressedDave 0:24f23c36dd24 134 gyro.rawz = (rawdata[5] << 8 | rawdata[4]);
StressedDave 0:24f23c36dd24 135 gyro.x = float(gyro.rawx)*rate_scale;
StressedDave 0:24f23c36dd24 136 gyro.y = float(gyro.rawy)*rate_scale;
StressedDave 0:24f23c36dd24 137 gyro.z = float(gyro.rawz)*rate_scale;
StressedDave 0:24f23c36dd24 138 }
StressedDave 0:24f23c36dd24 139
StressedDave 0:24f23c36dd24 140 void BNO055::get_mag(void){
StressedDave 0:24f23c36dd24 141 tx[0] = BNO055_MAG_DATA_X_LSB_ADDR;
StressedDave 0:24f23c36dd24 142 _i2c.write(address,tx,1,true);
StressedDave 0:24f23c36dd24 143 _i2c.read(address+1,rawdata,6,0);
StressedDave 0:24f23c36dd24 144 mag.rawx = (rawdata[1] << 8 | rawdata[0]);
StressedDave 0:24f23c36dd24 145 mag.rawy = (rawdata[3] << 8 | rawdata[2]);
StressedDave 0:24f23c36dd24 146 mag.rawz = (rawdata[5] << 8 | rawdata[4]);
StressedDave 0:24f23c36dd24 147 mag.x = float(mag.rawx);
StressedDave 0:24f23c36dd24 148 mag.y = float(mag.rawy);
StressedDave 0:24f23c36dd24 149 mag.z = float(mag.rawz);
StressedDave 0:24f23c36dd24 150 }
StressedDave 0:24f23c36dd24 151
StressedDave 0:24f23c36dd24 152 void BNO055::get_lia(void){
StressedDave 0:24f23c36dd24 153 tx[0] = BNO055_LINEAR_ACCEL_DATA_X_LSB_ADDR;
StressedDave 0:24f23c36dd24 154 _i2c.write(address,tx,1,true);
StressedDave 0:24f23c36dd24 155 _i2c.read(address+1,rawdata,6,0);
StressedDave 0:24f23c36dd24 156 lia.rawx = (rawdata[1] << 8 | rawdata[0]);
StressedDave 0:24f23c36dd24 157 lia.rawy = (rawdata[3] << 8 | rawdata[2]);
StressedDave 0:24f23c36dd24 158 lia.rawz = (rawdata[5] << 8 | rawdata[4]);
StressedDave 0:24f23c36dd24 159 lia.x = float(lia.rawx)*accel_scale;
StressedDave 0:24f23c36dd24 160 lia.y = float(lia.rawy)*accel_scale;
StressedDave 0:24f23c36dd24 161 lia.z = float(lia.rawz)*accel_scale;
StressedDave 0:24f23c36dd24 162 }
StressedDave 0:24f23c36dd24 163
StressedDave 0:24f23c36dd24 164 void BNO055::get_grv(void){
StressedDave 0:24f23c36dd24 165 tx[0] = BNO055_GRAVITY_DATA_X_LSB_ADDR;
StressedDave 0:24f23c36dd24 166 _i2c.write(address,tx,1,true);
StressedDave 0:24f23c36dd24 167 _i2c.read(address+1,rawdata,6,0);
StressedDave 0:24f23c36dd24 168 gravity.rawx = (rawdata[1] << 8 | rawdata[0]);
StressedDave 0:24f23c36dd24 169 gravity.rawy = (rawdata[3] << 8 | rawdata[2]);
StressedDave 0:24f23c36dd24 170 gravity.rawz = (rawdata[5] << 8 | rawdata[4]);
StressedDave 0:24f23c36dd24 171 gravity.x = float(gravity.rawx)*accel_scale;
StressedDave 0:24f23c36dd24 172 gravity.y = float(gravity.rawy)*accel_scale;
StressedDave 0:24f23c36dd24 173 gravity.z = float(gravity.rawz)*accel_scale;
StressedDave 0:24f23c36dd24 174 }
StressedDave 0:24f23c36dd24 175
StressedDave 0:24f23c36dd24 176 void BNO055::get_calib(void){
StressedDave 0:24f23c36dd24 177 readchar(BNO055_CALIB_STAT_ADDR);
StressedDave 0:24f23c36dd24 178 calib = rx;
StressedDave 0:24f23c36dd24 179 }