Jeremy Dawkins
/
ES456_Labs
Initial Commit
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 Wed Jul 20 2022 08:19:56 by 1.7.2