Joseph Bradshaw
/
BNO055
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.
Fork of
BNO055
by 
Dave Turner
Embed:
(wiki syntax)
Show/hide line numbers
BNO055.cpp
00001 #include "BNO055.h" 00002 #include "mbed.h" 00003 00004 BNO055::BNO055(PinName SDA, PinName SCL) : _i2c(SDA,SCL){ 00005 //Set I2C fast and bring reset line high 00006 _i2c.frequency(400000); 00007 address = BNOAddress; 00008 accel_scale = 0.001f; 00009 rate_scale = 1.0f/16.0f; 00010 angle_scale = 1.0f/16.0f; 00011 temp_scale = 1; 00012 } 00013 00014 void BNO055::reset(){ 00015 //Perform a power-on-reset 00016 readchar(BNO055_SYS_TRIGGER_ADDR); 00017 rx = rx | 0x20; 00018 writechar(BNO055_SYS_TRIGGER_ADDR,rx); 00019 //Wait for the system to come back up again (datasheet says 650ms) 00020 wait_ms(675); 00021 } 00022 00023 bool BNO055::check(){ 00024 //Check we have communication link with the chip 00025 readchar(BNO055_CHIP_ID_ADDR); 00026 if (rx != 0xA0) return false; 00027 //Grab the chip ID and software versions 00028 tx[0] = BNO055_CHIP_ID_ADDR; 00029 _i2c.write(address,tx,1,true); 00030 _i2c.read(address+1,rawdata,7,false); 00031 ID.id = rawdata[0]; 00032 ID.accel = rawdata[1]; 00033 ID.mag = rawdata[2]; 00034 ID.gyro = rawdata[3]; 00035 ID.sw[0] = rawdata[4]; 00036 ID.sw[1] = rawdata[5]; 00037 ID.bootload = rawdata[6]; 00038 setpage(1); 00039 tx[0] = BNO055_UNIQUE_ID_ADDR; 00040 _i2c.write(address,tx,1,true); 00041 _i2c.read(address+1,ID.serial,16,false); 00042 setpage(0); 00043 return true; 00044 } 00045 00046 void BNO055::SetExternalCrystal(bool yn){ 00047 // Read the current status from the device 00048 readchar(BNO055_SYS_TRIGGER_ADDR); 00049 if (yn) rx = rx | 0x80; 00050 else rx = rx & 0x7F; 00051 writechar(BNO055_SYS_TRIGGER_ADDR,rx); 00052 } 00053 00054 void BNO055::set_accel_units(char units){ 00055 readchar(BNO055_UNIT_SEL_ADDR); 00056 if(units == MPERSPERS){ 00057 rx = rx & 0xFE; 00058 accel_scale = 0.01f; 00059 } 00060 else { 00061 rx = rx | units; 00062 accel_scale = 0.001f; 00063 } 00064 writechar(BNO055_UNIT_SEL_ADDR,rx); 00065 } 00066 00067 void BNO055::set_anglerate_units(char units){ 00068 readchar(BNO055_UNIT_SEL_ADDR); 00069 if (units == DEG_PER_SEC){ 00070 rx = rx & 0xFD; 00071 rate_scale = 1.0f/16.0f; 00072 } 00073 else { 00074 rx = rx | units; 00075 rate_scale = 1.0f/900.0f; 00076 } 00077 writechar(BNO055_UNIT_SEL_ADDR,rx); 00078 } 00079 00080 void BNO055::set_angle_units(char units){ 00081 readchar(BNO055_UNIT_SEL_ADDR); 00082 if (units == DEGREES){ 00083 rx = rx & 0xFB; 00084 angle_scale = 1.0f/16.0f; 00085 } 00086 else { 00087 rx = rx | units; 00088 angle_scale = 1.0f/900.0f; 00089 } 00090 writechar(BNO055_UNIT_SEL_ADDR,rx); 00091 } 00092 00093 void BNO055::set_temp_units(char units){ 00094 readchar(BNO055_UNIT_SEL_ADDR); 00095 if (units == CENTIGRADE){ 00096 rx = rx & 0xEF; 00097 temp_scale = 1; 00098 } 00099 else { 00100 rx = rx | units; 00101 temp_scale = 2; 00102 } 00103 writechar(BNO055_UNIT_SEL_ADDR,rx); 00104 } 00105 00106 void BNO055::set_orientation(int position){ 00107 switch(position){ 00108 case 0: 00109 writechar(BNO055_AXIS_MAP_CONFIG_ADDR,0x21); 00110 writechar(BNO055_AXIS_MAP_SIGN_ADDR,0x04); 00111 break; 00112 case 1: //(Default) 00113 writechar(BNO055_AXIS_MAP_CONFIG_ADDR,0x24); 00114 writechar(BNO055_AXIS_MAP_SIGN_ADDR,0x00); 00115 break; 00116 case 2: 00117 writechar(BNO055_AXIS_MAP_CONFIG_ADDR,0x24); 00118 writechar(BNO055_AXIS_MAP_SIGN_ADDR,0x06); 00119 break; 00120 case 3: 00121 writechar(BNO055_AXIS_MAP_CONFIG_ADDR,0x21); 00122 writechar(BNO055_AXIS_MAP_SIGN_ADDR,0x02); 00123 break; 00124 case 4: 00125 writechar(BNO055_AXIS_MAP_CONFIG_ADDR,0x24); 00126 writechar(BNO055_AXIS_MAP_SIGN_ADDR,0x03); 00127 break; 00128 case 5: 00129 writechar(BNO055_AXIS_MAP_CONFIG_ADDR,0x21); 00130 writechar(BNO055_AXIS_MAP_SIGN_ADDR,0x01); 00131 break; 00132 case 6: 00133 writechar(BNO055_AXIS_MAP_CONFIG_ADDR,0x21); 00134 writechar(BNO055_AXIS_MAP_SIGN_ADDR,0x07); 00135 break; 00136 case 7: 00137 writechar(BNO055_AXIS_MAP_CONFIG_ADDR,0x24); 00138 writechar(BNO055_AXIS_MAP_SIGN_ADDR,0x05); 00139 break; 00140 default: 00141 writechar(BNO055_AXIS_MAP_CONFIG_ADDR,0x24); 00142 writechar(BNO055_AXIS_MAP_SIGN_ADDR,0x00); 00143 break; 00144 } 00145 } 00146 00147 void BNO055::setmode(char omode){ 00148 writechar(BNO055_OPR_MODE_ADDR,omode); 00149 op_mode = omode; 00150 } 00151 00152 void BNO055::setpowermode(char pmode){ 00153 writechar(BNO055_PWR_MODE_ADDR,pmode); 00154 pwr_mode = pmode; 00155 } 00156 00157 void BNO055::get_accel(void){ 00158 tx[0] = BNO055_ACCEL_DATA_X_LSB_ADDR; 00159 _i2c.write(address,tx,1,true); 00160 _i2c.read(address+1,rawdata,6,0); 00161 accel.rawx = (rawdata[1] << 8 | rawdata[0]); 00162 accel.rawy = (rawdata[3] << 8 | rawdata[2]); 00163 accel.rawz = (rawdata[5] << 8 | rawdata[4]); 00164 accel.x = float(accel.rawx)*accel_scale; 00165 accel.y = float(accel.rawy)*accel_scale; 00166 accel.z = float(accel.rawz)*accel_scale; 00167 } 00168 00169 void BNO055::get_gyro(void){ 00170 tx[0] = BNO055_GYRO_DATA_X_LSB_ADDR; 00171 _i2c.write(address,tx,1,true); 00172 _i2c.read(address+1,rawdata,6,0); 00173 gyro.rawx = (rawdata[1] << 8 | rawdata[0]); 00174 gyro.rawy = (rawdata[3] << 8 | rawdata[2]); 00175 gyro.rawz = (rawdata[5] << 8 | rawdata[4]); 00176 gyro.x = float(gyro.rawx)*rate_scale; 00177 gyro.y = float(gyro.rawy)*rate_scale; 00178 gyro.z = float(gyro.rawz)*rate_scale; 00179 } 00180 00181 void BNO055::get_mag(void){ 00182 tx[0] = BNO055_MAG_DATA_X_LSB_ADDR; 00183 _i2c.write(address,tx,1,true); 00184 _i2c.read(address+1,rawdata,6,0); 00185 mag.rawx = (rawdata[1] << 8 | rawdata[0]); 00186 mag.rawy = (rawdata[3] << 8 | rawdata[2]); 00187 mag.rawz = (rawdata[5] << 8 | rawdata[4]); 00188 mag.x = float(mag.rawx); 00189 mag.y = float(mag.rawy); 00190 mag.z = float(mag.rawz); 00191 } 00192 00193 void BNO055::get_lia(void){ 00194 tx[0] = BNO055_LINEAR_ACCEL_DATA_X_LSB_ADDR; 00195 _i2c.write(address,tx,1,true); 00196 _i2c.read(address+1,rawdata,6,0); 00197 lia.rawx = (rawdata[1] << 8 | rawdata[0]); 00198 lia.rawy = (rawdata[3] << 8 | rawdata[2]); 00199 lia.rawz = (rawdata[5] << 8 | rawdata[4]); 00200 lia.x = float(lia.rawx)*accel_scale; 00201 lia.y = float(lia.rawy)*accel_scale; 00202 lia.z = float(lia.rawz)*accel_scale; 00203 } 00204 00205 void BNO055::get_grv(void){ 00206 tx[0] = BNO055_GRAVITY_DATA_X_LSB_ADDR; 00207 _i2c.write(address,tx,1,true); 00208 _i2c.read(address+1,rawdata,6,0); 00209 gravity.rawx = (rawdata[1] << 8 | rawdata[0]); 00210 gravity.rawy = (rawdata[3] << 8 | rawdata[2]); 00211 gravity.rawz = (rawdata[5] << 8 | rawdata[4]); 00212 gravity.x = float(gravity.rawx)*accel_scale; 00213 gravity.y = float(gravity.rawy)*accel_scale; 00214 gravity.z = float(gravity.rawz)*accel_scale; 00215 } 00216 00217 void BNO055::get_quat(void){ 00218 tx[0] = BNO055_QUATERNION_DATA_W_LSB_ADDR; 00219 _i2c.write(address,tx,1,true); 00220 _i2c.read(address+1,rawdata,8,0); 00221 quat.raww = (rawdata[1] << 8 | rawdata[0]); 00222 quat.rawx = (rawdata[3] << 8 | rawdata[2]); 00223 quat.rawy = (rawdata[5] << 8 | rawdata[4]); 00224 quat.rawz = (rawdata[7] << 8 | rawdata[6]); 00225 quat.w = float(quat.raww)/16384.0f; 00226 quat.x = float(quat.rawx)/16384.0f; 00227 quat.y = float(quat.rawy)/16384.0f; 00228 quat.z = float(quat.rawz)/16384.0f; 00229 } 00230 00231 void BNO055::get_angles(void){ 00232 tx[0] = BNO055_EULER_H_LSB_ADDR; 00233 _i2c.write(address,tx,1,true); 00234 _i2c.read(address+1,rawdata,6,0); 00235 euler.rawyaw = (rawdata[1] << 8 | rawdata[0]); 00236 euler.rawroll = (rawdata[3] << 8 | rawdata[2]); 00237 euler.rawpitch = (rawdata[5] << 8 | rawdata[4]); 00238 euler.yaw = float(euler.rawyaw)*angle_scale; 00239 euler.roll = float(euler.rawroll)*angle_scale; 00240 euler.pitch = float(euler.rawpitch)*angle_scale; 00241 } 00242 00243 00244 void BNO055::get_temp(void){ 00245 readchar(BNO055_TEMP_ADDR); 00246 temperature = rx / temp_scale; 00247 } 00248 00249 void BNO055::get_calib(void){ 00250 readchar(BNO055_CALIB_STAT_ADDR); 00251 calib = rx; 00252 } 00253 00254 void BNO055::read_calibration_data(void){ 00255 char tempmode = op_mode; 00256 setmode(OPERATION_MODE_CONFIG); 00257 wait_ms(20); 00258 tx[0] = ACCEL_OFFSET_X_LSB_ADDR; 00259 _i2c.write(address,tx,1,true); 00260 _i2c.read(address,calibration,22,false); 00261 setmode(tempmode); 00262 wait_ms(10); 00263 } 00264 00265 void BNO055::write_calibration_data(void){ 00266 char tempmode = op_mode; 00267 setmode(OPERATION_MODE_CONFIG); 00268 wait_ms(20); 00269 tx[0] = ACCEL_OFFSET_X_LSB_ADDR; 00270 _i2c.write(address,tx,1,true); 00271 _i2c.write(address,calibration,22,false); 00272 setmode(tempmode); 00273 wait_ms(10); 00274 } 00275 00276 void BNO055::set_mapping(char orient){ 00277 switch (orient){ 00278 case 0: 00279 writechar(BNO055_AXIS_MAP_CONFIG_ADDR,0x21); 00280 writechar(BNO055_AXIS_MAP_SIGN_ADDR,0x04); 00281 break; 00282 case 1: 00283 writechar(BNO055_AXIS_MAP_CONFIG_ADDR,0x24); 00284 writechar(BNO055_AXIS_MAP_SIGN_ADDR,0x00); 00285 break; 00286 case 2: 00287 writechar(BNO055_AXIS_MAP_CONFIG_ADDR,0x24); 00288 writechar(BNO055_AXIS_MAP_SIGN_ADDR,0x00); 00289 break; 00290 case 3: 00291 writechar(BNO055_AXIS_MAP_CONFIG_ADDR,0x21); 00292 writechar(BNO055_AXIS_MAP_SIGN_ADDR,0x02); 00293 break; 00294 case 4: 00295 writechar(BNO055_AXIS_MAP_CONFIG_ADDR,0x24); 00296 writechar(BNO055_AXIS_MAP_SIGN_ADDR,0x03); 00297 break; 00298 case 5: 00299 writechar(BNO055_AXIS_MAP_CONFIG_ADDR,0x21); 00300 writechar(BNO055_AXIS_MAP_SIGN_ADDR,0x01); 00301 break; 00302 case 6: 00303 writechar(BNO055_AXIS_MAP_CONFIG_ADDR,0x21); 00304 writechar(BNO055_AXIS_MAP_SIGN_ADDR,0x07); 00305 break; 00306 case 7: 00307 writechar(BNO055_AXIS_MAP_CONFIG_ADDR,0x24); 00308 writechar(BNO055_AXIS_MAP_SIGN_ADDR,0x05); 00309 break; 00310 default: 00311 writechar(BNO055_AXIS_MAP_CONFIG_ADDR,0x24); 00312 writechar(BNO055_AXIS_MAP_SIGN_ADDR,0x00); 00313 } 00314 }
Generated on Tue Jul 26 2022 22:26:49 by
1.7.2