BNO055.cpp

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