A patched version of the BNO055 library for KMZ25 board + Adafruit BNO055 breakout.
Fork of BNO055 by
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?
User | Revision | Line number | New 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 | } |