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