Sog Yang / BNO055

BNO055 ARM mBed C++ Driver

Fork of BNO055 by Dave Turner

BNO055.cpp@3:1db1628eb8b2, 2015-05-31 (annotated)

Committer:
StressedDave
Date:
Sun May 31 07:22:40 2015 +0000
Revision:
3:1db1628eb8b2
Parent:
2:695c6e5d239a
Child:
4:481ecdf3baf8
Corrected error in acquisition of euler angles

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 2:695c6e5d239a 197 void BNO055::get_angles(void){
StressedDave 2:695c6e5d239a 198 tx[0] = BNO055_EULER_H_LSB_ADDR;
StressedDave 2:695c6e5d239a 199 _i2c.write(address,tx,1,true);
StressedDave 2:695c6e5d239a 200 _i2c.read(address+1,rawdata,6,0);
StressedDave 3:1db1628eb8b2 201 euler.rawyaw = (rawdata[1] << 8 | rawdata[0]);
StressedDave 3:1db1628eb8b2 202 euler.rawroll = (rawdata[3] << 8 | rawdata[2]);
StressedDave 3:1db1628eb8b2 203 euler.rawpitch = (rawdata[5] << 8 | rawdata[4]);
StressedDave 3:1db1628eb8b2 204 euler.yaw = float(euler.rawyaw)*angle_scale;
StressedDave 3:1db1628eb8b2 205 euler.roll = float(euler.rawroll)*angle_scale;
StressedDave 3:1db1628eb8b2 206 euler.pitch = float(euler.rawpitch)*angle_scale;
StressedDave 2:695c6e5d239a 207 }
StressedDave 2:695c6e5d239a 208
StressedDave 2:695c6e5d239a 209
StressedDave 1:2c3322a8d417 210 void BNO055::get_temp(void){
StressedDave 1:2c3322a8d417 211 readchar(BNO055_TEMP_ADDR);
StressedDave 1:2c3322a8d417 212 temperature = rx / temp_scale;
StressedDave 1:2c3322a8d417 213 }
StressedDave 1:2c3322a8d417 214
StressedDave 0:24f23c36dd24 215 void BNO055::get_calib(void){
StressedDave 0:24f23c36dd24 216 readchar(BNO055_CALIB_STAT_ADDR);
StressedDave 0:24f23c36dd24 217 calib = rx;
StressedDave 1:2c3322a8d417 218 }
StressedDave 1:2c3322a8d417 219
StressedDave 1:2c3322a8d417 220 void BNO055::read_calibration_data(void){
StressedDave 1:2c3322a8d417 221 char tempmode = op_mode;
StressedDave 1:2c3322a8d417 222 setmode(OPERATION_MODE_CONFIG);
StressedDave 1:2c3322a8d417 223 wait_ms(20);
StressedDave 1:2c3322a8d417 224 tx[0] = ACCEL_OFFSET_X_LSB_ADDR;
StressedDave 1:2c3322a8d417 225 _i2c.write(address,tx,1,true);
StressedDave 1:2c3322a8d417 226 _i2c.read(address,calibration,22,false);
StressedDave 1:2c3322a8d417 227 setmode(tempmode);
StressedDave 1:2c3322a8d417 228 wait_ms(10);
StressedDave 1:2c3322a8d417 229 }
StressedDave 1:2c3322a8d417 230
StressedDave 1:2c3322a8d417 231 void BNO055::write_calibration_data(void){
StressedDave 1:2c3322a8d417 232 char tempmode = op_mode;
StressedDave 1:2c3322a8d417 233 setmode(OPERATION_MODE_CONFIG);
StressedDave 1:2c3322a8d417 234 wait_ms(20);
StressedDave 1:2c3322a8d417 235 tx[0] = ACCEL_OFFSET_X_LSB_ADDR;
StressedDave 1:2c3322a8d417 236 _i2c.write(address,tx,1,true);
StressedDave 1:2c3322a8d417 237 _i2c.write(address,calibration,22,false);
StressedDave 1:2c3322a8d417 238 setmode(tempmode);
StressedDave 1:2c3322a8d417 239 wait_ms(10);
StressedDave 1:2c3322a8d417 240 }
StressedDave 1:2c3322a8d417 241
StressedDave 1:2c3322a8d417 242 void BNO055::set_mapping(char orient){
StressedDave 1:2c3322a8d417 243 switch (orient){
StressedDave 1:2c3322a8d417 244 case 0:
StressedDave 1:2c3322a8d417 245 writechar(BNO055_AXIS_MAP_CONFIG_ADDR,0x21);
StressedDave 1:2c3322a8d417 246 writechar(BNO055_AXIS_MAP_SIGN_ADDR,0x04);
StressedDave 1:2c3322a8d417 247 break;
StressedDave 1:2c3322a8d417 248 case 1:
StressedDave 1:2c3322a8d417 249 writechar(BNO055_AXIS_MAP_CONFIG_ADDR,0x24);
StressedDave 1:2c3322a8d417 250 writechar(BNO055_AXIS_MAP_SIGN_ADDR,0x00);
StressedDave 1:2c3322a8d417 251 break;
StressedDave 1:2c3322a8d417 252 case 2:
StressedDave 1:2c3322a8d417 253 writechar(BNO055_AXIS_MAP_CONFIG_ADDR,0x24);
StressedDave 1:2c3322a8d417 254 writechar(BNO055_AXIS_MAP_SIGN_ADDR,0x00);
StressedDave 1:2c3322a8d417 255 break;
StressedDave 1:2c3322a8d417 256 case 3:
StressedDave 1:2c3322a8d417 257 writechar(BNO055_AXIS_MAP_CONFIG_ADDR,0x21);
StressedDave 1:2c3322a8d417 258 writechar(BNO055_AXIS_MAP_SIGN_ADDR,0x02);
StressedDave 1:2c3322a8d417 259 break;
StressedDave 1:2c3322a8d417 260 case 4:
StressedDave 1:2c3322a8d417 261 writechar(BNO055_AXIS_MAP_CONFIG_ADDR,0x24);
StressedDave 1:2c3322a8d417 262 writechar(BNO055_AXIS_MAP_SIGN_ADDR,0x03);
StressedDave 1:2c3322a8d417 263 break;
StressedDave 1:2c3322a8d417 264 case 5:
StressedDave 1:2c3322a8d417 265 writechar(BNO055_AXIS_MAP_CONFIG_ADDR,0x21);
StressedDave 1:2c3322a8d417 266 writechar(BNO055_AXIS_MAP_SIGN_ADDR,0x01);
StressedDave 1:2c3322a8d417 267 break;
StressedDave 1:2c3322a8d417 268 case 6:
StressedDave 1:2c3322a8d417 269 writechar(BNO055_AXIS_MAP_CONFIG_ADDR,0x21);
StressedDave 1:2c3322a8d417 270 writechar(BNO055_AXIS_MAP_SIGN_ADDR,0x07);
StressedDave 1:2c3322a8d417 271 break;
StressedDave 1:2c3322a8d417 272 case 7:
StressedDave 1:2c3322a8d417 273 writechar(BNO055_AXIS_MAP_CONFIG_ADDR,0x24);
StressedDave 1:2c3322a8d417 274 writechar(BNO055_AXIS_MAP_SIGN_ADDR,0x05);
StressedDave 1:2c3322a8d417 275 break;
StressedDave 1:2c3322a8d417 276 default:
StressedDave 1:2c3322a8d417 277 writechar(BNO055_AXIS_MAP_CONFIG_ADDR,0x24);
StressedDave 1:2c3322a8d417 278 writechar(BNO055_AXIS_MAP_SIGN_ADDR,0x00);
StressedDave 1:2c3322a8d417 279 }
StressedDave 0:24f23c36dd24 280 }