I2C library for Bosch BNO055 sensor
Dependents: BNO055_HelloWorld robfish_test_IMU_and_hallsensor SCRIBE_stepper SCRIBE_servo ... more
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 Tue Jul 12 2022 17:50:09 by 1.7.2