Tsungta Wu / BNO055_AS7000

Combine AS7000 into BNO055

Fork of BNO055 by Sog Yang

BNO055.cpp@4:481ecdf3baf8, 2015-09-10 (annotated)

Committer:
StressedDave
Date:
Thu Sep 10 15:48:09 2015 +0000
Revision:
4:481ecdf3baf8
Parent:
3:1db1628eb8b2
Child:
5:beaa2bff7ff0
Dropped I2C rate down to 100k to see if it pulls data down more consistently on longer reads

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 4:481ecdf3baf8 6 _i2c.frequency(100000);
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 }