Ben, Simon, Inez IDD / Adafruit_BNO055

Adafruit driver converted to mbed.

Dependents:   BNO055_Adafruit SpriteIMU

Adafruit_BNO055.cpp@2:8092160b6a59, 2015-09-16 (annotated)

Committer:
simonscott
Date:
Wed Sep 16 22:32:29 2015 +0000
Revision:
2:8092160b6a59
Parent:
1:b48e4192c101 
Changed I2C pins

Who changed what in which revision?

UserRevisionLine numberNew contents of line
simonscott 0:22c544c8741a 1 /***************************************************************************
simonscott 0:22c544c8741a 2 This is a library for the BNO055 orientation sensor
simonscott 0:22c544c8741a 3
simonscott 0:22c544c8741a 4 Designed specifically to work with the Adafruit BNO055 Breakout.
simonscott 0:22c544c8741a 5
simonscott 0:22c544c8741a 6 Pick one up today in the adafruit shop!
simonscott 0:22c544c8741a 7 ------> http://www.adafruit.com/products
simonscott 0:22c544c8741a 8
simonscott 0:22c544c8741a 9 These sensors use I2C to communicate, 2 pins are required to interface.
simonscott 0:22c544c8741a 10
simonscott 0:22c544c8741a 11 Adafruit invests time and resources providing this open source code,
simonscott 0:22c544c8741a 12 please support Adafruit andopen-source hardware by purchasing products
simonscott 0:22c544c8741a 13 from Adafruit!
simonscott 0:22c544c8741a 14
simonscott 0:22c544c8741a 15 Written by KTOWN for Adafruit Industries.
simonscott 0:22c544c8741a 16
simonscott 0:22c544c8741a 17 MIT license, all text above must be included in any redistribution
simonscott 0:22c544c8741a 18 ***************************************************************************/
simonscott 0:22c544c8741a 19
simonscott 0:22c544c8741a 20 #include <math.h>
simonscott 0:22c544c8741a 21 #include <limits.h>
simonscott 0:22c544c8741a 22 #include "mbed.h"
simonscott 0:22c544c8741a 23
simonscott 0:22c544c8741a 24 #include "Adafruit_BNO055.h"
simonscott 0:22c544c8741a 25
simonscott 0:22c544c8741a 26 /***************************************************************************
simonscott 0:22c544c8741a 27 CONSTRUCTOR
simonscott 0:22c544c8741a 28 ***************************************************************************/
simonscott 0:22c544c8741a 29
simonscott 0:22c544c8741a 30 /**************************************************************************/
simonscott 0:22c544c8741a 31 /*!
simonscott 0:22c544c8741a 32 @brief Instantiates a new Adafruit_BNO055 class
simonscott 0:22c544c8741a 33 */
simonscott 0:22c544c8741a 34 /**************************************************************************/
simonscott 0:22c544c8741a 35 Adafruit_BNO055::Adafruit_BNO055(int32_t sensorID, uint8_t address, I2C* i2c_ptr)
simonscott 0:22c544c8741a 36 {
simonscott 0:22c544c8741a 37 _sensorID = sensorID;
simonscott 0:22c544c8741a 38 _address = address;
simonscott 0:22c544c8741a 39 i2c = i2c_ptr;
simonscott 0:22c544c8741a 40 }
simonscott 0:22c544c8741a 41
simonscott 0:22c544c8741a 42 /***************************************************************************
simonscott 0:22c544c8741a 43 PUBLIC FUNCTIONS
simonscott 0:22c544c8741a 44 ***************************************************************************/
simonscott 0:22c544c8741a 45
simonscott 0:22c544c8741a 46 /**************************************************************************/
simonscott 0:22c544c8741a 47 /*!
simonscott 0:22c544c8741a 48 @brief Sets up the HW
simonscott 0:22c544c8741a 49 */
simonscott 0:22c544c8741a 50 /**************************************************************************/
simonscott 0:22c544c8741a 51 bool Adafruit_BNO055::begin(adafruit_bno055_opmode_t mode)
simonscott 0:22c544c8741a 52 {
simonscott 0:22c544c8741a 53 /* Enable I2C */
simonscott 2:8092160b6a59 54 //i2c->frequency(10000);
simonscott 0:22c544c8741a 55
simonscott 0:22c544c8741a 56 /* Make sure we have the right device */
simonscott 0:22c544c8741a 57 uint8_t id = read8(BNO055_CHIP_ID_ADDR);
simonscott 0:22c544c8741a 58 if(id != BNO055_ID)
simonscott 0:22c544c8741a 59 {
simonscott 0:22c544c8741a 60 wait_ms(1000); // hold on for boot
simonscott 0:22c544c8741a 61 id = read8(BNO055_CHIP_ID_ADDR);
simonscott 0:22c544c8741a 62 if(id != BNO055_ID) {
simonscott 0:22c544c8741a 63 return false; // still not? ok bail
simonscott 0:22c544c8741a 64 }
simonscott 0:22c544c8741a 65 }
simonscott 0:22c544c8741a 66
simonscott 0:22c544c8741a 67 /* Switch to config mode (just in case since this is the default) */
simonscott 0:22c544c8741a 68 setMode(OPERATION_MODE_CONFIG);
simonscott 0:22c544c8741a 69
simonscott 0:22c544c8741a 70 /* Reset */
simonscott 0:22c544c8741a 71 write8(BNO055_SYS_TRIGGER_ADDR, 0x20);
simonscott 0:22c544c8741a 72 while (read8(BNO055_CHIP_ID_ADDR) != BNO055_ID)
simonscott 0:22c544c8741a 73 {
simonscott 0:22c544c8741a 74 wait_ms(10);
simonscott 0:22c544c8741a 75 }
simonscott 0:22c544c8741a 76 wait_ms(50);
simonscott 0:22c544c8741a 77
simonscott 0:22c544c8741a 78 /* Set to normal power mode */
simonscott 0:22c544c8741a 79 write8(BNO055_PWR_MODE_ADDR, POWER_MODE_NORMAL);
simonscott 0:22c544c8741a 80 wait_ms(10);
simonscott 0:22c544c8741a 81
simonscott 0:22c544c8741a 82 write8(BNO055_PAGE_ID_ADDR, 0);
simonscott 0:22c544c8741a 83
simonscott 0:22c544c8741a 84 /* Set the output units */
simonscott 0:22c544c8741a 85 /*
simonscott 0:22c544c8741a 86 uint8_t unitsel = (0 << 7) | // Orientation = Android
simonscott 0:22c544c8741a 87 (0 << 4) | // Temperature = Celsius
simonscott 0:22c544c8741a 88 (0 << 2) | // Euler = Degrees
simonscott 0:22c544c8741a 89 (1 << 1) | // Gyro = Rads
simonscott 0:22c544c8741a 90 (0 << 0); // Accelerometer = m/s^2
simonscott 0:22c544c8741a 91 write8(BNO055_UNIT_SEL_ADDR, unitsel);
simonscott 0:22c544c8741a 92 */
simonscott 0:22c544c8741a 93
simonscott 0:22c544c8741a 94 write8(BNO055_SYS_TRIGGER_ADDR, 0x0);
simonscott 0:22c544c8741a 95 wait_ms(10);
simonscott 0:22c544c8741a 96 /* Set the requested operating mode (see section 3.3) */
simonscott 0:22c544c8741a 97 setMode(mode);
simonscott 0:22c544c8741a 98 wait_ms(20);
simonscott 0:22c544c8741a 99
simonscott 0:22c544c8741a 100 return true;
simonscott 0:22c544c8741a 101 }
simonscott 0:22c544c8741a 102
simonscott 0:22c544c8741a 103 /**************************************************************************/
simonscott 0:22c544c8741a 104 /*!
simonscott 0:22c544c8741a 105 @brief Puts the chip in the specified operating mode
simonscott 0:22c544c8741a 106 */
simonscott 0:22c544c8741a 107 /**************************************************************************/
simonscott 0:22c544c8741a 108 void Adafruit_BNO055::setMode(adafruit_bno055_opmode_t mode)
simonscott 0:22c544c8741a 109 {
simonscott 0:22c544c8741a 110 _mode = mode;
simonscott 0:22c544c8741a 111 write8(BNO055_OPR_MODE_ADDR, _mode);
simonscott 0:22c544c8741a 112 wait_ms(30);
simonscott 0:22c544c8741a 113 }
simonscott 0:22c544c8741a 114
simonscott 0:22c544c8741a 115 /**************************************************************************/
simonscott 0:22c544c8741a 116 /*!
simonscott 0:22c544c8741a 117 @brief Use the external 32.768KHz crystal
simonscott 0:22c544c8741a 118 */
simonscott 0:22c544c8741a 119 /**************************************************************************/
simonscott 0:22c544c8741a 120 void Adafruit_BNO055::setExtCrystalUse(bool usextal)
simonscott 0:22c544c8741a 121 {
simonscott 0:22c544c8741a 122 adafruit_bno055_opmode_t modeback = _mode;
simonscott 0:22c544c8741a 123
simonscott 0:22c544c8741a 124 /* Switch to config mode (just in case since this is the default) */
simonscott 0:22c544c8741a 125 setMode(OPERATION_MODE_CONFIG);
simonscott 0:22c544c8741a 126 wait_ms(25);
simonscott 0:22c544c8741a 127 write8(BNO055_PAGE_ID_ADDR, 0);
simonscott 0:22c544c8741a 128 if (usextal) {
simonscott 0:22c544c8741a 129 write8(BNO055_SYS_TRIGGER_ADDR, 0x80);
simonscott 0:22c544c8741a 130 } else {
simonscott 0:22c544c8741a 131 write8(BNO055_SYS_TRIGGER_ADDR, 0x00);
simonscott 0:22c544c8741a 132 }
simonscott 0:22c544c8741a 133 wait_ms(10);
simonscott 0:22c544c8741a 134 /* Set the requested operating mode (see section 3.3) */
simonscott 0:22c544c8741a 135 setMode(modeback);
simonscott 0:22c544c8741a 136 wait_ms(20);
simonscott 0:22c544c8741a 137 }
simonscott 0:22c544c8741a 138
simonscott 0:22c544c8741a 139
simonscott 0:22c544c8741a 140 /**************************************************************************/
simonscott 0:22c544c8741a 141 /*!
simonscott 0:22c544c8741a 142 @brief Gets the latest system status info
simonscott 0:22c544c8741a 143 */
simonscott 0:22c544c8741a 144 /**************************************************************************/
simonscott 0:22c544c8741a 145 void Adafruit_BNO055::getSystemStatus(uint8_t *system_status, uint8_t *self_test_result, uint8_t *system_error)
simonscott 0:22c544c8741a 146 {
simonscott 0:22c544c8741a 147 write8(BNO055_PAGE_ID_ADDR, 0);
simonscott 0:22c544c8741a 148
simonscott 0:22c544c8741a 149 /* System Status (see section 4.3.58)
simonscott 0:22c544c8741a 150 ---------------------------------
simonscott 0:22c544c8741a 151 0 = Idle
simonscott 0:22c544c8741a 152 1 = System Error
simonscott 0:22c544c8741a 153 2 = Initializing Peripherals
simonscott 0:22c544c8741a 154 3 = System Iniitalization
simonscott 0:22c544c8741a 155 4 = Executing Self-Test
simonscott 0:22c544c8741a 156 5 = Sensor fusio algorithm running
simonscott 0:22c544c8741a 157 6 = System running without fusion algorithms */
simonscott 0:22c544c8741a 158
simonscott 0:22c544c8741a 159 if (system_status != 0)
simonscott 0:22c544c8741a 160 *system_status = read8(BNO055_SYS_STAT_ADDR);
simonscott 0:22c544c8741a 161
simonscott 0:22c544c8741a 162 /* Self Test Results (see section )
simonscott 0:22c544c8741a 163 --------------------------------
simonscott 0:22c544c8741a 164 1 = test passed, 0 = test failed
simonscott 0:22c544c8741a 165
simonscott 0:22c544c8741a 166 Bit 0 = Accelerometer self test
simonscott 0:22c544c8741a 167 Bit 1 = Magnetometer self test
simonscott 0:22c544c8741a 168 Bit 2 = Gyroscope self test
simonscott 0:22c544c8741a 169 Bit 3 = MCU self test
simonscott 0:22c544c8741a 170
simonscott 0:22c544c8741a 171 0x0F = all good! */
simonscott 0:22c544c8741a 172
simonscott 0:22c544c8741a 173 if (self_test_result != 0)
simonscott 0:22c544c8741a 174 *self_test_result = read8(BNO055_SELFTEST_RESULT_ADDR);
simonscott 0:22c544c8741a 175
simonscott 0:22c544c8741a 176 /* System Error (see section 4.3.59)
simonscott 0:22c544c8741a 177 ---------------------------------
simonscott 0:22c544c8741a 178 0 = No error
simonscott 0:22c544c8741a 179 1 = Peripheral initialization error
simonscott 0:22c544c8741a 180 2 = System initialization error
simonscott 0:22c544c8741a 181 3 = Self test result failed
simonscott 0:22c544c8741a 182 4 = Register map value out of range
simonscott 0:22c544c8741a 183 5 = Register map address out of range
simonscott 0:22c544c8741a 184 6 = Register map write error
simonscott 0:22c544c8741a 185 7 = BNO low power mode not available for selected operat ion mode
simonscott 0:22c544c8741a 186 8 = Accelerometer power mode not available
simonscott 0:22c544c8741a 187 9 = Fusion algorithm configuration error
simonscott 0:22c544c8741a 188 A = Sensor configuration error */
simonscott 0:22c544c8741a 189
simonscott 0:22c544c8741a 190 if (system_error != 0)
simonscott 0:22c544c8741a 191 *system_error = read8(BNO055_SYS_ERR_ADDR);
simonscott 0:22c544c8741a 192
simonscott 0:22c544c8741a 193 wait_ms(200);
simonscott 0:22c544c8741a 194 }
simonscott 0:22c544c8741a 195
simonscott 0:22c544c8741a 196 /**************************************************************************/
simonscott 0:22c544c8741a 197 /*!
simonscott 0:22c544c8741a 198 @brief Gets the chip revision numbers
simonscott 0:22c544c8741a 199 */
simonscott 0:22c544c8741a 200 /**************************************************************************/
simonscott 0:22c544c8741a 201 void Adafruit_BNO055::getRevInfo(adafruit_bno055_rev_info_t* info)
simonscott 0:22c544c8741a 202 {
simonscott 0:22c544c8741a 203 uint8_t a, b;
simonscott 0:22c544c8741a 204
simonscott 0:22c544c8741a 205 memset(info, 0, sizeof(adafruit_bno055_rev_info_t));
simonscott 0:22c544c8741a 206
simonscott 0:22c544c8741a 207 /* Check the accelerometer revision */
simonscott 0:22c544c8741a 208 info->accel_rev = read8(BNO055_ACCEL_REV_ID_ADDR);
simonscott 0:22c544c8741a 209
simonscott 0:22c544c8741a 210 /* Check the magnetometer revision */
simonscott 0:22c544c8741a 211 info->mag_rev = read8(BNO055_MAG_REV_ID_ADDR);
simonscott 0:22c544c8741a 212
simonscott 0:22c544c8741a 213 /* Check the gyroscope revision */
simonscott 0:22c544c8741a 214 info->gyro_rev = read8(BNO055_GYRO_REV_ID_ADDR);
simonscott 0:22c544c8741a 215
simonscott 0:22c544c8741a 216 /* Check the SW revision */
simonscott 0:22c544c8741a 217 info->bl_rev = read8(BNO055_BL_REV_ID_ADDR);
simonscott 0:22c544c8741a 218
simonscott 0:22c544c8741a 219 a = read8(BNO055_SW_REV_ID_LSB_ADDR);
simonscott 0:22c544c8741a 220 b = read8(BNO055_SW_REV_ID_MSB_ADDR);
simonscott 0:22c544c8741a 221 info->sw_rev = (((uint16_t)b) << 8) | ((uint16_t)a);
simonscott 0:22c544c8741a 222 }
simonscott 0:22c544c8741a 223
simonscott 0:22c544c8741a 224 /**************************************************************************/
simonscott 0:22c544c8741a 225 /*!
simonscott 0:22c544c8741a 226 @brief Gets current calibration state. Each value should be a uint8_t
simonscott 0:22c544c8741a 227 pointer and it will be set to 0 if not calibrated and 3 if
simonscott 0:22c544c8741a 228 fully calibrated.
simonscott 0:22c544c8741a 229 */
simonscott 0:22c544c8741a 230 /**************************************************************************/
simonscott 0:22c544c8741a 231 void Adafruit_BNO055::getCalibration(uint8_t* sys, uint8_t* gyro, uint8_t* accel, uint8_t* mag) {
simonscott 0:22c544c8741a 232 uint8_t calData = read8(BNO055_CALIB_STAT_ADDR);
simonscott 0:22c544c8741a 233 if (sys != NULL) {
simonscott 0:22c544c8741a 234 *sys = (calData >> 6) & 0x03;
simonscott 0:22c544c8741a 235 }
simonscott 0:22c544c8741a 236 if (gyro != NULL) {
simonscott 0:22c544c8741a 237 *gyro = (calData >> 4) & 0x03;
simonscott 0:22c544c8741a 238 }
simonscott 0:22c544c8741a 239 if (accel != NULL) {
simonscott 0:22c544c8741a 240 *accel = (calData >> 2) & 0x03;
simonscott 0:22c544c8741a 241 }
simonscott 0:22c544c8741a 242 if (mag != NULL) {
simonscott 0:22c544c8741a 243 *mag = calData & 0x03;
simonscott 0:22c544c8741a 244 }
simonscott 0:22c544c8741a 245 }
simonscott 0:22c544c8741a 246
simonscott 0:22c544c8741a 247 /**************************************************************************/
simonscott 0:22c544c8741a 248 /*!
simonscott 0:22c544c8741a 249 @brief Gets the temperature in degrees celsius
simonscott 0:22c544c8741a 250 */
simonscott 0:22c544c8741a 251 /**************************************************************************/
simonscott 0:22c544c8741a 252 int8_t Adafruit_BNO055::getTemp(void)
simonscott 0:22c544c8741a 253 {
simonscott 0:22c544c8741a 254 int8_t temp = (int8_t)(read8(BNO055_TEMP_ADDR));
simonscott 0:22c544c8741a 255 return temp;
simonscott 0:22c544c8741a 256 }
simonscott 0:22c544c8741a 257
simonscott 0:22c544c8741a 258 /**************************************************************************/
simonscott 0:22c544c8741a 259 /*!
simonscott 0:22c544c8741a 260 @brief Gets a vector reading from the specified source
simonscott 0:22c544c8741a 261 */
simonscott 0:22c544c8741a 262 /**************************************************************************/
simonscott 0:22c544c8741a 263 imu::Vector<3> Adafruit_BNO055::getVector(adafruit_vector_type_t vector_type)
simonscott 0:22c544c8741a 264 {
simonscott 0:22c544c8741a 265 imu::Vector<3> xyz;
simonscott 0:22c544c8741a 266 unsigned char buffer[6];
simonscott 0:22c544c8741a 267 memset (buffer, 0, 6);
simonscott 0:22c544c8741a 268
simonscott 0:22c544c8741a 269 int16_t x, y, z;
simonscott 0:22c544c8741a 270 x = y = z = 0;
simonscott 0:22c544c8741a 271
simonscott 0:22c544c8741a 272 /* Read vector data (6 bytes) */
simonscott 2:8092160b6a59 273 //readLen((adafruit_bno055_reg_t)vector_type, (char*)buffer, 6);
simonscott 2:8092160b6a59 274 readLen((adafruit_bno055_reg_t)0x08, (char*)buffer, 6);
simonscott 0:22c544c8741a 275
simonscott 0:22c544c8741a 276 x = ((int16_t)buffer[0]) | (((int16_t)buffer[1]) << 8);
simonscott 0:22c544c8741a 277 y = ((int16_t)buffer[2]) | (((int16_t)buffer[3]) << 8);
simonscott 0:22c544c8741a 278 z = ((int16_t)buffer[4]) | (((int16_t)buffer[5]) << 8);
simonscott 0:22c544c8741a 279
simonscott 0:22c544c8741a 280 /* Convert the value to an appropriate range (section 3.6.4) */
simonscott 0:22c544c8741a 281 /* and assign the value to the Vector type */
simonscott 0:22c544c8741a 282 switch(vector_type)
simonscott 0:22c544c8741a 283 {
simonscott 0:22c544c8741a 284 case VECTOR_MAGNETOMETER:
simonscott 0:22c544c8741a 285 /* 1uT = 16 LSB */
simonscott 0:22c544c8741a 286 xyz[0] = ((double)x)/16.0;
simonscott 0:22c544c8741a 287 xyz[1] = ((double)y)/16.0;
simonscott 0:22c544c8741a 288 xyz[2] = ((double)z)/16.0;
simonscott 0:22c544c8741a 289 break;
simonscott 0:22c544c8741a 290 case VECTOR_GYROSCOPE:
simonscott 0:22c544c8741a 291 /* 1rps = 900 LSB */
simonscott 0:22c544c8741a 292 xyz[0] = ((double)x)/900.0;
simonscott 0:22c544c8741a 293 xyz[1] = ((double)y)/900.0;
simonscott 0:22c544c8741a 294 xyz[2] = ((double)z)/900.0;
simonscott 0:22c544c8741a 295 break;
simonscott 0:22c544c8741a 296 case VECTOR_EULER:
simonscott 0:22c544c8741a 297 /* 1 degree = 16 LSB */
simonscott 0:22c544c8741a 298 xyz[0] = ((double)x)/16.0;
simonscott 0:22c544c8741a 299 xyz[1] = ((double)y)/16.0;
simonscott 0:22c544c8741a 300 xyz[2] = ((double)z)/16.0;
simonscott 0:22c544c8741a 301 break;
simonscott 0:22c544c8741a 302 case VECTOR_ACCELEROMETER:
simonscott 0:22c544c8741a 303 case VECTOR_LINEARACCEL:
simonscott 0:22c544c8741a 304 case VECTOR_GRAVITY:
simonscott 0:22c544c8741a 305 /* 1m/s^2 = 100 LSB */
simonscott 0:22c544c8741a 306 xyz[0] = ((double)x)/100.0;
simonscott 0:22c544c8741a 307 xyz[1] = ((double)y)/100.0;
simonscott 0:22c544c8741a 308 xyz[2] = ((double)z)/100.0;
simonscott 0:22c544c8741a 309 break;
simonscott 0:22c544c8741a 310 }
simonscott 0:22c544c8741a 311
simonscott 0:22c544c8741a 312 return xyz;
simonscott 0:22c544c8741a 313 }
simonscott 0:22c544c8741a 314
simonscott 0:22c544c8741a 315 /**************************************************************************/
simonscott 0:22c544c8741a 316 /*!
simonscott 0:22c544c8741a 317 @brief Gets a quaternion reading from the specified source
simonscott 0:22c544c8741a 318 */
simonscott 0:22c544c8741a 319 /**************************************************************************/
simonscott 0:22c544c8741a 320 imu::Quaternion Adafruit_BNO055::getQuat(void)
simonscott 0:22c544c8741a 321 {
simonscott 0:22c544c8741a 322 unsigned char buffer[8];
simonscott 0:22c544c8741a 323 memset (buffer, 0, 8);
simonscott 0:22c544c8741a 324
simonscott 0:22c544c8741a 325 int x, y, z, w;
simonscott 0:22c544c8741a 326 x = y = z = w = 0;
simonscott 0:22c544c8741a 327
simonscott 0:22c544c8741a 328 /* Read quat data (8 bytes) */
simonscott 0:22c544c8741a 329 readLen(BNO055_QUATERNION_DATA_W_LSB_ADDR, (char*)buffer, 8);
simonscott 0:22c544c8741a 330 w = (((uint16_t)buffer[1]) << 8) | ((uint16_t)buffer[0]);
simonscott 0:22c544c8741a 331 x = (((uint16_t)buffer[3]) << 8) | ((uint16_t)buffer[2]);
simonscott 0:22c544c8741a 332 y = (((uint16_t)buffer[5]) << 8) | ((uint16_t)buffer[4]);
simonscott 0:22c544c8741a 333 z = (((uint16_t)buffer[7]) << 8) | ((uint16_t)buffer[6]);
simonscott 0:22c544c8741a 334
simonscott 0:22c544c8741a 335 /* Assign to Quaternion */
simonscott 0:22c544c8741a 336 /* See http://ae-bst.resource.bosch.com/media/products/dokumente/bno055/BST_BNO055_DS000_12~1.pdf
simonscott 0:22c544c8741a 337 3.6.5.5 Orientation (Quaternion) */
simonscott 0:22c544c8741a 338 const double scale = (1.0 / (1<<14));
simonscott 0:22c544c8741a 339 imu::Quaternion quat(scale * w, scale * x, scale * y, scale * z);
simonscott 0:22c544c8741a 340 return quat;
simonscott 0:22c544c8741a 341 }
simonscott 0:22c544c8741a 342
simonscott 0:22c544c8741a 343 /**************************************************************************/
simonscott 0:22c544c8741a 344 /*!
simonscott 0:22c544c8741a 345 @brief Provides the sensor_t data for this sensor
simonscott 0:22c544c8741a 346 */
simonscott 0:22c544c8741a 347 /**************************************************************************/
simonscott 0:22c544c8741a 348 void Adafruit_BNO055::getSensor(sensor_t *sensor)
simonscott 0:22c544c8741a 349 {
simonscott 0:22c544c8741a 350 /* Clear the sensor_t object */
simonscott 0:22c544c8741a 351 memset(sensor, 0, sizeof(sensor_t));
simonscott 0:22c544c8741a 352
simonscott 0:22c544c8741a 353 /* Insert the sensor name in the fixed length char array */
simonscott 0:22c544c8741a 354 strncpy (sensor->name, "BNO055", sizeof(sensor->name) - 1);
simonscott 0:22c544c8741a 355 sensor->name[sizeof(sensor->name)- 1] = 0;
simonscott 0:22c544c8741a 356 sensor->version = 1;
simonscott 0:22c544c8741a 357 sensor->sensor_id = _sensorID;
simonscott 0:22c544c8741a 358 sensor->type = SENSOR_TYPE_ORIENTATION;
simonscott 0:22c544c8741a 359 sensor->min_delay = 0;
simonscott 0:22c544c8741a 360 sensor->max_value = 0.0F;
simonscott 0:22c544c8741a 361 sensor->min_value = 0.0F;
simonscott 0:22c544c8741a 362 sensor->resolution = 0.01F;
simonscott 0:22c544c8741a 363 }
simonscott 0:22c544c8741a 364
simonscott 0:22c544c8741a 365 /**************************************************************************/
simonscott 0:22c544c8741a 366 /*!
simonscott 0:22c544c8741a 367 @brief Reads the sensor and returns the data as a sensors_event_t
simonscott 0:22c544c8741a 368 */
simonscott 0:22c544c8741a 369 /**************************************************************************/
simonscott 0:22c544c8741a 370 bool Adafruit_BNO055::getEvent(sensors_event_t *event)
simonscott 0:22c544c8741a 371 {
simonscott 0:22c544c8741a 372 /* Clear the event */
simonscott 0:22c544c8741a 373 memset(event, 0, sizeof(sensors_event_t));
simonscott 0:22c544c8741a 374
simonscott 0:22c544c8741a 375 event->version = sizeof(sensors_event_t);
simonscott 0:22c544c8741a 376 event->sensor_id = _sensorID;
simonscott 0:22c544c8741a 377 event->type = SENSOR_TYPE_ORIENTATION;
simonscott 0:22c544c8741a 378 event->timestamp = 0; //TODO: fix this with a millis() call
simonscott 0:22c544c8741a 379
simonscott 0:22c544c8741a 380 /* Get a Euler angle sample for orientation */
simonscott 0:22c544c8741a 381 imu::Vector<3> euler = getVector(Adafruit_BNO055::VECTOR_EULER);
simonscott 0:22c544c8741a 382 event->orientation.x = euler.x();
simonscott 0:22c544c8741a 383 event->orientation.y = euler.y();
simonscott 0:22c544c8741a 384 event->orientation.z = euler.z();
simonscott 0:22c544c8741a 385
simonscott 0:22c544c8741a 386 return true;
simonscott 0:22c544c8741a 387 }
simonscott 0:22c544c8741a 388
simonscott 0:22c544c8741a 389 /***************************************************************************
simonscott 0:22c544c8741a 390 PRIVATE FUNCTIONS
simonscott 0:22c544c8741a 391 ***************************************************************************/
simonscott 0:22c544c8741a 392
simonscott 0:22c544c8741a 393 /**************************************************************************/
simonscott 0:22c544c8741a 394 /*!
simonscott 0:22c544c8741a 395 @brief Writes an 8 bit value over I2C
simonscott 0:22c544c8741a 396 */
simonscott 0:22c544c8741a 397 /**************************************************************************/
simonscott 0:22c544c8741a 398 bool Adafruit_BNO055::write8(adafruit_bno055_reg_t reg, char value)
simonscott 0:22c544c8741a 399 {
simonscott 0:22c544c8741a 400 char reg_to_write = (char)(reg);
simonscott 0:22c544c8741a 401 i2c->write(_address<<1, &reg_to_write, 1, true);
simonscott 1:b48e4192c101 402 wait(0.001);
simonscott 0:22c544c8741a 403 i2c->write(_address<<1, &value, 1, false);
simonscott 1:b48e4192c101 404 wait(0.001);
simonscott 0:22c544c8741a 405
simonscott 0:22c544c8741a 406 /* ToDo: Check for error! */
simonscott 0:22c544c8741a 407 return true;
simonscott 0:22c544c8741a 408 }
simonscott 0:22c544c8741a 409
simonscott 0:22c544c8741a 410 /**************************************************************************/
simonscott 0:22c544c8741a 411 /*!
simonscott 0:22c544c8741a 412 @brief Reads an 8 bit value over I2C
simonscott 0:22c544c8741a 413 */
simonscott 0:22c544c8741a 414 /**************************************************************************/
simonscott 0:22c544c8741a 415 char Adafruit_BNO055::read8(adafruit_bno055_reg_t reg )
simonscott 0:22c544c8741a 416 {
simonscott 0:22c544c8741a 417 char to_read = 0;
simonscott 0:22c544c8741a 418 char to_write = (char)reg;
simonscott 0:22c544c8741a 419
simonscott 0:22c544c8741a 420 i2c->write(_address<<1, &to_write, 1, false);
simonscott 1:b48e4192c101 421 wait(0.001);
simonscott 0:22c544c8741a 422 i2c->read(_address<<1, &to_read, 1, false);
simonscott 1:b48e4192c101 423 wait(0.001);
simonscott 2:8092160b6a59 424
simonscott 2:8092160b6a59 425 printf(" I2C Read : %d from addr: %d\r\n", to_read, to_write);
simonscott 0:22c544c8741a 426 return to_read;
simonscott 0:22c544c8741a 427 }
simonscott 0:22c544c8741a 428
simonscott 0:22c544c8741a 429 /**************************************************************************/
simonscott 0:22c544c8741a 430 /*!
simonscott 0:22c544c8741a 431 @brief Reads the specified number of bytes over I2C
simonscott 0:22c544c8741a 432 */
simonscott 0:22c544c8741a 433 /**************************************************************************/
simonscott 0:22c544c8741a 434 bool Adafruit_BNO055::readLen(adafruit_bno055_reg_t reg, char* buffer, int len)
simonscott 0:22c544c8741a 435 {
simonscott 0:22c544c8741a 436 char reg_to_write = (char)(reg);
simonscott 0:22c544c8741a 437
simonscott 0:22c544c8741a 438 i2c->write(_address<<1, &reg_to_write, 1, false);
simonscott 1:b48e4192c101 439 wait(0.001);
simonscott 0:22c544c8741a 440 i2c->read(_address<<1, buffer, len, false);
simonscott 1:b48e4192c101 441 wait(0.001);
simonscott 0:22c544c8741a 442
simonscott 2:8092160b6a59 443 printf("I2C: Read %d bytes from address %d\r\n", len, reg_to_write);
simonscott 2:8092160b6a59 444
simonscott 0:22c544c8741a 445 /* ToDo: Check for errors! */
simonscott 0:22c544c8741a 446 return true;
simonscott 0:22c544c8741a 447 }