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@1:2c3322a8d417, 2015-05-30 (annotated)

Committer:
StressedDave
Date:
Sat May 30 18:36:36 2015 +0000
Revision:
1:2c3322a8d417
Parent:
0:24f23c36dd24
Child:
2:695c6e5d239a
Added device orientation changes as per p25 of datasheet

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 1:2c3322a8d417 11 temp_scale = 1;
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 1:2c3322a8d417 96 rx = rx & 0xEF;
StressedDave 1:2c3322a8d417 97 temp_scale = 1;
StressedDave 0:24f23c36dd24 98 }
StressedDave 0:24f23c36dd24 99 else {
StressedDave 0:24f23c36dd24 100 rx = rx | units;
StressedDave 1:2c3322a8d417 101 temp_scale = 2;
StressedDave 0:24f23c36dd24 102 }
StressedDave 0:24f23c36dd24 103 writechar(BNO055_UNIT_SEL_ADDR,rx);
StressedDave 0:24f23c36dd24 104 }
StressedDave 0:24f23c36dd24 105
StressedDave 1:2c3322a8d417 106 void BNO055::set_orientation(char units){
StressedDave 1:2c3322a8d417 107 readchar(BNO055_UNIT_SEL_ADDR);
StressedDave 1:2c3322a8d417 108 if (units == WINDOWS) rx = rx &0x7F;
StressedDave 1:2c3322a8d417 109 else rx = rx | units;
StressedDave 1:2c3322a8d417 110 writechar(BNO055_UNIT_SEL_ADDR,rx);
StressedDave 1:2c3322a8d417 111 }
StressedDave 1:2c3322a8d417 112
StressedDave 0:24f23c36dd24 113 void BNO055::setmode(char omode){
StressedDave 0:24f23c36dd24 114 writechar(BNO055_OPR_MODE_ADDR,omode);
StressedDave 0:24f23c36dd24 115 op_mode = omode;
StressedDave 0:24f23c36dd24 116 }
StressedDave 0:24f23c36dd24 117
StressedDave 0:24f23c36dd24 118 void BNO055::setpowermode(char pmode){
StressedDave 0:24f23c36dd24 119 writechar(BNO055_PWR_MODE_ADDR,pmode);
StressedDave 0:24f23c36dd24 120 pwr_mode = pmode;
StressedDave 0:24f23c36dd24 121 }
StressedDave 0:24f23c36dd24 122
StressedDave 0:24f23c36dd24 123 void BNO055::get_accel(void){
StressedDave 0:24f23c36dd24 124 tx[0] = BNO055_ACCEL_DATA_X_LSB_ADDR;
StressedDave 0:24f23c36dd24 125 _i2c.write(address,tx,1,true);
StressedDave 0:24f23c36dd24 126 _i2c.read(address+1,rawdata,6,0);
StressedDave 0:24f23c36dd24 127 accel.rawx = (rawdata[1] << 8 | rawdata[0]);
StressedDave 0:24f23c36dd24 128 accel.rawy = (rawdata[3] << 8 | rawdata[2]);
StressedDave 0:24f23c36dd24 129 accel.rawz = (rawdata[5] << 8 | rawdata[4]);
StressedDave 0:24f23c36dd24 130 accel.x = float(accel.rawx)*accel_scale;
StressedDave 0:24f23c36dd24 131 accel.y = float(accel.rawy)*accel_scale;
StressedDave 0:24f23c36dd24 132 accel.z = float(accel.rawz)*accel_scale;
StressedDave 0:24f23c36dd24 133 }
StressedDave 0:24f23c36dd24 134
StressedDave 0:24f23c36dd24 135 void BNO055::get_gyro(void){
StressedDave 0:24f23c36dd24 136 tx[0] = BNO055_GYRO_DATA_X_LSB_ADDR;
StressedDave 0:24f23c36dd24 137 _i2c.write(address,tx,1,true);
StressedDave 0:24f23c36dd24 138 _i2c.read(address+1,rawdata,6,0);
StressedDave 0:24f23c36dd24 139 gyro.rawx = (rawdata[1] << 8 | rawdata[0]);
StressedDave 0:24f23c36dd24 140 gyro.rawy = (rawdata[3] << 8 | rawdata[2]);
StressedDave 0:24f23c36dd24 141 gyro.rawz = (rawdata[5] << 8 | rawdata[4]);
StressedDave 0:24f23c36dd24 142 gyro.x = float(gyro.rawx)*rate_scale;
StressedDave 0:24f23c36dd24 143 gyro.y = float(gyro.rawy)*rate_scale;
StressedDave 0:24f23c36dd24 144 gyro.z = float(gyro.rawz)*rate_scale;
StressedDave 0:24f23c36dd24 145 }
StressedDave 0:24f23c36dd24 146
StressedDave 0:24f23c36dd24 147 void BNO055::get_mag(void){
StressedDave 0:24f23c36dd24 148 tx[0] = BNO055_MAG_DATA_X_LSB_ADDR;
StressedDave 0:24f23c36dd24 149 _i2c.write(address,tx,1,true);
StressedDave 0:24f23c36dd24 150 _i2c.read(address+1,rawdata,6,0);
StressedDave 0:24f23c36dd24 151 mag.rawx = (rawdata[1] << 8 | rawdata[0]);
StressedDave 0:24f23c36dd24 152 mag.rawy = (rawdata[3] << 8 | rawdata[2]);
StressedDave 0:24f23c36dd24 153 mag.rawz = (rawdata[5] << 8 | rawdata[4]);
StressedDave 0:24f23c36dd24 154 mag.x = float(mag.rawx);
StressedDave 0:24f23c36dd24 155 mag.y = float(mag.rawy);
StressedDave 0:24f23c36dd24 156 mag.z = float(mag.rawz);
StressedDave 0:24f23c36dd24 157 }
StressedDave 0:24f23c36dd24 158
StressedDave 0:24f23c36dd24 159 void BNO055::get_lia(void){
StressedDave 0:24f23c36dd24 160 tx[0] = BNO055_LINEAR_ACCEL_DATA_X_LSB_ADDR;
StressedDave 0:24f23c36dd24 161 _i2c.write(address,tx,1,true);
StressedDave 0:24f23c36dd24 162 _i2c.read(address+1,rawdata,6,0);
StressedDave 0:24f23c36dd24 163 lia.rawx = (rawdata[1] << 8 | rawdata[0]);
StressedDave 0:24f23c36dd24 164 lia.rawy = (rawdata[3] << 8 | rawdata[2]);
StressedDave 0:24f23c36dd24 165 lia.rawz = (rawdata[5] << 8 | rawdata[4]);
StressedDave 0:24f23c36dd24 166 lia.x = float(lia.rawx)*accel_scale;
StressedDave 0:24f23c36dd24 167 lia.y = float(lia.rawy)*accel_scale;
StressedDave 0:24f23c36dd24 168 lia.z = float(lia.rawz)*accel_scale;
StressedDave 0:24f23c36dd24 169 }
StressedDave 0:24f23c36dd24 170
StressedDave 0:24f23c36dd24 171 void BNO055::get_grv(void){
StressedDave 0:24f23c36dd24 172 tx[0] = BNO055_GRAVITY_DATA_X_LSB_ADDR;
StressedDave 0:24f23c36dd24 173 _i2c.write(address,tx,1,true);
StressedDave 0:24f23c36dd24 174 _i2c.read(address+1,rawdata,6,0);
StressedDave 0:24f23c36dd24 175 gravity.rawx = (rawdata[1] << 8 | rawdata[0]);
StressedDave 0:24f23c36dd24 176 gravity.rawy = (rawdata[3] << 8 | rawdata[2]);
StressedDave 0:24f23c36dd24 177 gravity.rawz = (rawdata[5] << 8 | rawdata[4]);
StressedDave 0:24f23c36dd24 178 gravity.x = float(gravity.rawx)*accel_scale;
StressedDave 0:24f23c36dd24 179 gravity.y = float(gravity.rawy)*accel_scale;
StressedDave 0:24f23c36dd24 180 gravity.z = float(gravity.rawz)*accel_scale;
StressedDave 0:24f23c36dd24 181 }
StressedDave 0:24f23c36dd24 182
StressedDave 1:2c3322a8d417 183 void BNO055::get_quat(void){
StressedDave 1:2c3322a8d417 184 tx[0] = BNO055_QUATERNION_DATA_W_LSB_ADDR;
StressedDave 1:2c3322a8d417 185 _i2c.write(address,tx,1,true);
StressedDave 1:2c3322a8d417 186 _i2c.read(address+1,rawdata,8,0);
StressedDave 1:2c3322a8d417 187 quat.raww = (rawdata[1] << 8 | rawdata[0]);
StressedDave 1:2c3322a8d417 188 quat.rawx = (rawdata[3] << 8 | rawdata[2]);
StressedDave 1:2c3322a8d417 189 quat.rawy = (rawdata[5] << 8 | rawdata[4]);
StressedDave 1:2c3322a8d417 190 quat.rawz = (rawdata[7] << 8 | rawdata[6]);
StressedDave 1:2c3322a8d417 191 quat.w = float(quat.raww)/16384.0f;
StressedDave 1:2c3322a8d417 192 quat.x = float(quat.rawx)/16384.0f;
StressedDave 1:2c3322a8d417 193 quat.y = float(quat.rawy)/16384.0f;
StressedDave 1:2c3322a8d417 194 quat.z = float(quat.rawz)/16384.0f;
StressedDave 1:2c3322a8d417 195 }
StressedDave 1:2c3322a8d417 196
StressedDave 1:2c3322a8d417 197 void BNO055::get_temp(void){
StressedDave 1:2c3322a8d417 198 readchar(BNO055_TEMP_ADDR);
StressedDave 1:2c3322a8d417 199 temperature = rx / temp_scale;
StressedDave 1:2c3322a8d417 200 }
StressedDave 1:2c3322a8d417 201
StressedDave 0:24f23c36dd24 202 void BNO055::get_calib(void){
StressedDave 0:24f23c36dd24 203 readchar(BNO055_CALIB_STAT_ADDR);
StressedDave 0:24f23c36dd24 204 calib = rx;
StressedDave 1:2c3322a8d417 205 }
StressedDave 1:2c3322a8d417 206
StressedDave 1:2c3322a8d417 207 void BNO055::read_calibration_data(void){
StressedDave 1:2c3322a8d417 208 char tempmode = op_mode;
StressedDave 1:2c3322a8d417 209 setmode(OPERATION_MODE_CONFIG);
StressedDave 1:2c3322a8d417 210 wait_ms(20);
StressedDave 1:2c3322a8d417 211 tx[0] = ACCEL_OFFSET_X_LSB_ADDR;
StressedDave 1:2c3322a8d417 212 _i2c.write(address,tx,1,true);
StressedDave 1:2c3322a8d417 213 _i2c.read(address,calibration,22,false);
StressedDave 1:2c3322a8d417 214 setmode(tempmode);
StressedDave 1:2c3322a8d417 215 wait_ms(10);
StressedDave 1:2c3322a8d417 216 }
StressedDave 1:2c3322a8d417 217
StressedDave 1:2c3322a8d417 218 void BNO055::write_calibration_data(void){
StressedDave 1:2c3322a8d417 219 char tempmode = op_mode;
StressedDave 1:2c3322a8d417 220 setmode(OPERATION_MODE_CONFIG);
StressedDave 1:2c3322a8d417 221 wait_ms(20);
StressedDave 1:2c3322a8d417 222 tx[0] = ACCEL_OFFSET_X_LSB_ADDR;
StressedDave 1:2c3322a8d417 223 _i2c.write(address,tx,1,true);
StressedDave 1:2c3322a8d417 224 _i2c.write(address,calibration,22,false);
StressedDave 1:2c3322a8d417 225 setmode(tempmode);
StressedDave 1:2c3322a8d417 226 wait_ms(10);
StressedDave 1:2c3322a8d417 227 }
StressedDave 1:2c3322a8d417 228
StressedDave 1:2c3322a8d417 229 void BNO055::set_mapping(char orient){
StressedDave 1:2c3322a8d417 230 switch (orient){
StressedDave 1:2c3322a8d417 231 case 0:
StressedDave 1:2c3322a8d417 232 writechar(BNO055_AXIS_MAP_CONFIG_ADDR,0x21);
StressedDave 1:2c3322a8d417 233 writechar(BNO055_AXIS_MAP_SIGN_ADDR,0x04);
StressedDave 1:2c3322a8d417 234 break;
StressedDave 1:2c3322a8d417 235 case 1:
StressedDave 1:2c3322a8d417 236 writechar(BNO055_AXIS_MAP_CONFIG_ADDR,0x24);
StressedDave 1:2c3322a8d417 237 writechar(BNO055_AXIS_MAP_SIGN_ADDR,0x00);
StressedDave 1:2c3322a8d417 238 break;
StressedDave 1:2c3322a8d417 239 case 2:
StressedDave 1:2c3322a8d417 240 writechar(BNO055_AXIS_MAP_CONFIG_ADDR,0x24);
StressedDave 1:2c3322a8d417 241 writechar(BNO055_AXIS_MAP_SIGN_ADDR,0x00);
StressedDave 1:2c3322a8d417 242 break;
StressedDave 1:2c3322a8d417 243 case 3:
StressedDave 1:2c3322a8d417 244 writechar(BNO055_AXIS_MAP_CONFIG_ADDR,0x21);
StressedDave 1:2c3322a8d417 245 writechar(BNO055_AXIS_MAP_SIGN_ADDR,0x02);
StressedDave 1:2c3322a8d417 246 break;
StressedDave 1:2c3322a8d417 247 case 4:
StressedDave 1:2c3322a8d417 248 writechar(BNO055_AXIS_MAP_CONFIG_ADDR,0x24);
StressedDave 1:2c3322a8d417 249 writechar(BNO055_AXIS_MAP_SIGN_ADDR,0x03);
StressedDave 1:2c3322a8d417 250 break;
StressedDave 1:2c3322a8d417 251 case 5:
StressedDave 1:2c3322a8d417 252 writechar(BNO055_AXIS_MAP_CONFIG_ADDR,0x21);
StressedDave 1:2c3322a8d417 253 writechar(BNO055_AXIS_MAP_SIGN_ADDR,0x01);
StressedDave 1:2c3322a8d417 254 break;
StressedDave 1:2c3322a8d417 255 case 6:
StressedDave 1:2c3322a8d417 256 writechar(BNO055_AXIS_MAP_CONFIG_ADDR,0x21);
StressedDave 1:2c3322a8d417 257 writechar(BNO055_AXIS_MAP_SIGN_ADDR,0x07);
StressedDave 1:2c3322a8d417 258 break;
StressedDave 1:2c3322a8d417 259 case 7:
StressedDave 1:2c3322a8d417 260 writechar(BNO055_AXIS_MAP_CONFIG_ADDR,0x24);
StressedDave 1:2c3322a8d417 261 writechar(BNO055_AXIS_MAP_SIGN_ADDR,0x05);
StressedDave 1:2c3322a8d417 262 break;
StressedDave 1:2c3322a8d417 263 default:
StressedDave 1:2c3322a8d417 264 writechar(BNO055_AXIS_MAP_CONFIG_ADDR,0x24);
StressedDave 1:2c3322a8d417 265 writechar(BNO055_AXIS_MAP_SIGN_ADDR,0x00);
StressedDave 1:2c3322a8d417 266 }
StressedDave 0:24f23c36dd24 267 }