Statistics
| Branch: | Revision:

adafruit_bno055 / Adafruit_BNO055.cpp @ 2d9324e5

History | View | Annotate | Download (19.834 KB)

1 4bc1c0c1 Kevin Townsend
/***************************************************************************
2
  This is a library for the BNO055 orientation sensor
3

4
  Designed specifically to work with the Adafruit BNO055 Breakout.
5

6
  Pick one up today in the adafruit shop!
7
  ------> http://www.adafruit.com/products
8

9
  These sensors use I2C to communicate, 2 pins are required to interface.
10

11
  Adafruit invests time and resources providing this open source code,
12
  please support Adafruit andopen-source hardware by purchasing products
13
  from Adafruit!
14

15
  Written by KTOWN for Adafruit Industries.
16

17
  MIT license, all text above must be included in any redistribution
18 463eabf7 Wetmelon
 ***************************************************************************/
19 4bc1c0c1 Kevin Townsend
20
#if ARDUINO >= 100
21 463eabf7 Wetmelon
 #include "Arduino.h"
22 4bc1c0c1 Kevin Townsend
#else
23 463eabf7 Wetmelon
 #include "WProgram.h"
24 4bc1c0c1 Kevin Townsend
#endif
25
26
#include <math.h>
27
#include <limits.h>
28
29
#include "Adafruit_BNO055.h"
30
31
/***************************************************************************
32
 CONSTRUCTOR
33
 ***************************************************************************/
34 40f91f6f Tony DiCola
35 4bc1c0c1 Kevin Townsend
/**************************************************************************/
36
/*!
37 463eabf7 Wetmelon
    @brief  Instantiates a new Adafruit_BNO055 class
38
*/
39 4bc1c0c1 Kevin Townsend
/**************************************************************************/
40
Adafruit_BNO055::Adafruit_BNO055(int32_t sensorID, uint8_t address)
41
{
42 463eabf7 Wetmelon
  _sensorID = sensorID;
43
  _address = address;
44 4bc1c0c1 Kevin Townsend
}
45
46
/***************************************************************************
47
 PUBLIC FUNCTIONS
48
 ***************************************************************************/
49
50
/**************************************************************************/
51
/*!
52 463eabf7 Wetmelon
    @brief  Sets up the HW
53
*/
54 4bc1c0c1 Kevin Townsend
/**************************************************************************/
55
bool Adafruit_BNO055::begin(adafruit_bno055_opmode_t mode)
56
{
57 463eabf7 Wetmelon
  /* Enable I2C */
58
  Wire.begin();
59
60 78cc710f ladyada
  // BNO055 clock stretches for 500us or more!
61
#ifdef ESP8266
62
  Wire.setClockStretchLimit(1000); // Allow for 1000us of clock stretching
63
#endif
64
65 463eabf7 Wetmelon
  /* Make sure we have the right device */
66
  uint8_t id = read8(BNO055_CHIP_ID_ADDR);
67
  if(id != BNO055_ID)
68
  {
69
    delay(1000); // hold on for boot
70
    id = read8(BNO055_CHIP_ID_ADDR);
71
    if(id != BNO055_ID) {
72
      return false;  // still not? ok bail
73
    }
74
  }
75
76
  /* Switch to config mode (just in case since this is the default) */
77
  setMode(OPERATION_MODE_CONFIG);
78
79
  /* Reset */
80
  write8(BNO055_SYS_TRIGGER_ADDR, 0x20);
81
  while (read8(BNO055_CHIP_ID_ADDR) != BNO055_ID)
82
  {
83
    delay(10);
84
  }
85
  delay(50);
86
87
  /* Set to normal power mode */
88
  write8(BNO055_PWR_MODE_ADDR, POWER_MODE_NORMAL);
89
  delay(10);
90
91
  write8(BNO055_PAGE_ID_ADDR, 0);
92
93
  /* Set the output units */
94
  /*
95
  uint8_t unitsel = (0 << 7) | // Orientation = Android
96
                    (0 << 4) | // Temperature = Celsius
97
                    (0 << 2) | // Euler = Degrees
98
                    (1 << 1) | // Gyro = Rads
99
                    (0 << 0);  // Accelerometer = m/s^2
100
  write8(BNO055_UNIT_SEL_ADDR, unitsel);
101
  */
102
103 378858ec Shunya Sato
  /* Configure axis mapping (see section 3.4) */
104
  /*
105
  write8(BNO055_AXIS_MAP_CONFIG_ADDR, REMAP_CONFIG_P2); // P0-P7, Default is P1
106
  delay(10);
107
  write8(BNO055_AXIS_MAP_SIGN_ADDR, REMAP_SIGN_P2); // P0-P7, Default is P1
108
  delay(10);
109
  */
110
  
111 463eabf7 Wetmelon
  write8(BNO055_SYS_TRIGGER_ADDR, 0x0);
112
  delay(10);
113
  /* Set the requested operating mode (see section 3.3) */
114
  setMode(mode);
115
  delay(20);
116
117
  return true;
118 4bc1c0c1 Kevin Townsend
}
119
120
/**************************************************************************/
121
/*!
122 463eabf7 Wetmelon
    @brief  Puts the chip in the specified operating mode
123
*/
124 4bc1c0c1 Kevin Townsend
/**************************************************************************/
125
void Adafruit_BNO055::setMode(adafruit_bno055_opmode_t mode)
126
{
127 463eabf7 Wetmelon
  _mode = mode;
128
  write8(BNO055_OPR_MODE_ADDR, _mode);
129
  delay(30);
130 4bc1c0c1 Kevin Townsend
}
131
132
/**************************************************************************/
133
/*!
134 463eabf7 Wetmelon
    @brief  Use the external 32.768KHz crystal
135
*/
136 c4f272e1 ladyada
/**************************************************************************/
137
void Adafruit_BNO055::setExtCrystalUse(boolean usextal)
138
{
139 463eabf7 Wetmelon
  adafruit_bno055_opmode_t modeback = _mode;
140
141
  /* Switch to config mode (just in case since this is the default) */
142
  setMode(OPERATION_MODE_CONFIG);
143
  delay(25);
144
  write8(BNO055_PAGE_ID_ADDR, 0);
145
  if (usextal) {
146
    write8(BNO055_SYS_TRIGGER_ADDR, 0x80);
147
  } else {
148
    write8(BNO055_SYS_TRIGGER_ADDR, 0x00);
149
  }
150
  delay(10);
151
  /* Set the requested operating mode (see section 3.3) */
152
  setMode(modeback);
153
  delay(20);
154 c4f272e1 ladyada
}
155
156
157
/**************************************************************************/
158
/*!
159 463eabf7 Wetmelon
    @brief  Gets the latest system status info
160
*/
161 4bc1c0c1 Kevin Townsend
/**************************************************************************/
162 3b2655dc ladyada
void Adafruit_BNO055::getSystemStatus(uint8_t *system_status, uint8_t *self_test_result, uint8_t *system_error)
163 4bc1c0c1 Kevin Townsend
{
164 463eabf7 Wetmelon
  write8(BNO055_PAGE_ID_ADDR, 0);
165
166
  /* System Status (see section 4.3.58)
167
     ---------------------------------
168
     0 = Idle
169
     1 = System Error
170
     2 = Initializing Peripherals
171
     3 = System Iniitalization
172
     4 = Executing Self-Test
173
     5 = Sensor fusio algorithm running
174
     6 = System running without fusion algorithms */
175
176
  if (system_status != 0)
177
    *system_status    = read8(BNO055_SYS_STAT_ADDR);
178
179
  /* Self Test Results (see section )
180
     --------------------------------
181
     1 = test passed, 0 = test failed
182

183
     Bit 0 = Accelerometer self test
184
     Bit 1 = Magnetometer self test
185
     Bit 2 = Gyroscope self test
186
     Bit 3 = MCU self test
187

188
     0x0F = all good! */
189
190
  if (self_test_result != 0)
191
    *self_test_result = read8(BNO055_SELFTEST_RESULT_ADDR);
192
193
  /* System Error (see section 4.3.59)
194
     ---------------------------------
195
     0 = No error
196
     1 = Peripheral initialization error
197
     2 = System initialization error
198
     3 = Self test result failed
199
     4 = Register map value out of range
200
     5 = Register map address out of range
201
     6 = Register map write error
202
     7 = BNO low power mode not available for selected operat ion mode
203
     8 = Accelerometer power mode not available
204
     9 = Fusion algorithm configuration error
205
     A = Sensor configuration error */
206
207
  if (system_error != 0)
208
    *system_error     = read8(BNO055_SYS_ERR_ADDR);
209
210
  delay(200);
211 4bc1c0c1 Kevin Townsend
}
212
213
/**************************************************************************/
214
/*!
215 463eabf7 Wetmelon
    @brief  Gets the chip revision numbers
216
*/
217 4bc1c0c1 Kevin Townsend
/**************************************************************************/
218
void Adafruit_BNO055::getRevInfo(adafruit_bno055_rev_info_t* info)
219
{
220 463eabf7 Wetmelon
  uint8_t a, b;
221 4bc1c0c1 Kevin Townsend
222 463eabf7 Wetmelon
  memset(info, 0, sizeof(adafruit_bno055_rev_info_t));
223 4bc1c0c1 Kevin Townsend
224 463eabf7 Wetmelon
  /* Check the accelerometer revision */
225
  info->accel_rev = read8(BNO055_ACCEL_REV_ID_ADDR);
226 67f3cff5 Kevin Townsend
227 463eabf7 Wetmelon
  /* Check the magnetometer revision */
228
  info->mag_rev   = read8(BNO055_MAG_REV_ID_ADDR);
229 67f3cff5 Kevin Townsend
230 463eabf7 Wetmelon
  /* Check the gyroscope revision */
231
  info->gyro_rev  = read8(BNO055_GYRO_REV_ID_ADDR);
232 67f3cff5 Kevin Townsend
233 463eabf7 Wetmelon
  /* Check the SW revision */
234
  info->bl_rev    = read8(BNO055_BL_REV_ID_ADDR);
235 40f91f6f Tony DiCola
236 463eabf7 Wetmelon
  a = read8(BNO055_SW_REV_ID_LSB_ADDR);
237
  b = read8(BNO055_SW_REV_ID_MSB_ADDR);
238
  info->sw_rev = (((uint16_t)b) << 8) | ((uint16_t)a);
239 4bc1c0c1 Kevin Townsend
}
240
241
/**************************************************************************/
242
/*!
243 463eabf7 Wetmelon
    @brief  Gets current calibration state.  Each value should be a uint8_t
244
            pointer and it will be set to 0 if not calibrated and 3 if
245
            fully calibrated.
246
*/
247 40f91f6f Tony DiCola
/**************************************************************************/
248
void Adafruit_BNO055::getCalibration(uint8_t* sys, uint8_t* gyro, uint8_t* accel, uint8_t* mag) {
249 463eabf7 Wetmelon
  uint8_t calData = read8(BNO055_CALIB_STAT_ADDR);
250
  if (sys != NULL) {
251
    *sys = (calData >> 6) & 0x03;
252
  }
253
  if (gyro != NULL) {
254
    *gyro = (calData >> 4) & 0x03;
255
  }
256
  if (accel != NULL) {
257
    *accel = (calData >> 2) & 0x03;
258
  }
259
  if (mag != NULL) {
260
    *mag = calData & 0x03;
261
  }
262 40f91f6f Tony DiCola
}
263
264
/**************************************************************************/
265
/*!
266 463eabf7 Wetmelon
    @brief  Gets the temperature in degrees celsius
267
*/
268 0e2e2723 Kevin Townsend
/**************************************************************************/
269
int8_t Adafruit_BNO055::getTemp(void)
270
{
271 463eabf7 Wetmelon
  int8_t temp = (int8_t)(read8(BNO055_TEMP_ADDR));
272
  return temp;
273 0e2e2723 Kevin Townsend
}
274
275
/**************************************************************************/
276
/*!
277 463eabf7 Wetmelon
    @brief  Gets a vector reading from the specified source
278
*/
279 4bc1c0c1 Kevin Townsend
/**************************************************************************/
280 48741e1f Kevin Townsend
imu::Vector<3> Adafruit_BNO055::getVector(adafruit_vector_type_t vector_type)
281 4bc1c0c1 Kevin Townsend
{
282 463eabf7 Wetmelon
  imu::Vector<3> xyz;
283
  uint8_t buffer[6];
284
  memset (buffer, 0, 6);
285
286
  int16_t x, y, z;
287
  x = y = z = 0;
288
289
  /* Read vector data (6 bytes) */
290
  readLen((adafruit_bno055_reg_t)vector_type, buffer, 6);
291
292
  x = ((int16_t)buffer[0]) | (((int16_t)buffer[1]) << 8);
293
  y = ((int16_t)buffer[2]) | (((int16_t)buffer[3]) << 8);
294
  z = ((int16_t)buffer[4]) | (((int16_t)buffer[5]) << 8);
295
296
  /* Convert the value to an appropriate range (section 3.6.4) */
297
  /* and assign the value to the Vector type */
298
  switch(vector_type)
299
  {
300
    case VECTOR_MAGNETOMETER:
301
      /* 1uT = 16 LSB */
302
      xyz[0] = ((double)x)/16.0;
303
      xyz[1] = ((double)y)/16.0;
304
      xyz[2] = ((double)z)/16.0;
305
      break;
306
    case VECTOR_GYROSCOPE:
307
      /* 1rps = 900 LSB */
308
      xyz[0] = ((double)x)/900.0;
309
      xyz[1] = ((double)y)/900.0;
310
      xyz[2] = ((double)z)/900.0;
311
      break;
312
    case VECTOR_EULER:
313
      /* 1 degree = 16 LSB */
314
      xyz[0] = ((double)x)/16.0;
315
      xyz[1] = ((double)y)/16.0;
316
      xyz[2] = ((double)z)/16.0;
317
      break;
318
    case VECTOR_ACCELEROMETER:
319
    case VECTOR_LINEARACCEL:
320
    case VECTOR_GRAVITY:
321
      /* 1m/s^2 = 100 LSB */
322
      xyz[0] = ((double)x)/100.0;
323
      xyz[1] = ((double)y)/100.0;
324
      xyz[2] = ((double)z)/100.0;
325
      break;
326
  }
327
328
  return xyz;
329 4bc1c0c1 Kevin Townsend
}
330
331
/**************************************************************************/
332
/*!
333 463eabf7 Wetmelon
    @brief  Gets a quaternion reading from the specified source
334
*/
335 48741e1f Kevin Townsend
/**************************************************************************/
336
imu::Quaternion Adafruit_BNO055::getQuat(void)
337
{
338 463eabf7 Wetmelon
  uint8_t buffer[8];
339
  memset (buffer, 0, 8);
340
341
  int16_t x, y, z, w;
342
  x = y = z = w = 0;
343
344
  /* Read quat data (8 bytes) */
345
  readLen(BNO055_QUATERNION_DATA_W_LSB_ADDR, buffer, 8);
346
  w = (((uint16_t)buffer[1]) << 8) | ((uint16_t)buffer[0]);
347
  x = (((uint16_t)buffer[3]) << 8) | ((uint16_t)buffer[2]);
348
  y = (((uint16_t)buffer[5]) << 8) | ((uint16_t)buffer[4]);
349
  z = (((uint16_t)buffer[7]) << 8) | ((uint16_t)buffer[6]);
350
351
  /* Assign to Quaternion */
352
  /* See http://ae-bst.resource.bosch.com/media/products/dokumente/bno055/BST_BNO055_DS000_12~1.pdf
353
     3.6.5.5 Orientation (Quaternion)  */
354
  const double scale = (1.0 / (1<<14));
355
  imu::Quaternion quat(scale * w, scale * x, scale * y, scale * z);
356
  return quat;
357 48741e1f Kevin Townsend
}
358
359
/**************************************************************************/
360
/*!
361 463eabf7 Wetmelon
    @brief  Provides the sensor_t data for this sensor
362
*/
363 4bc1c0c1 Kevin Townsend
/**************************************************************************/
364
void Adafruit_BNO055::getSensor(sensor_t *sensor)
365
{
366 463eabf7 Wetmelon
  /* Clear the sensor_t object */
367
  memset(sensor, 0, sizeof(sensor_t));
368
369
  /* Insert the sensor name in the fixed length char array */
370
  strncpy (sensor->name, "BNO055", sizeof(sensor->name) - 1);
371
  sensor->name[sizeof(sensor->name)- 1] = 0;
372
  sensor->version     = 1;
373
  sensor->sensor_id   = _sensorID;
374
  sensor->type        = SENSOR_TYPE_ORIENTATION;
375
  sensor->min_delay   = 0;
376
  sensor->max_value   = 0.0F;
377
  sensor->min_value   = 0.0F;
378
  sensor->resolution  = 0.01F;
379 4bc1c0c1 Kevin Townsend
}
380
381
/**************************************************************************/
382
/*!
383 463eabf7 Wetmelon
    @brief  Reads the sensor and returns the data as a sensors_event_t
384
*/
385 4bc1c0c1 Kevin Townsend
/**************************************************************************/
386
bool Adafruit_BNO055::getEvent(sensors_event_t *event)
387
{
388 463eabf7 Wetmelon
  /* Clear the event */
389
  memset(event, 0, sizeof(sensors_event_t));
390 4bc1c0c1 Kevin Townsend
391 463eabf7 Wetmelon
  event->version   = sizeof(sensors_event_t);
392
  event->sensor_id = _sensorID;
393
  event->type      = SENSOR_TYPE_ORIENTATION;
394
  event->timestamp = millis();
395 fcd68623 Kevin Townsend
396 463eabf7 Wetmelon
  /* Get a Euler angle sample for orientation */
397
  imu::Vector<3> euler = getVector(Adafruit_BNO055::VECTOR_EULER);
398
  event->orientation.x = euler.x();
399
  event->orientation.y = euler.y();
400
  event->orientation.z = euler.z();
401 312a5b9e Wetmelon
402 463eabf7 Wetmelon
  return true;
403 312a5b9e Wetmelon
}
404 fcd68623 Kevin Townsend
405 312a5b9e Wetmelon
/**************************************************************************/
406
/*!
407
@brief  Reads the sensor's offset registers into a byte array
408
*/
409
/**************************************************************************/
410 8e095f02 Wetmelon
bool Adafruit_BNO055::getSensorOffsets(uint8_t* calibData)
411 312a5b9e Wetmelon
{
412 463eabf7 Wetmelon
    if (isFullyCalibrated())
413
    {
414
        adafruit_bno055_opmode_t lastMode = _mode;
415
        setMode(OPERATION_MODE_CONFIG);
416 312a5b9e Wetmelon
417 463eabf7 Wetmelon
        readLen(ACCEL_OFFSET_X_LSB_ADDR, calibData, NUM_BNO055_OFFSET_REGISTERS);
418 312a5b9e Wetmelon
419 463eabf7 Wetmelon
        setMode(lastMode);
420
        return true;
421
    }
422
    return false;
423 312a5b9e Wetmelon
}
424
425
/**************************************************************************/
426
/*!
427
@brief  Reads the sensor's offset registers into an offset struct
428
*/
429
/**************************************************************************/
430
bool Adafruit_BNO055::getSensorOffsets(adafruit_bno055_offsets_t &offsets_type)
431
{
432 463eabf7 Wetmelon
    if (isFullyCalibrated())
433
    {
434
        adafruit_bno055_opmode_t lastMode = _mode;
435
        setMode(OPERATION_MODE_CONFIG);
436
        delay(25);
437
438
        offsets_type.accel_offset_x = (read8(ACCEL_OFFSET_X_MSB_ADDR) << 8) | (read8(ACCEL_OFFSET_X_LSB_ADDR));
439
        offsets_type.accel_offset_y = (read8(ACCEL_OFFSET_Y_MSB_ADDR) << 8) | (read8(ACCEL_OFFSET_Y_LSB_ADDR));
440
        offsets_type.accel_offset_z = (read8(ACCEL_OFFSET_Z_MSB_ADDR) << 8) | (read8(ACCEL_OFFSET_Z_LSB_ADDR));
441
442
        offsets_type.gyro_offset_x = (read8(GYRO_OFFSET_X_MSB_ADDR) << 8) | (read8(GYRO_OFFSET_X_LSB_ADDR));
443
        offsets_type.gyro_offset_y = (read8(GYRO_OFFSET_Y_MSB_ADDR) << 8) | (read8(GYRO_OFFSET_Y_LSB_ADDR));
444
        offsets_type.gyro_offset_z = (read8(GYRO_OFFSET_Z_MSB_ADDR) << 8) | (read8(GYRO_OFFSET_Z_LSB_ADDR));
445
446
        offsets_type.mag_offset_x = (read8(MAG_OFFSET_X_MSB_ADDR) << 8) | (read8(MAG_OFFSET_X_LSB_ADDR));
447
        offsets_type.mag_offset_y = (read8(MAG_OFFSET_Y_MSB_ADDR) << 8) | (read8(MAG_OFFSET_Y_LSB_ADDR));
448
        offsets_type.mag_offset_z = (read8(MAG_OFFSET_Z_MSB_ADDR) << 8) | (read8(MAG_OFFSET_Z_LSB_ADDR));
449
450
        offsets_type.accel_radius = (read8(ACCEL_RADIUS_MSB_ADDR) << 8) | (read8(ACCEL_RADIUS_LSB_ADDR));
451
        offsets_type.mag_radius = (read8(MAG_RADIUS_MSB_ADDR) << 8) | (read8(MAG_RADIUS_LSB_ADDR));
452
453
        setMode(lastMode);
454
        return true;
455
    }
456
    return false;
457 312a5b9e Wetmelon
}
458
459
460
/**************************************************************************/
461
/*!
462
@brief  Writes an array of calibration values to the sensor's offset registers
463
*/
464
/**************************************************************************/
465 8e095f02 Wetmelon
void Adafruit_BNO055::setSensorOffsets(const uint8_t* calibData)
466 312a5b9e Wetmelon
{
467 463eabf7 Wetmelon
    adafruit_bno055_opmode_t lastMode = _mode;
468
    setMode(OPERATION_MODE_CONFIG);
469
    delay(25);
470
471
    /* A writeLen() would make this much cleaner */
472
    write8(ACCEL_OFFSET_X_LSB_ADDR, calibData[0]);
473
    write8(ACCEL_OFFSET_X_MSB_ADDR, calibData[1]);
474
    write8(ACCEL_OFFSET_Y_LSB_ADDR, calibData[2]);
475
    write8(ACCEL_OFFSET_Y_MSB_ADDR, calibData[3]);
476
    write8(ACCEL_OFFSET_Z_LSB_ADDR, calibData[4]);
477
    write8(ACCEL_OFFSET_Z_MSB_ADDR, calibData[5]);
478
479
    write8(GYRO_OFFSET_X_LSB_ADDR, calibData[6]);
480
    write8(GYRO_OFFSET_X_MSB_ADDR, calibData[7]);
481
    write8(GYRO_OFFSET_Y_LSB_ADDR, calibData[8]);
482
    write8(GYRO_OFFSET_Y_MSB_ADDR, calibData[9]);
483
    write8(GYRO_OFFSET_Z_LSB_ADDR, calibData[10]);
484
    write8(GYRO_OFFSET_Z_MSB_ADDR, calibData[11]);
485
486
    write8(MAG_OFFSET_X_LSB_ADDR, calibData[12]);
487
    write8(MAG_OFFSET_X_MSB_ADDR, calibData[13]);
488
    write8(MAG_OFFSET_Y_LSB_ADDR, calibData[14]);
489
    write8(MAG_OFFSET_Y_MSB_ADDR, calibData[15]);
490
    write8(MAG_OFFSET_Z_LSB_ADDR, calibData[16]);
491
    write8(MAG_OFFSET_Z_MSB_ADDR, calibData[17]);
492
493
    write8(ACCEL_RADIUS_LSB_ADDR, calibData[18]);
494
    write8(ACCEL_RADIUS_MSB_ADDR, calibData[19]);
495
496
    write8(MAG_RADIUS_LSB_ADDR, calibData[20]);
497
    write8(MAG_RADIUS_MSB_ADDR, calibData[21]);
498
499
    setMode(lastMode);
500 4bc1c0c1 Kevin Townsend
}
501
502 312a5b9e Wetmelon
/**************************************************************************/
503
/*!
504
@brief  Writes to the sensor's offset registers from an offset struct
505
*/
506
/**************************************************************************/
507
void Adafruit_BNO055::setSensorOffsets(const adafruit_bno055_offsets_t &offsets_type)
508
{
509 463eabf7 Wetmelon
    adafruit_bno055_opmode_t lastMode = _mode;
510
    setMode(OPERATION_MODE_CONFIG);
511
    delay(25);
512
513
    write8(ACCEL_OFFSET_X_LSB_ADDR, (offsets_type.accel_offset_x) & 0x0FF);
514
    write8(ACCEL_OFFSET_X_MSB_ADDR, (offsets_type.accel_offset_x >> 8) & 0x0FF);
515
    write8(ACCEL_OFFSET_Y_LSB_ADDR, (offsets_type.accel_offset_y) & 0x0FF);
516
    write8(ACCEL_OFFSET_Y_MSB_ADDR, (offsets_type.accel_offset_y >> 8) & 0x0FF);
517
    write8(ACCEL_OFFSET_Z_LSB_ADDR, (offsets_type.accel_offset_z) & 0x0FF);
518
    write8(ACCEL_OFFSET_Z_MSB_ADDR, (offsets_type.accel_offset_z >> 8) & 0x0FF);
519
520
    write8(GYRO_OFFSET_X_LSB_ADDR, (offsets_type.gyro_offset_x) & 0x0FF);
521
    write8(GYRO_OFFSET_X_MSB_ADDR, (offsets_type.gyro_offset_x >> 8) & 0x0FF);
522
    write8(GYRO_OFFSET_Y_LSB_ADDR, (offsets_type.gyro_offset_y) & 0x0FF);
523
    write8(GYRO_OFFSET_Y_MSB_ADDR, (offsets_type.gyro_offset_y >> 8) & 0x0FF);
524
    write8(GYRO_OFFSET_Z_LSB_ADDR, (offsets_type.gyro_offset_z) & 0x0FF);
525
    write8(GYRO_OFFSET_Z_MSB_ADDR, (offsets_type.gyro_offset_z >> 8) & 0x0FF);
526
527
    write8(MAG_OFFSET_X_LSB_ADDR, (offsets_type.mag_offset_x) & 0x0FF);
528
    write8(MAG_OFFSET_X_MSB_ADDR, (offsets_type.mag_offset_x >> 8) & 0x0FF);
529
    write8(MAG_OFFSET_Y_LSB_ADDR, (offsets_type.mag_offset_y) & 0x0FF);
530
    write8(MAG_OFFSET_Y_MSB_ADDR, (offsets_type.mag_offset_y >> 8) & 0x0FF);
531
    write8(MAG_OFFSET_Z_LSB_ADDR, (offsets_type.mag_offset_z) & 0x0FF);
532
    write8(MAG_OFFSET_Z_MSB_ADDR, (offsets_type.mag_offset_z >> 8) & 0x0FF);
533
534
    write8(ACCEL_RADIUS_LSB_ADDR, (offsets_type.accel_radius) & 0x0FF);
535
    write8(ACCEL_RADIUS_MSB_ADDR, (offsets_type.accel_radius >> 8) & 0x0FF);
536
537
    write8(MAG_RADIUS_LSB_ADDR, (offsets_type.mag_radius) & 0x0FF);
538
    write8(MAG_RADIUS_MSB_ADDR, (offsets_type.mag_radius >> 8) & 0x0FF);
539
540
    setMode(lastMode);
541 312a5b9e Wetmelon
}
542
543
bool Adafruit_BNO055::isFullyCalibrated(void)
544
{
545 463eabf7 Wetmelon
    uint8_t system, gyro, accel, mag;
546
    getCalibration(&system, &gyro, &accel, &mag);
547
    if (system < 3 || gyro < 3 || accel < 3 || mag < 3)
548
        return false;
549
    return true;
550 312a5b9e Wetmelon
}
551
552
553 4bc1c0c1 Kevin Townsend
/***************************************************************************
554
 PRIVATE FUNCTIONS
555
 ***************************************************************************/
556
557
/**************************************************************************/
558
/*!
559 463eabf7 Wetmelon
    @brief  Writes an 8 bit value over I2C
560
*/
561 4bc1c0c1 Kevin Townsend
/**************************************************************************/
562
bool Adafruit_BNO055::write8(adafruit_bno055_reg_t reg, byte value)
563
{
564 463eabf7 Wetmelon
  Wire.beginTransmission(_address);
565
  #if ARDUINO >= 100
566
    Wire.write((uint8_t)reg);
567
    Wire.write((uint8_t)value);
568
  #else
569
    Wire.send(reg);
570
    Wire.send(value);
571
  #endif
572
  Wire.endTransmission();
573
574
  /* ToDo: Check for error! */
575
  return true;
576 4bc1c0c1 Kevin Townsend
}
577
578
/**************************************************************************/
579
/*!
580 463eabf7 Wetmelon
    @brief  Reads an 8 bit value over I2C
581
*/
582 4bc1c0c1 Kevin Townsend
/**************************************************************************/
583 463eabf7 Wetmelon
byte Adafruit_BNO055::read8(adafruit_bno055_reg_t reg )
584 4bc1c0c1 Kevin Townsend
{
585 463eabf7 Wetmelon
  byte value = 0;
586
587
  Wire.beginTransmission(_address);
588
  #if ARDUINO >= 100
589
    Wire.write((uint8_t)reg);
590
  #else
591
    Wire.send(reg);
592
  #endif
593
  Wire.endTransmission();
594
  Wire.requestFrom(_address, (byte)1);
595
  #if ARDUINO >= 100
596
    value = Wire.read();
597
  #else
598
    value = Wire.receive();
599
  #endif
600
601
  return value;
602 4bc1c0c1 Kevin Townsend
}
603
604
/**************************************************************************/
605
/*!
606 463eabf7 Wetmelon
    @brief  Reads the specified number of bytes over I2C
607
*/
608 4bc1c0c1 Kevin Townsend
/**************************************************************************/
609
bool Adafruit_BNO055::readLen(adafruit_bno055_reg_t reg, byte * buffer, uint8_t len)
610
{
611 463eabf7 Wetmelon
  Wire.beginTransmission(_address);
612
  #if ARDUINO >= 100
613
    Wire.write((uint8_t)reg);
614
  #else
615
    Wire.send(reg);
616
  #endif
617
  Wire.endTransmission();
618
  Wire.requestFrom(_address, (byte)len);
619
620
  for (uint8_t i = 0; i < len; i++)
621
  {
622
    #if ARDUINO >= 100
623
      buffer[i] = Wire.read();
624
    #else
625
      buffer[i] = Wire.receive();
626
    #endif
627
  }
628
629
  /* ToDo: Check for errors! */
630
  return true;
631
}