richard misi / BNO055

bno055

Dependents:   LPC1768-GPS-FUSION-17102018-1

Fork of BNO055 by Dave Turner

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 }