UVic Assistive Technology Lab / Mbed 2 deprecated DSLR_Camera_Gimbal

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   mbed ros_lib_kinetic

BNO055/BNO055.cpp@15:aeb817f93336, 2018-12-14 (annotated)

Committer:
MikeGray92
Date:
Fri Dec 14 22:35:36 2018 +0000
Revision:
15:aeb817f93336
Parent:
0:3a767f41cf04 
Removed some unused code

Who changed what in which revision?

UserRevisionLine numberNew contents of line
group-UVic-Assistive-Technolog 0:3a767f41cf04 1 #include "BNO055.h"
group-UVic-Assistive-Technolog 0:3a767f41cf04 2 #include "mbed.h"
group-UVic-Assistive-Technolog 0:3a767f41cf04 3
group-UVic-Assistive-Technolog 0:3a767f41cf04 4 BNO055::BNO055(PinName SDA, PinName SCL) : _i2c(SDA,SCL){
group-UVic-Assistive-Technolog 0:3a767f41cf04 5 //Set I2C fast and bring reset line high
group-UVic-Assistive-Technolog 0:3a767f41cf04 6 _i2c.frequency(400000);
group-UVic-Assistive-Technolog 0:3a767f41cf04 7 address = BNOAddress;
group-UVic-Assistive-Technolog 0:3a767f41cf04 8 accel_scale = 0.001f;
group-UVic-Assistive-Technolog 0:3a767f41cf04 9 rate_scale = 1.0f/16.0f;
group-UVic-Assistive-Technolog 0:3a767f41cf04 10 angle_scale = 1.0f/16.0f;
group-UVic-Assistive-Technolog 0:3a767f41cf04 11 temp_scale = 1;
group-UVic-Assistive-Technolog 0:3a767f41cf04 12 }
group-UVic-Assistive-Technolog 0:3a767f41cf04 13
group-UVic-Assistive-Technolog 0:3a767f41cf04 14 void BNO055::reset(){
group-UVic-Assistive-Technolog 0:3a767f41cf04 15 //Perform a power-on-reset
group-UVic-Assistive-Technolog 0:3a767f41cf04 16 readchar(BNO055_SYS_TRIGGER_ADDR);
group-UVic-Assistive-Technolog 0:3a767f41cf04 17 rx = rx | 0x20;
group-UVic-Assistive-Technolog 0:3a767f41cf04 18 writechar(BNO055_SYS_TRIGGER_ADDR,rx);
group-UVic-Assistive-Technolog 0:3a767f41cf04 19 //Wait for the system to come back up again (datasheet says 650ms)
group-UVic-Assistive-Technolog 0:3a767f41cf04 20 wait_ms(675);
group-UVic-Assistive-Technolog 0:3a767f41cf04 21 }
group-UVic-Assistive-Technolog 0:3a767f41cf04 22
group-UVic-Assistive-Technolog 0:3a767f41cf04 23 bool BNO055::check(){
group-UVic-Assistive-Technolog 0:3a767f41cf04 24 //Check we have communication link with the chip
group-UVic-Assistive-Technolog 0:3a767f41cf04 25 readchar(BNO055_CHIP_ID_ADDR);
group-UVic-Assistive-Technolog 0:3a767f41cf04 26 if (rx != 0xA0) return false;
group-UVic-Assistive-Technolog 0:3a767f41cf04 27 //Grab the chip ID and software versions
group-UVic-Assistive-Technolog 0:3a767f41cf04 28 tx[0] = BNO055_CHIP_ID_ADDR;
group-UVic-Assistive-Technolog 0:3a767f41cf04 29 _i2c.write(address,tx,1,true);
group-UVic-Assistive-Technolog 0:3a767f41cf04 30 _i2c.read(address+1,rawdata,7,false);
group-UVic-Assistive-Technolog 0:3a767f41cf04 31 ID.id = rawdata[0];
group-UVic-Assistive-Technolog 0:3a767f41cf04 32 ID.accel = rawdata[1];
group-UVic-Assistive-Technolog 0:3a767f41cf04 33 ID.mag = rawdata[2];
group-UVic-Assistive-Technolog 0:3a767f41cf04 34 ID.gyro = rawdata[3];
group-UVic-Assistive-Technolog 0:3a767f41cf04 35 ID.sw[0] = rawdata[4];
group-UVic-Assistive-Technolog 0:3a767f41cf04 36 ID.sw[1] = rawdata[5];
group-UVic-Assistive-Technolog 0:3a767f41cf04 37 ID.bootload = rawdata[6];
group-UVic-Assistive-Technolog 0:3a767f41cf04 38 setpage(1);
group-UVic-Assistive-Technolog 0:3a767f41cf04 39 tx[0] = BNO055_UNIQUE_ID_ADDR;
group-UVic-Assistive-Technolog 0:3a767f41cf04 40 _i2c.write(address,tx,1,true);
group-UVic-Assistive-Technolog 0:3a767f41cf04 41 _i2c.read(address+1,ID.serial,16,false);
group-UVic-Assistive-Technolog 0:3a767f41cf04 42 setpage(0);
group-UVic-Assistive-Technolog 0:3a767f41cf04 43 return true;
group-UVic-Assistive-Technolog 0:3a767f41cf04 44 }
group-UVic-Assistive-Technolog 0:3a767f41cf04 45
group-UVic-Assistive-Technolog 0:3a767f41cf04 46 void BNO055::SetExternalCrystal(bool yn){
group-UVic-Assistive-Technolog 0:3a767f41cf04 47 // Read the current status from the device
group-UVic-Assistive-Technolog 0:3a767f41cf04 48 readchar(BNO055_SYS_TRIGGER_ADDR);
group-UVic-Assistive-Technolog 0:3a767f41cf04 49 if (yn) rx = rx | 0x80;
group-UVic-Assistive-Technolog 0:3a767f41cf04 50 else rx = rx & 0x7F;
group-UVic-Assistive-Technolog 0:3a767f41cf04 51 writechar(BNO055_SYS_TRIGGER_ADDR,rx);
group-UVic-Assistive-Technolog 0:3a767f41cf04 52 }
group-UVic-Assistive-Technolog 0:3a767f41cf04 53
group-UVic-Assistive-Technolog 0:3a767f41cf04 54 void BNO055::set_accel_units(char units){
group-UVic-Assistive-Technolog 0:3a767f41cf04 55 readchar(BNO055_UNIT_SEL_ADDR);
group-UVic-Assistive-Technolog 0:3a767f41cf04 56 if(units == MPERSPERS){
group-UVic-Assistive-Technolog 0:3a767f41cf04 57 rx = rx & 0xFE;
group-UVic-Assistive-Technolog 0:3a767f41cf04 58 accel_scale = 0.01f;
group-UVic-Assistive-Technolog 0:3a767f41cf04 59 }
group-UVic-Assistive-Technolog 0:3a767f41cf04 60 else {
group-UVic-Assistive-Technolog 0:3a767f41cf04 61 rx = rx | units;
group-UVic-Assistive-Technolog 0:3a767f41cf04 62 accel_scale = 0.001f;
group-UVic-Assistive-Technolog 0:3a767f41cf04 63 }
group-UVic-Assistive-Technolog 0:3a767f41cf04 64 writechar(BNO055_UNIT_SEL_ADDR,rx);
group-UVic-Assistive-Technolog 0:3a767f41cf04 65 }
group-UVic-Assistive-Technolog 0:3a767f41cf04 66
group-UVic-Assistive-Technolog 0:3a767f41cf04 67 void BNO055::set_anglerate_units(char units){
group-UVic-Assistive-Technolog 0:3a767f41cf04 68 readchar(BNO055_UNIT_SEL_ADDR);
group-UVic-Assistive-Technolog 0:3a767f41cf04 69 if (units == DEG_PER_SEC){
group-UVic-Assistive-Technolog 0:3a767f41cf04 70 rx = rx & 0xFD;
group-UVic-Assistive-Technolog 0:3a767f41cf04 71 rate_scale = 1.0f/16.0f;
group-UVic-Assistive-Technolog 0:3a767f41cf04 72 }
group-UVic-Assistive-Technolog 0:3a767f41cf04 73 else {
group-UVic-Assistive-Technolog 0:3a767f41cf04 74 rx = rx | units;
group-UVic-Assistive-Technolog 0:3a767f41cf04 75 rate_scale = 1.0f/900.0f;
group-UVic-Assistive-Technolog 0:3a767f41cf04 76 }
group-UVic-Assistive-Technolog 0:3a767f41cf04 77 writechar(BNO055_UNIT_SEL_ADDR,rx);
group-UVic-Assistive-Technolog 0:3a767f41cf04 78 }
group-UVic-Assistive-Technolog 0:3a767f41cf04 79
group-UVic-Assistive-Technolog 0:3a767f41cf04 80 void BNO055::set_angle_units(char units){
group-UVic-Assistive-Technolog 0:3a767f41cf04 81 readchar(BNO055_UNIT_SEL_ADDR);
group-UVic-Assistive-Technolog 0:3a767f41cf04 82 if (units == DEGREES){
group-UVic-Assistive-Technolog 0:3a767f41cf04 83 rx = rx & 0xFB;
group-UVic-Assistive-Technolog 0:3a767f41cf04 84 angle_scale = 1.0f/16.0f;
group-UVic-Assistive-Technolog 0:3a767f41cf04 85 }
group-UVic-Assistive-Technolog 0:3a767f41cf04 86 else {
group-UVic-Assistive-Technolog 0:3a767f41cf04 87 rx = rx | units;
group-UVic-Assistive-Technolog 0:3a767f41cf04 88 rate_scale = 1.0f/900.0f;
group-UVic-Assistive-Technolog 0:3a767f41cf04 89 }
group-UVic-Assistive-Technolog 0:3a767f41cf04 90 writechar(BNO055_UNIT_SEL_ADDR,rx);
group-UVic-Assistive-Technolog 0:3a767f41cf04 91 }
group-UVic-Assistive-Technolog 0:3a767f41cf04 92
group-UVic-Assistive-Technolog 0:3a767f41cf04 93 void BNO055::set_temp_units(char units){
group-UVic-Assistive-Technolog 0:3a767f41cf04 94 readchar(BNO055_UNIT_SEL_ADDR);
group-UVic-Assistive-Technolog 0:3a767f41cf04 95 if (units == CENTIGRADE){
group-UVic-Assistive-Technolog 0:3a767f41cf04 96 rx = rx & 0xEF;
group-UVic-Assistive-Technolog 0:3a767f41cf04 97 temp_scale = 1;
group-UVic-Assistive-Technolog 0:3a767f41cf04 98 }
group-UVic-Assistive-Technolog 0:3a767f41cf04 99 else {
group-UVic-Assistive-Technolog 0:3a767f41cf04 100 rx = rx | units;
group-UVic-Assistive-Technolog 0:3a767f41cf04 101 temp_scale = 2;
group-UVic-Assistive-Technolog 0:3a767f41cf04 102 }
group-UVic-Assistive-Technolog 0:3a767f41cf04 103 writechar(BNO055_UNIT_SEL_ADDR,rx);
group-UVic-Assistive-Technolog 0:3a767f41cf04 104 }
group-UVic-Assistive-Technolog 0:3a767f41cf04 105
group-UVic-Assistive-Technolog 0:3a767f41cf04 106 void BNO055::set_orientation(char units){
group-UVic-Assistive-Technolog 0:3a767f41cf04 107 readchar(BNO055_UNIT_SEL_ADDR);
group-UVic-Assistive-Technolog 0:3a767f41cf04 108 if (units == WINDOWS) rx = rx &0x7F;
group-UVic-Assistive-Technolog 0:3a767f41cf04 109 else rx = rx | units;
group-UVic-Assistive-Technolog 0:3a767f41cf04 110 writechar(BNO055_UNIT_SEL_ADDR,rx);
group-UVic-Assistive-Technolog 0:3a767f41cf04 111 }
group-UVic-Assistive-Technolog 0:3a767f41cf04 112
group-UVic-Assistive-Technolog 0:3a767f41cf04 113 void BNO055::setmode(char omode){
group-UVic-Assistive-Technolog 0:3a767f41cf04 114 writechar(BNO055_OPR_MODE_ADDR,omode);
group-UVic-Assistive-Technolog 0:3a767f41cf04 115 op_mode = omode;
group-UVic-Assistive-Technolog 0:3a767f41cf04 116 }
group-UVic-Assistive-Technolog 0:3a767f41cf04 117
group-UVic-Assistive-Technolog 0:3a767f41cf04 118 void BNO055::setpowermode(char pmode){
group-UVic-Assistive-Technolog 0:3a767f41cf04 119 writechar(BNO055_PWR_MODE_ADDR,pmode);
group-UVic-Assistive-Technolog 0:3a767f41cf04 120 pwr_mode = pmode;
group-UVic-Assistive-Technolog 0:3a767f41cf04 121 }
group-UVic-Assistive-Technolog 0:3a767f41cf04 122
group-UVic-Assistive-Technolog 0:3a767f41cf04 123 void BNO055::get_accel(void){
group-UVic-Assistive-Technolog 0:3a767f41cf04 124 tx[0] = BNO055_ACCEL_DATA_X_LSB_ADDR;
group-UVic-Assistive-Technolog 0:3a767f41cf04 125 _i2c.write(address,tx,1,true);
group-UVic-Assistive-Technolog 0:3a767f41cf04 126 _i2c.read(address+1,rawdata,6,0);
group-UVic-Assistive-Technolog 0:3a767f41cf04 127 accel.rawx = (rawdata[1] << 8 | rawdata[0]);
group-UVic-Assistive-Technolog 0:3a767f41cf04 128 accel.rawy = (rawdata[3] << 8 | rawdata[2]);
group-UVic-Assistive-Technolog 0:3a767f41cf04 129 accel.rawz = (rawdata[5] << 8 | rawdata[4]);
group-UVic-Assistive-Technolog 0:3a767f41cf04 130 accel.x = float(accel.rawx)*accel_scale;
group-UVic-Assistive-Technolog 0:3a767f41cf04 131 accel.y = float(accel.rawy)*accel_scale;
group-UVic-Assistive-Technolog 0:3a767f41cf04 132 accel.z = float(accel.rawz)*accel_scale;
group-UVic-Assistive-Technolog 0:3a767f41cf04 133 }
group-UVic-Assistive-Technolog 0:3a767f41cf04 134
group-UVic-Assistive-Technolog 0:3a767f41cf04 135 void BNO055::get_gyro(void){
group-UVic-Assistive-Technolog 0:3a767f41cf04 136 tx[0] = BNO055_GYRO_DATA_X_LSB_ADDR;
group-UVic-Assistive-Technolog 0:3a767f41cf04 137 _i2c.write(address,tx,1,true);
group-UVic-Assistive-Technolog 0:3a767f41cf04 138 _i2c.read(address+1,rawdata,6,0);
group-UVic-Assistive-Technolog 0:3a767f41cf04 139 gyro.rawx = (rawdata[1] << 8 | rawdata[0]);
group-UVic-Assistive-Technolog 0:3a767f41cf04 140 gyro.rawy = (rawdata[3] << 8 | rawdata[2]);
group-UVic-Assistive-Technolog 0:3a767f41cf04 141 gyro.rawz = (rawdata[5] << 8 | rawdata[4]);
group-UVic-Assistive-Technolog 0:3a767f41cf04 142 gyro.x = float(gyro.rawx)*rate_scale;
group-UVic-Assistive-Technolog 0:3a767f41cf04 143 gyro.y = float(gyro.rawy)*rate_scale;
group-UVic-Assistive-Technolog 0:3a767f41cf04 144 gyro.z = float(gyro.rawz)*rate_scale;
group-UVic-Assistive-Technolog 0:3a767f41cf04 145 }
group-UVic-Assistive-Technolog 0:3a767f41cf04 146
group-UVic-Assistive-Technolog 0:3a767f41cf04 147 void BNO055::get_mag(void){
group-UVic-Assistive-Technolog 0:3a767f41cf04 148 tx[0] = BNO055_MAG_DATA_X_LSB_ADDR;
group-UVic-Assistive-Technolog 0:3a767f41cf04 149 _i2c.write(address,tx,1,true);
group-UVic-Assistive-Technolog 0:3a767f41cf04 150 _i2c.read(address+1,rawdata,6,0);
group-UVic-Assistive-Technolog 0:3a767f41cf04 151 mag.rawx = (rawdata[1] << 8 | rawdata[0]);
group-UVic-Assistive-Technolog 0:3a767f41cf04 152 mag.rawy = (rawdata[3] << 8 | rawdata[2]);
group-UVic-Assistive-Technolog 0:3a767f41cf04 153 mag.rawz = (rawdata[5] << 8 | rawdata[4]);
group-UVic-Assistive-Technolog 0:3a767f41cf04 154 mag.x = float(mag.rawx);
group-UVic-Assistive-Technolog 0:3a767f41cf04 155 mag.y = float(mag.rawy);
group-UVic-Assistive-Technolog 0:3a767f41cf04 156 mag.z = float(mag.rawz);
group-UVic-Assistive-Technolog 0:3a767f41cf04 157 }
group-UVic-Assistive-Technolog 0:3a767f41cf04 158
group-UVic-Assistive-Technolog 0:3a767f41cf04 159 void BNO055::get_lia(void){
group-UVic-Assistive-Technolog 0:3a767f41cf04 160 tx[0] = BNO055_LINEAR_ACCEL_DATA_X_LSB_ADDR;
group-UVic-Assistive-Technolog 0:3a767f41cf04 161 _i2c.write(address,tx,1,true);
group-UVic-Assistive-Technolog 0:3a767f41cf04 162 _i2c.read(address+1,rawdata,6,0);
group-UVic-Assistive-Technolog 0:3a767f41cf04 163 lia.rawx = (rawdata[1] << 8 | rawdata[0]);
group-UVic-Assistive-Technolog 0:3a767f41cf04 164 lia.rawy = (rawdata[3] << 8 | rawdata[2]);
group-UVic-Assistive-Technolog 0:3a767f41cf04 165 lia.rawz = (rawdata[5] << 8 | rawdata[4]);
group-UVic-Assistive-Technolog 0:3a767f41cf04 166 lia.x = float(lia.rawx)*accel_scale;
group-UVic-Assistive-Technolog 0:3a767f41cf04 167 lia.y = float(lia.rawy)*accel_scale;
group-UVic-Assistive-Technolog 0:3a767f41cf04 168 lia.z = float(lia.rawz)*accel_scale;
group-UVic-Assistive-Technolog 0:3a767f41cf04 169 }
group-UVic-Assistive-Technolog 0:3a767f41cf04 170
group-UVic-Assistive-Technolog 0:3a767f41cf04 171 void BNO055::get_grv(void){
group-UVic-Assistive-Technolog 0:3a767f41cf04 172 tx[0] = BNO055_GRAVITY_DATA_X_LSB_ADDR;
group-UVic-Assistive-Technolog 0:3a767f41cf04 173 _i2c.write(address,tx,1,true);
group-UVic-Assistive-Technolog 0:3a767f41cf04 174 _i2c.read(address+1,rawdata,6,0);
group-UVic-Assistive-Technolog 0:3a767f41cf04 175 gravity.rawx = (rawdata[1] << 8 | rawdata[0]);
group-UVic-Assistive-Technolog 0:3a767f41cf04 176 gravity.rawy = (rawdata[3] << 8 | rawdata[2]);
group-UVic-Assistive-Technolog 0:3a767f41cf04 177 gravity.rawz = (rawdata[5] << 8 | rawdata[4]);
group-UVic-Assistive-Technolog 0:3a767f41cf04 178 gravity.x = float(gravity.rawx)*accel_scale;
group-UVic-Assistive-Technolog 0:3a767f41cf04 179 gravity.y = float(gravity.rawy)*accel_scale;
group-UVic-Assistive-Technolog 0:3a767f41cf04 180 gravity.z = float(gravity.rawz)*accel_scale;
group-UVic-Assistive-Technolog 0:3a767f41cf04 181 }
group-UVic-Assistive-Technolog 0:3a767f41cf04 182
group-UVic-Assistive-Technolog 0:3a767f41cf04 183 void BNO055::get_quat(void){
group-UVic-Assistive-Technolog 0:3a767f41cf04 184 tx[0] = BNO055_QUATERNION_DATA_W_LSB_ADDR;
group-UVic-Assistive-Technolog 0:3a767f41cf04 185 _i2c.write(address,tx,1,true);
group-UVic-Assistive-Technolog 0:3a767f41cf04 186 _i2c.read(address+1,rawdata,8,0);
group-UVic-Assistive-Technolog 0:3a767f41cf04 187 quat.raww = (rawdata[1] << 8 | rawdata[0]);
group-UVic-Assistive-Technolog 0:3a767f41cf04 188 quat.rawx = (rawdata[3] << 8 | rawdata[2]);
group-UVic-Assistive-Technolog 0:3a767f41cf04 189 quat.rawy = (rawdata[5] << 8 | rawdata[4]);
group-UVic-Assistive-Technolog 0:3a767f41cf04 190 quat.rawz = (rawdata[7] << 8 | rawdata[6]);
group-UVic-Assistive-Technolog 0:3a767f41cf04 191 quat.w = float(quat.raww)/16384.0f;
group-UVic-Assistive-Technolog 0:3a767f41cf04 192 quat.x = float(quat.rawx)/16384.0f;
group-UVic-Assistive-Technolog 0:3a767f41cf04 193 quat.y = float(quat.rawy)/16384.0f;
group-UVic-Assistive-Technolog 0:3a767f41cf04 194 quat.z = float(quat.rawz)/16384.0f;
group-UVic-Assistive-Technolog 0:3a767f41cf04 195 }
group-UVic-Assistive-Technolog 0:3a767f41cf04 196
group-UVic-Assistive-Technolog 0:3a767f41cf04 197 void BNO055::get_angles(void){
group-UVic-Assistive-Technolog 0:3a767f41cf04 198 tx[0] = BNO055_EULER_H_LSB_ADDR;
group-UVic-Assistive-Technolog 0:3a767f41cf04 199 _i2c.write(address,tx,1,true);
group-UVic-Assistive-Technolog 0:3a767f41cf04 200 _i2c.read(address+1,rawdata,6,0);
group-UVic-Assistive-Technolog 0:3a767f41cf04 201 euler.rawyaw = (rawdata[1] << 8 | rawdata[0]);
group-UVic-Assistive-Technolog 0:3a767f41cf04 202 euler.rawroll = (rawdata[3] << 8 | rawdata[2]);
group-UVic-Assistive-Technolog 0:3a767f41cf04 203 euler.rawpitch = (rawdata[5] << 8 | rawdata[4]);
group-UVic-Assistive-Technolog 0:3a767f41cf04 204 euler.yaw = float(euler.rawyaw)*angle_scale;
group-UVic-Assistive-Technolog 0:3a767f41cf04 205 euler.roll = float(euler.rawroll)*angle_scale;
group-UVic-Assistive-Technolog 0:3a767f41cf04 206 euler.pitch = float(euler.rawpitch)*angle_scale;
group-UVic-Assistive-Technolog 0:3a767f41cf04 207 }
group-UVic-Assistive-Technolog 0:3a767f41cf04 208
group-UVic-Assistive-Technolog 0:3a767f41cf04 209
group-UVic-Assistive-Technolog 0:3a767f41cf04 210 void BNO055::get_temp(void){
group-UVic-Assistive-Technolog 0:3a767f41cf04 211 readchar(BNO055_TEMP_ADDR);
group-UVic-Assistive-Technolog 0:3a767f41cf04 212 temperature = rx / temp_scale;
group-UVic-Assistive-Technolog 0:3a767f41cf04 213 }
group-UVic-Assistive-Technolog 0:3a767f41cf04 214
group-UVic-Assistive-Technolog 0:3a767f41cf04 215 void BNO055::get_calib(void){
group-UVic-Assistive-Technolog 0:3a767f41cf04 216 readchar(BNO055_CALIB_STAT_ADDR);
group-UVic-Assistive-Technolog 0:3a767f41cf04 217 calib = rx;
group-UVic-Assistive-Technolog 0:3a767f41cf04 218 }
group-UVic-Assistive-Technolog 0:3a767f41cf04 219
group-UVic-Assistive-Technolog 0:3a767f41cf04 220 void BNO055::read_calibration_data(void){
group-UVic-Assistive-Technolog 0:3a767f41cf04 221 char tempmode = op_mode;
group-UVic-Assistive-Technolog 0:3a767f41cf04 222 setmode(OPERATION_MODE_CONFIG);
group-UVic-Assistive-Technolog 0:3a767f41cf04 223 wait_ms(20);
group-UVic-Assistive-Technolog 0:3a767f41cf04 224 tx[0] = ACCEL_OFFSET_X_LSB_ADDR;
group-UVic-Assistive-Technolog 0:3a767f41cf04 225 _i2c.write(address,tx,1,true);
group-UVic-Assistive-Technolog 0:3a767f41cf04 226 _i2c.read(address,calibration,22,false);
group-UVic-Assistive-Technolog 0:3a767f41cf04 227 setmode(tempmode);
group-UVic-Assistive-Technolog 0:3a767f41cf04 228 wait_ms(10);
group-UVic-Assistive-Technolog 0:3a767f41cf04 229 }
group-UVic-Assistive-Technolog 0:3a767f41cf04 230
group-UVic-Assistive-Technolog 0:3a767f41cf04 231 void BNO055::write_calibration_data(void){
group-UVic-Assistive-Technolog 0:3a767f41cf04 232 char tempmode = op_mode;
group-UVic-Assistive-Technolog 0:3a767f41cf04 233 setmode(OPERATION_MODE_CONFIG);
group-UVic-Assistive-Technolog 0:3a767f41cf04 234 wait_ms(20);
group-UVic-Assistive-Technolog 0:3a767f41cf04 235 tx[0] = ACCEL_OFFSET_X_LSB_ADDR;
group-UVic-Assistive-Technolog 0:3a767f41cf04 236 _i2c.write(address,tx,1,true);
group-UVic-Assistive-Technolog 0:3a767f41cf04 237 _i2c.write(address,calibration,22,false);
group-UVic-Assistive-Technolog 0:3a767f41cf04 238 setmode(tempmode);
group-UVic-Assistive-Technolog 0:3a767f41cf04 239 wait_ms(10);
group-UVic-Assistive-Technolog 0:3a767f41cf04 240 }
group-UVic-Assistive-Technolog 0:3a767f41cf04 241
group-UVic-Assistive-Technolog 0:3a767f41cf04 242 void BNO055::set_mapping(char orient){
group-UVic-Assistive-Technolog 0:3a767f41cf04 243 switch (orient){
group-UVic-Assistive-Technolog 0:3a767f41cf04 244 case 0:
group-UVic-Assistive-Technolog 0:3a767f41cf04 245 writechar(BNO055_AXIS_MAP_CONFIG_ADDR,0x21);
group-UVic-Assistive-Technolog 0:3a767f41cf04 246 writechar(BNO055_AXIS_MAP_SIGN_ADDR,0x04);
group-UVic-Assistive-Technolog 0:3a767f41cf04 247 break;
group-UVic-Assistive-Technolog 0:3a767f41cf04 248 case 1:
group-UVic-Assistive-Technolog 0:3a767f41cf04 249 writechar(BNO055_AXIS_MAP_CONFIG_ADDR,0x24);
group-UVic-Assistive-Technolog 0:3a767f41cf04 250 writechar(BNO055_AXIS_MAP_SIGN_ADDR,0x00);
group-UVic-Assistive-Technolog 0:3a767f41cf04 251 break;
group-UVic-Assistive-Technolog 0:3a767f41cf04 252 case 2:
group-UVic-Assistive-Technolog 0:3a767f41cf04 253 writechar(BNO055_AXIS_MAP_CONFIG_ADDR,0x24);
group-UVic-Assistive-Technolog 0:3a767f41cf04 254 writechar(BNO055_AXIS_MAP_SIGN_ADDR,0x00);
group-UVic-Assistive-Technolog 0:3a767f41cf04 255 break;
group-UVic-Assistive-Technolog 0:3a767f41cf04 256 case 3:
group-UVic-Assistive-Technolog 0:3a767f41cf04 257 writechar(BNO055_AXIS_MAP_CONFIG_ADDR,0x21);
group-UVic-Assistive-Technolog 0:3a767f41cf04 258 writechar(BNO055_AXIS_MAP_SIGN_ADDR,0x02);
group-UVic-Assistive-Technolog 0:3a767f41cf04 259 break;
group-UVic-Assistive-Technolog 0:3a767f41cf04 260 case 4:
group-UVic-Assistive-Technolog 0:3a767f41cf04 261 writechar(BNO055_AXIS_MAP_CONFIG_ADDR,0x24);
group-UVic-Assistive-Technolog 0:3a767f41cf04 262 writechar(BNO055_AXIS_MAP_SIGN_ADDR,0x03);
group-UVic-Assistive-Technolog 0:3a767f41cf04 263 break;
group-UVic-Assistive-Technolog 0:3a767f41cf04 264 case 5:
group-UVic-Assistive-Technolog 0:3a767f41cf04 265 writechar(BNO055_AXIS_MAP_CONFIG_ADDR,0x21);
group-UVic-Assistive-Technolog 0:3a767f41cf04 266 writechar(BNO055_AXIS_MAP_SIGN_ADDR,0x01);
group-UVic-Assistive-Technolog 0:3a767f41cf04 267 break;
group-UVic-Assistive-Technolog 0:3a767f41cf04 268 case 6:
group-UVic-Assistive-Technolog 0:3a767f41cf04 269 writechar(BNO055_AXIS_MAP_CONFIG_ADDR,0x21);
group-UVic-Assistive-Technolog 0:3a767f41cf04 270 writechar(BNO055_AXIS_MAP_SIGN_ADDR,0x07);
group-UVic-Assistive-Technolog 0:3a767f41cf04 271 break;
group-UVic-Assistive-Technolog 0:3a767f41cf04 272 case 7:
group-UVic-Assistive-Technolog 0:3a767f41cf04 273 writechar(BNO055_AXIS_MAP_CONFIG_ADDR,0x24);
group-UVic-Assistive-Technolog 0:3a767f41cf04 274 writechar(BNO055_AXIS_MAP_SIGN_ADDR,0x05);
group-UVic-Assistive-Technolog 0:3a767f41cf04 275 break;
group-UVic-Assistive-Technolog 0:3a767f41cf04 276 default:
group-UVic-Assistive-Technolog 0:3a767f41cf04 277 writechar(BNO055_AXIS_MAP_CONFIG_ADDR,0x24);
group-UVic-Assistive-Technolog 0:3a767f41cf04 278 writechar(BNO055_AXIS_MAP_SIGN_ADDR,0x00);
group-UVic-Assistive-Technolog 0:3a767f41cf04 279 }
group-UVic-Assistive-Technolog 0:3a767f41cf04 280 }