Adafruit driver converted to Mbed OS 6.x.

Dependents:   Adafruit-BNO055-test

Embed: (wiki syntax)

« Back to documentation index

Show/hide line numbers Adafruit_BNO055.cpp Source File

Adafruit_BNO055.cpp

00001 /***************************************************************************
00002   This is a library for the BNO055 orientation sensor
00003 
00004   Designed specifically to work with the Adafruit BNO055 Breakout.
00005 
00006   Pick one up today in the adafruit shop!
00007   ------> http://www.adafruit.com/products
00008 
00009   These sensors use I2C to communicate, 2 pins are required to interface.
00010 
00011   Adafruit invests time and resources providing this open source code,
00012   please support Adafruit andopen-source hardware by purchasing products
00013   from Adafruit!
00014 
00015   Written by KTOWN for Adafruit Industries.
00016 
00017   MIT license, all text above must be included in any redistribution
00018  ***************************************************************************/
00019 
00020 #include <math.h>
00021 #include <limits.h>
00022 #include "mbed.h"
00023 
00024 #include "Adafruit_BNO055.h"
00025 
00026 /***************************************************************************
00027  CONSTRUCTOR
00028  ***************************************************************************/
00029 
00030 /**************************************************************************/
00031 /*!
00032     @brief  Instantiates a new Adafruit_BNO055 class
00033 */
00034 /**************************************************************************/
00035 Adafruit_BNO055::Adafruit_BNO055(int32_t sensorID, uint8_t address, I2C* i2c_ptr)
00036 {
00037   _sensorID = sensorID;
00038   _address = address;
00039   i2c = i2c_ptr;
00040 }
00041 
00042 /***************************************************************************
00043  PUBLIC FUNCTIONS
00044  ***************************************************************************/
00045 
00046 /**************************************************************************/
00047 /*!
00048     @brief  Sets up the HW
00049 */
00050 /**************************************************************************/
00051 bool Adafruit_BNO055::begin(adafruit_bno055_opmode_t mode)
00052 {
00053   /* Enable I2C */
00054   //i2c->frequency(10000);
00055 
00056   /* Make sure we have the right device */
00057   uint8_t id = read8(BNO055_CHIP_ID_ADDR);
00058   if(id != BNO055_ID)
00059   {
00060     thread_sleep_for(1000); // hold on for boot
00061     id = read8(BNO055_CHIP_ID_ADDR);
00062     if(id != BNO055_ID) {
00063       return false;  // still not? ok bail
00064     }
00065   }
00066 
00067   /* Switch to config mode (just in case since this is the default) */
00068   setMode(OPERATION_MODE_CONFIG);
00069 
00070   /* Reset */
00071   write8(BNO055_SYS_TRIGGER_ADDR, 0x20);
00072   while (read8(BNO055_CHIP_ID_ADDR) != BNO055_ID)
00073   {
00074     thread_sleep_for(10);
00075   }
00076   thread_sleep_for(50);
00077 
00078   /* Set to normal power mode */
00079   write8(BNO055_PWR_MODE_ADDR, POWER_MODE_NORMAL);
00080   thread_sleep_for(10);
00081 
00082   write8(BNO055_PAGE_ID_ADDR, 0);
00083 
00084   /* Set the output units */
00085   /*
00086   uint8_t unitsel = (0 << 7) | // Orientation = Android
00087                     (0 << 4) | // Temperature = Celsius
00088                     (0 << 2) | // Euler = Degrees
00089                     (1 << 1) | // Gyro = Rads
00090                     (0 << 0);  // Accelerometer = m/s^2
00091   write8(BNO055_UNIT_SEL_ADDR, unitsel);
00092   */
00093 
00094   write8(BNO055_SYS_TRIGGER_ADDR, 0x0);
00095   thread_sleep_for(10);
00096   /* Set the requested operating mode (see section 3.3) */
00097   setMode(mode);
00098   thread_sleep_for(20);
00099 
00100   return true;
00101 }
00102 
00103 /**************************************************************************/
00104 /*!
00105     @brief  Puts the chip in the specified operating mode
00106 */
00107 /**************************************************************************/
00108 void Adafruit_BNO055::setMode(adafruit_bno055_opmode_t mode)
00109 {
00110   _mode = mode;
00111   write8(BNO055_OPR_MODE_ADDR, _mode);
00112   thread_sleep_for(30);
00113 }
00114 
00115 /**************************************************************************/
00116 /*!
00117     @brief  Use the external 32.768KHz crystal
00118 */
00119 /**************************************************************************/
00120 void Adafruit_BNO055::setExtCrystalUse(bool usextal)
00121 {
00122   adafruit_bno055_opmode_t modeback = _mode;
00123 
00124   /* Switch to config mode (just in case since this is the default) */
00125   setMode(OPERATION_MODE_CONFIG);
00126   thread_sleep_for(25);
00127   write8(BNO055_PAGE_ID_ADDR, 0);
00128   if (usextal) {
00129     write8(BNO055_SYS_TRIGGER_ADDR, 0x80);
00130   } else {
00131     write8(BNO055_SYS_TRIGGER_ADDR, 0x00);
00132   }
00133   thread_sleep_for(10);
00134   /* Set the requested operating mode (see section 3.3) */
00135   setMode(modeback);
00136   thread_sleep_for(20);
00137 }
00138 
00139 
00140 /**************************************************************************/
00141 /*!
00142     @brief  Gets the latest system status info
00143 */
00144 /**************************************************************************/
00145 void Adafruit_BNO055::getSystemStatus(uint8_t *system_status, uint8_t *self_test_result, uint8_t *system_error)
00146 {
00147   write8(BNO055_PAGE_ID_ADDR, 0);
00148 
00149   /* System Status (see section 4.3.58)
00150      ---------------------------------
00151      0 = Idle
00152      1 = System Error
00153      2 = Initializing Peripherals
00154      3 = System Iniitalization
00155      4 = Executing Self-Test
00156      5 = Sensor fusio algorithm running
00157      6 = System running without fusion algorithms */
00158 
00159   if (system_status != 0)
00160     *system_status    = read8(BNO055_SYS_STAT_ADDR);
00161 
00162   /* Self Test Results (see section )
00163      --------------------------------
00164      1 = test passed, 0 = test failed
00165 
00166      Bit 0 = Accelerometer self test
00167      Bit 1 = Magnetometer self test
00168      Bit 2 = Gyroscope self test
00169      Bit 3 = MCU self test
00170 
00171      0x0F = all good! */
00172 
00173   if (self_test_result != 0)
00174     *self_test_result = read8(BNO055_SELFTEST_RESULT_ADDR);
00175 
00176   /* System Error (see section 4.3.59)
00177      ---------------------------------
00178      0 = No error
00179      1 = Peripheral initialization error
00180      2 = System initialization error
00181      3 = Self test result failed
00182      4 = Register map value out of range
00183      5 = Register map address out of range
00184      6 = Register map write error
00185      7 = BNO low power mode not available for selected operat ion mode
00186      8 = Accelerometer power mode not available
00187      9 = Fusion algorithm configuration error
00188      A = Sensor configuration error */
00189 
00190   if (system_error != 0)
00191     *system_error     = read8(BNO055_SYS_ERR_ADDR);
00192 
00193   thread_sleep_for(200);
00194 }
00195 
00196 /**************************************************************************/
00197 /*!
00198     @brief  Gets the chip revision numbers
00199 */
00200 /**************************************************************************/
00201 void Adafruit_BNO055::getRevInfo(adafruit_bno055_rev_info_t* info)
00202 {
00203   uint8_t a, b;
00204 
00205   memset(info, 0, sizeof(adafruit_bno055_rev_info_t));
00206 
00207   /* Check the accelerometer revision */
00208   info->accel_rev = read8(BNO055_ACCEL_REV_ID_ADDR);
00209 
00210   /* Check the magnetometer revision */
00211   info->mag_rev   = read8(BNO055_MAG_REV_ID_ADDR);
00212 
00213   /* Check the gyroscope revision */
00214   info->gyro_rev  = read8(BNO055_GYRO_REV_ID_ADDR);
00215 
00216   /* Check the SW revision */
00217   info->bl_rev    = read8(BNO055_BL_REV_ID_ADDR);
00218 
00219   a = read8(BNO055_SW_REV_ID_LSB_ADDR);
00220   b = read8(BNO055_SW_REV_ID_MSB_ADDR);
00221   info->sw_rev = (((uint16_t)b) << 8) | ((uint16_t)a);
00222 }
00223 
00224 /**************************************************************************/
00225 /*!
00226     @brief  Gets current calibration state.  Each value should be a uint8_t
00227             pointer and it will be set to 0 if not calibrated and 3 if
00228             fully calibrated.
00229 */
00230 /**************************************************************************/
00231 void Adafruit_BNO055::getCalibration(uint8_t* sys, uint8_t* gyro, uint8_t* accel, uint8_t* mag) {
00232   uint8_t calData = read8(BNO055_CALIB_STAT_ADDR);
00233   if (sys != NULL) {
00234     *sys = (calData >> 6) & 0x03;
00235   }
00236   if (gyro != NULL) {
00237     *gyro = (calData >> 4) & 0x03;
00238   }
00239   if (accel != NULL) {
00240     *accel = (calData >> 2) & 0x03;
00241   }
00242   if (mag != NULL) {
00243     *mag = calData & 0x03;
00244   }
00245 }
00246 
00247 /**************************************************************************/
00248 /*!
00249     @brief  Gets the temperature in degrees celsius
00250 */
00251 /**************************************************************************/
00252 int8_t Adafruit_BNO055::getTemp(void)
00253 {
00254   int8_t temp = (int8_t)(read8(BNO055_TEMP_ADDR));
00255   return temp;
00256 }
00257 
00258 /**************************************************************************/
00259 /*!
00260     @brief  Gets a vector reading from the specified source
00261 */
00262 /**************************************************************************/
00263 imu::Vector<3> Adafruit_BNO055::getVector(adafruit_vector_type_t vector_type)
00264 {
00265   imu::Vector<3> xyz;
00266   unsigned char buffer[6];
00267   memset (buffer, 0, 6);
00268 
00269   int16_t x, y, z;
00270   x = y = z = 0;
00271 
00272   /* Read vector data (6 bytes) */
00273   //readLen((adafruit_bno055_reg_t)vector_type, (char*)buffer, 6);
00274   readLen((adafruit_bno055_reg_t)0x08, (char*)buffer, 6);
00275 
00276   x = ((int16_t)buffer[0]) | (((int16_t)buffer[1]) << 8);
00277   y = ((int16_t)buffer[2]) | (((int16_t)buffer[3]) << 8);
00278   z = ((int16_t)buffer[4]) | (((int16_t)buffer[5]) << 8);
00279 
00280   /* Convert the value to an appropriate range (section 3.6.4) */
00281   /* and assign the value to the Vector type */
00282   switch(vector_type)
00283   {
00284     case VECTOR_MAGNETOMETER:
00285       /* 1uT = 16 LSB */
00286       xyz[0] = ((double)x)/16.0;
00287       xyz[1] = ((double)y)/16.0;
00288       xyz[2] = ((double)z)/16.0;
00289       break;
00290     case VECTOR_GYROSCOPE:
00291       /* 1rps = 900 LSB */
00292       xyz[0] = ((double)x)/900.0;
00293       xyz[1] = ((double)y)/900.0;
00294       xyz[2] = ((double)z)/900.0;
00295       break;
00296     case VECTOR_EULER:
00297       /* 1 degree = 16 LSB */
00298       xyz[0] = ((double)x)/16.0;
00299       xyz[1] = ((double)y)/16.0;
00300       xyz[2] = ((double)z)/16.0;
00301       break;
00302     case VECTOR_ACCELEROMETER:
00303     case VECTOR_LINEARACCEL:
00304     case VECTOR_GRAVITY:
00305       /* 1m/s^2 = 100 LSB */
00306       xyz[0] = ((double)x)/100.0;
00307       xyz[1] = ((double)y)/100.0;
00308       xyz[2] = ((double)z)/100.0;
00309       break;
00310   }
00311 
00312   return xyz;
00313 }
00314 
00315 /**************************************************************************/
00316 /*!
00317     @brief  Gets a quaternion reading from the specified source
00318 */
00319 /**************************************************************************/
00320 imu::Quaternion Adafruit_BNO055::getQuat(void)
00321 {
00322   unsigned char buffer[8];
00323   memset (buffer, 0, 8);
00324 
00325   int x, y, z, w;
00326   x = y = z = w = 0;
00327 
00328   /* Read quat data (8 bytes) */
00329   readLen(BNO055_QUATERNION_DATA_W_LSB_ADDR, (char*)buffer, 8);
00330   w = (((uint16_t)buffer[1]) << 8) | ((uint16_t)buffer[0]);
00331   x = (((uint16_t)buffer[3]) << 8) | ((uint16_t)buffer[2]);
00332   y = (((uint16_t)buffer[5]) << 8) | ((uint16_t)buffer[4]);
00333   z = (((uint16_t)buffer[7]) << 8) | ((uint16_t)buffer[6]);
00334 
00335   /* Assign to Quaternion */
00336   /* See http://ae-bst.resource.bosch.com/media/products/dokumente/bno055/BST_BNO055_DS000_12~1.pdf
00337      3.6.5.5 Orientation (Quaternion)  */
00338   const double scale = (1.0 / (1<<14));
00339   imu::Quaternion quat(scale * w, scale * x, scale * y, scale * z);
00340   return quat;
00341 }
00342 
00343 /**************************************************************************/
00344 /*!
00345     @brief  Provides the sensor_t data for this sensor
00346 */
00347 /**************************************************************************/
00348 void Adafruit_BNO055::getSensor(sensor_t *sensor)
00349 {
00350   /* Clear the sensor_t object */
00351   memset(sensor, 0, sizeof(sensor_t));
00352 
00353   /* Insert the sensor name in the fixed length char array */
00354   strncpy (sensor->name, "BNO055", sizeof(sensor->name) - 1);
00355   sensor->name[sizeof(sensor->name)- 1] = 0;
00356   sensor->version     = 1;
00357   sensor->sensor_id   = _sensorID;
00358   sensor->type        = SENSOR_TYPE_ORIENTATION;
00359   sensor->min_delay   = 0;
00360   sensor->max_value   = 0.0F;
00361   sensor->min_value   = 0.0F;
00362   sensor->resolution  = 0.01F;
00363 }
00364 
00365 /**************************************************************************/
00366 /*!
00367     @brief  Reads the sensor and returns the data as a sensors_event_t
00368 */
00369 /**************************************************************************/
00370 bool Adafruit_BNO055::getEvent(sensors_event_t *event)
00371 {
00372   /* Clear the event */
00373   memset(event, 0, sizeof(sensors_event_t));
00374 
00375   event->version   = sizeof(sensors_event_t);
00376   event->sensor_id = _sensorID;
00377   event->type      = SENSOR_TYPE_ORIENTATION;
00378   event->timestamp = 0; //TODO: fix this with a millis() call
00379 
00380   /* Get a Euler angle sample for orientation */
00381   imu::Vector<3> euler = getVector(Adafruit_BNO055::VECTOR_EULER);
00382   event->orientation.x = euler.x();
00383   event->orientation.y = euler.y();
00384   event->orientation.z = euler.z();
00385 
00386   return true;
00387 }
00388 
00389 /***************************************************************************
00390  PRIVATE FUNCTIONS
00391  ***************************************************************************/
00392 
00393 /**************************************************************************/
00394 /*!
00395     @brief  Writes an 8 bit value over I2C
00396 */
00397 /**************************************************************************/
00398 bool Adafruit_BNO055::write8(adafruit_bno055_reg_t reg, char value)
00399 {
00400   char reg_to_write = (char)(reg);
00401   i2c->write(_address<<1, &reg_to_write, 1, true); 
00402   thread_sleep_for(1);
00403   i2c->write(_address<<1, &value, 1, false); 
00404   thread_sleep_for(1);
00405 
00406   /* ToDo: Check for error! */
00407   return true;
00408 }
00409 
00410 /**************************************************************************/
00411 /*!
00412     @brief  Reads an 8 bit value over I2C
00413 */
00414 /**************************************************************************/
00415 char Adafruit_BNO055::read8(adafruit_bno055_reg_t reg )
00416 {
00417   char to_read = 0;
00418   char to_write = (char)reg;
00419 
00420   i2c->write(_address<<1, &to_write, 1, false);
00421   thread_sleep_for(1);
00422   i2c->read(_address<<1, &to_read, 1, false);
00423   thread_sleep_for(1);
00424   
00425   printf(" I2C Read : %d from addr: %d\r\n", to_read, to_write);
00426   return to_read;
00427 }
00428 
00429 /**************************************************************************/
00430 /*!
00431     @brief  Reads the specified number of bytes over I2C
00432 */
00433 /**************************************************************************/
00434 bool Adafruit_BNO055::readLen(adafruit_bno055_reg_t reg, char* buffer, int len)
00435 {
00436   char reg_to_write = (char)(reg);
00437 
00438   i2c->write(_address<<1, &reg_to_write, 1, false); 
00439   thread_sleep_for(1);
00440   i2c->read(_address<<1, buffer, len, false);
00441   thread_sleep_for(1);
00442 
00443   printf("I2C: Read %d bytes from address %d\r\n", len, reg_to_write);
00444 
00445   /* ToDo: Check for errors! */
00446   return true;
00447 }