BNO055.cpp

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