Statistics
| Branch: | Revision:

adafruit_bno055 / Adafruit_BNO055.cpp @ fd9de024

History | View | Annotate | Download (19.692 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
  /* Make sure we have the right device */
61
  uint8_t id = read8(BNO055_CHIP_ID_ADDR);
62
  if(id != BNO055_ID)
63
  {
64
    delay(1000); // hold on for boot
65
    id = read8(BNO055_CHIP_ID_ADDR);
66
    if(id != BNO055_ID) {
67
      return false;  // still not? ok bail
68
    }
69
  }
70
71
  /* Switch to config mode (just in case since this is the default) */
72
  setMode(OPERATION_MODE_CONFIG);
73
74
  /* Reset */
75
  write8(BNO055_SYS_TRIGGER_ADDR, 0x20);
76
  while (read8(BNO055_CHIP_ID_ADDR) != BNO055_ID)
77
  {
78
    delay(10);
79
  }
80
  delay(50);
81
82
  /* Set to normal power mode */
83
  write8(BNO055_PWR_MODE_ADDR, POWER_MODE_NORMAL);
84
  delay(10);
85
86
  write8(BNO055_PAGE_ID_ADDR, 0);
87
88
  /* Set the output units */
89
  /*
90
  uint8_t unitsel = (0 << 7) | // Orientation = Android
91
                    (0 << 4) | // Temperature = Celsius
92
                    (0 << 2) | // Euler = Degrees
93
                    (1 << 1) | // Gyro = Rads
94
                    (0 << 0);  // Accelerometer = m/s^2
95
  write8(BNO055_UNIT_SEL_ADDR, unitsel);
96
  */
97
98 378858ec Shunya Sato
  /* Configure axis mapping (see section 3.4) */
99
  /*
100
  write8(BNO055_AXIS_MAP_CONFIG_ADDR, REMAP_CONFIG_P2); // P0-P7, Default is P1
101
  delay(10);
102
  write8(BNO055_AXIS_MAP_SIGN_ADDR, REMAP_SIGN_P2); // P0-P7, Default is P1
103
  delay(10);
104
  */
105
  
106 463eabf7 Wetmelon
  write8(BNO055_SYS_TRIGGER_ADDR, 0x0);
107
  delay(10);
108
  /* Set the requested operating mode (see section 3.3) */
109
  setMode(mode);
110
  delay(20);
111
112
  return true;
113 4bc1c0c1 Kevin Townsend
}
114
115
/**************************************************************************/
116
/*!
117 463eabf7 Wetmelon
    @brief  Puts the chip in the specified operating mode
118
*/
119 4bc1c0c1 Kevin Townsend
/**************************************************************************/
120
void Adafruit_BNO055::setMode(adafruit_bno055_opmode_t mode)
121
{
122 463eabf7 Wetmelon
  _mode = mode;
123
  write8(BNO055_OPR_MODE_ADDR, _mode);
124
  delay(30);
125 4bc1c0c1 Kevin Townsend
}
126
127
/**************************************************************************/
128
/*!
129 463eabf7 Wetmelon
    @brief  Use the external 32.768KHz crystal
130
*/
131 c4f272e1 ladyada
/**************************************************************************/
132
void Adafruit_BNO055::setExtCrystalUse(boolean usextal)
133
{
134 463eabf7 Wetmelon
  adafruit_bno055_opmode_t modeback = _mode;
135
136
  /* Switch to config mode (just in case since this is the default) */
137
  setMode(OPERATION_MODE_CONFIG);
138
  delay(25);
139
  write8(BNO055_PAGE_ID_ADDR, 0);
140
  if (usextal) {
141
    write8(BNO055_SYS_TRIGGER_ADDR, 0x80);
142
  } else {
143
    write8(BNO055_SYS_TRIGGER_ADDR, 0x00);
144
  }
145
  delay(10);
146
  /* Set the requested operating mode (see section 3.3) */
147
  setMode(modeback);
148
  delay(20);
149 c4f272e1 ladyada
}
150
151
152
/**************************************************************************/
153
/*!
154 463eabf7 Wetmelon
    @brief  Gets the latest system status info
155
*/
156 4bc1c0c1 Kevin Townsend
/**************************************************************************/
157 3b2655dc ladyada
void Adafruit_BNO055::getSystemStatus(uint8_t *system_status, uint8_t *self_test_result, uint8_t *system_error)
158 4bc1c0c1 Kevin Townsend
{
159 463eabf7 Wetmelon
  write8(BNO055_PAGE_ID_ADDR, 0);
160
161
  /* System Status (see section 4.3.58)
162
     ---------------------------------
163
     0 = Idle
164
     1 = System Error
165
     2 = Initializing Peripherals
166
     3 = System Iniitalization
167
     4 = Executing Self-Test
168
     5 = Sensor fusio algorithm running
169
     6 = System running without fusion algorithms */
170
171
  if (system_status != 0)
172
    *system_status    = read8(BNO055_SYS_STAT_ADDR);
173
174
  /* Self Test Results (see section )
175
     --------------------------------
176
     1 = test passed, 0 = test failed
177

178
     Bit 0 = Accelerometer self test
179
     Bit 1 = Magnetometer self test
180
     Bit 2 = Gyroscope self test
181
     Bit 3 = MCU self test
182

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