Sog Yang
/
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 rate_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(char units){ 00107 readchar(BNO055_UNIT_SEL_ADDR); 00108 if (units == WINDOWS) rx = rx &0x7F; 00109 else rx = rx | units; 00110 writechar(BNO055_UNIT_SEL_ADDR,rx); 00111 } 00112 00113 void BNO055::setmode(char omode){ 00114 writechar(BNO055_OPR_MODE_ADDR,omode); 00115 op_mode = omode; 00116 } 00117 00118 void BNO055::setpowermode(char pmode){ 00119 writechar(BNO055_PWR_MODE_ADDR,pmode); 00120 pwr_mode = pmode; 00121 } 00122 00123 void BNO055::get_accel(void){ 00124 tx[0] = BNO055_ACCEL_DATA_X_LSB_ADDR; 00125 _i2c.write(address,tx,1,true); 00126 _i2c.read(address+1,rawdata,6,0); 00127 accel.rawx = (rawdata[1] << 8 | rawdata[0]); 00128 accel.rawy = (rawdata[3] << 8 | rawdata[2]); 00129 accel.rawz = (rawdata[5] << 8 | rawdata[4]); 00130 accel.x = float(accel.rawx)*accel_scale; 00131 accel.y = float(accel.rawy)*accel_scale; 00132 accel.z = float(accel.rawz)*accel_scale; 00133 } 00134 00135 void BNO055::get_gyro(void){ 00136 tx[0] = BNO055_GYRO_DATA_X_LSB_ADDR; 00137 _i2c.write(address,tx,1,true); 00138 _i2c.read(address+1,rawdata,6,0); 00139 gyro.rawx = (rawdata[1] << 8 | rawdata[0]); 00140 gyro.rawy = (rawdata[3] << 8 | rawdata[2]); 00141 gyro.rawz = (rawdata[5] << 8 | rawdata[4]); 00142 gyro.x = float(gyro.rawx)*rate_scale; 00143 gyro.y = float(gyro.rawy)*rate_scale; 00144 gyro.z = float(gyro.rawz)*rate_scale; 00145 } 00146 00147 void BNO055::get_mag(void){ 00148 tx[0] = BNO055_MAG_DATA_X_LSB_ADDR; 00149 _i2c.write(address,tx,1,true); 00150 _i2c.read(address+1,rawdata,6,0); 00151 mag.rawx = (rawdata[1] << 8 | rawdata[0]); 00152 mag.rawy = (rawdata[3] << 8 | rawdata[2]); 00153 mag.rawz = (rawdata[5] << 8 | rawdata[4]); 00154 mag.x = float(mag.rawx); 00155 mag.y = float(mag.rawy); 00156 mag.z = float(mag.rawz); 00157 } 00158 00159 void BNO055::get_lia(void){ 00160 tx[0] = BNO055_LINEAR_ACCEL_DATA_X_LSB_ADDR; 00161 _i2c.write(address,tx,1,true); 00162 _i2c.read(address+1,rawdata,6,0); 00163 lia.rawx = (rawdata[1] << 8 | rawdata[0]); 00164 lia.rawy = (rawdata[3] << 8 | rawdata[2]); 00165 lia.rawz = (rawdata[5] << 8 | rawdata[4]); 00166 lia.x = float(lia.rawx)*accel_scale; 00167 lia.y = float(lia.rawy)*accel_scale; 00168 lia.z = float(lia.rawz)*accel_scale; 00169 } 00170 00171 void BNO055::get_grv(void){ 00172 tx[0] = BNO055_GRAVITY_DATA_X_LSB_ADDR; 00173 _i2c.write(address,tx,1,true); 00174 _i2c.read(address+1,rawdata,6,0); 00175 gravity.rawx = (rawdata[1] << 8 | rawdata[0]); 00176 gravity.rawy = (rawdata[3] << 8 | rawdata[2]); 00177 gravity.rawz = (rawdata[5] << 8 | rawdata[4]); 00178 gravity.x = float(gravity.rawx)*accel_scale; 00179 gravity.y = float(gravity.rawy)*accel_scale; 00180 gravity.z = float(gravity.rawz)*accel_scale; 00181 } 00182 00183 void BNO055::get_quat(void){ 00184 tx[0] = BNO055_QUATERNION_DATA_W_LSB_ADDR; 00185 _i2c.write(address,tx,1,true); 00186 _i2c.read(address+1,rawdata,8,0); 00187 quat.raww = (rawdata[1] << 8 | rawdata[0]); 00188 quat.rawx = (rawdata[3] << 8 | rawdata[2]); 00189 quat.rawy = (rawdata[5] << 8 | rawdata[4]); 00190 quat.rawz = (rawdata[7] << 8 | rawdata[6]); 00191 quat.w = float(quat.raww)/16384.0f; 00192 quat.x = float(quat.rawx)/16384.0f; 00193 quat.y = float(quat.rawy)/16384.0f; 00194 quat.z = float(quat.rawz)/16384.0f; 00195 } 00196 00197 void BNO055::get_angles(void){ 00198 tx[0] = BNO055_EULER_H_LSB_ADDR; 00199 _i2c.write(address,tx,1,true); 00200 _i2c.read(address+1,rawdata,6,0); 00201 euler.rawyaw = (rawdata[1] << 8 | rawdata[0]); 00202 euler.rawroll = (rawdata[3] << 8 | rawdata[2]); 00203 euler.rawpitch = (rawdata[5] << 8 | rawdata[4]); 00204 euler.yaw = float(euler.rawyaw)*angle_scale; 00205 euler.roll = float(euler.rawroll)*angle_scale; 00206 euler.pitch = float(euler.rawpitch)*angle_scale; 00207 } 00208 00209 00210 void BNO055::get_temp(void){ 00211 readchar(BNO055_TEMP_ADDR); 00212 temperature = rx / temp_scale; 00213 } 00214 00215 void BNO055::get_calib(void){ 00216 readchar(BNO055_CALIB_STAT_ADDR); 00217 calib = rx; 00218 } 00219 00220 void BNO055::read_calibration_data(void){ 00221 char tempmode = op_mode; 00222 setmode(OPERATION_MODE_CONFIG); 00223 wait_ms(20); 00224 tx[0] = ACCEL_OFFSET_X_LSB_ADDR; 00225 _i2c.write(address,tx,1,true); 00226 _i2c.read(address,calibration,22,false); 00227 setmode(tempmode); 00228 wait_ms(10); 00229 } 00230 00231 void BNO055::write_calibration_data(void){ 00232 char tempmode = op_mode; 00233 setmode(OPERATION_MODE_CONFIG); 00234 wait_ms(20); 00235 tx[0] = ACCEL_OFFSET_X_LSB_ADDR; 00236 _i2c.write(address,tx,1,true); 00237 _i2c.write(address,calibration,22,false); 00238 setmode(tempmode); 00239 wait_ms(10); 00240 } 00241 00242 void BNO055::set_mapping(char orient){ 00243 switch (orient){ 00244 case 0: 00245 writechar(BNO055_AXIS_MAP_CONFIG_ADDR,0x21); 00246 writechar(BNO055_AXIS_MAP_SIGN_ADDR,0x04); 00247 break; 00248 case 1: 00249 writechar(BNO055_AXIS_MAP_CONFIG_ADDR,0x24); 00250 writechar(BNO055_AXIS_MAP_SIGN_ADDR,0x00); 00251 break; 00252 case 2: 00253 writechar(BNO055_AXIS_MAP_CONFIG_ADDR,0x24); 00254 writechar(BNO055_AXIS_MAP_SIGN_ADDR,0x00); 00255 break; 00256 case 3: 00257 writechar(BNO055_AXIS_MAP_CONFIG_ADDR,0x21); 00258 writechar(BNO055_AXIS_MAP_SIGN_ADDR,0x02); 00259 break; 00260 case 4: 00261 writechar(BNO055_AXIS_MAP_CONFIG_ADDR,0x24); 00262 writechar(BNO055_AXIS_MAP_SIGN_ADDR,0x03); 00263 break; 00264 case 5: 00265 writechar(BNO055_AXIS_MAP_CONFIG_ADDR,0x21); 00266 writechar(BNO055_AXIS_MAP_SIGN_ADDR,0x01); 00267 break; 00268 case 6: 00269 writechar(BNO055_AXIS_MAP_CONFIG_ADDR,0x21); 00270 writechar(BNO055_AXIS_MAP_SIGN_ADDR,0x07); 00271 break; 00272 case 7: 00273 writechar(BNO055_AXIS_MAP_CONFIG_ADDR,0x24); 00274 writechar(BNO055_AXIS_MAP_SIGN_ADDR,0x05); 00275 break; 00276 default: 00277 writechar(BNO055_AXIS_MAP_CONFIG_ADDR,0x24); 00278 writechar(BNO055_AXIS_MAP_SIGN_ADDR,0x00); 00279 } 00280 }
Generated on Fri Jul 15 2022 07:19:30 by
1.7.2