Adafruit driver converted to Mbed OS 6.x.
Dependents: Adafruit-BNO055-test
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, ®_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, ®_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 }
Generated on Wed Jul 13 2022 00:45:44 by 1.7.2