Statistics
| Branch: | Revision:

adafruit_bno055 / Adafruit_BNO055.cpp @ 56d5ab50

History | View | Annotate | Download (22.351 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 56d5ab50 Jan
  // Select BNO055 gyro temperature source - REDUCES DRIFT SIGNIFICANTLY!
104
  write8(BNO055_TEMP_SOURCE_ADDR, 0x01);
105
106 378858ec Shunya Sato
  /* Configure axis mapping (see section 3.4) */
107
  /*
108
  write8(BNO055_AXIS_MAP_CONFIG_ADDR, REMAP_CONFIG_P2); // P0-P7, Default is P1
109
  delay(10);
110
  write8(BNO055_AXIS_MAP_SIGN_ADDR, REMAP_SIGN_P2); // P0-P7, Default is P1
111
  delay(10);
112
  */
113 a97e0fe1 kA®0šhî
114 463eabf7 Wetmelon
  write8(BNO055_SYS_TRIGGER_ADDR, 0x0);
115
  delay(10);
116
  /* Set the requested operating mode (see section 3.3) */
117
  setMode(mode);
118
  delay(20);
119
120
  return true;
121 4bc1c0c1 Kevin Townsend
}
122
123
/**************************************************************************/
124
/*!
125 463eabf7 Wetmelon
    @brief  Puts the chip in the specified operating mode
126
*/
127 4bc1c0c1 Kevin Townsend
/**************************************************************************/
128
void Adafruit_BNO055::setMode(adafruit_bno055_opmode_t mode)
129
{
130 463eabf7 Wetmelon
  _mode = mode;
131
  write8(BNO055_OPR_MODE_ADDR, _mode);
132
  delay(30);
133 4bc1c0c1 Kevin Townsend
}
134
135
/**************************************************************************/
136
/*!
137 a97e0fe1 kA®0šhî
    @brief  Changes the chip's axis remap
138
*/
139
/**************************************************************************/
140
void Adafruit_BNO055::setAxisRemap( adafruit_bno055_axis_remap_config_t remapcode )
141
{
142
  adafruit_bno055_opmode_t modeback = _mode;
143
144
  setMode(OPERATION_MODE_CONFIG);
145
  delay(25);
146
  write8(BNO055_AXIS_MAP_CONFIG_ADDR, remapcode);
147
  delay(10);
148
  /* Set the requested operating mode (see section 3.3) */
149
  setMode(modeback);
150
  delay(20);
151
}
152
153
/**************************************************************************/
154
/*!
155
    @brief  Changes the chip's axis signs
156
*/
157
/**************************************************************************/
158
void Adafruit_BNO055::setAxisSign( adafruit_bno055_axis_remap_sign_t remapsign )
159
{
160
  adafruit_bno055_opmode_t modeback = _mode;
161
162
  setMode(OPERATION_MODE_CONFIG);
163
  delay(25);
164
  write8(BNO055_AXIS_MAP_SIGN_ADDR, remapsign);
165
  delay(10);
166
  /* Set the requested operating mode (see section 3.3) */
167
  setMode(modeback);
168
  delay(20);
169
}
170
171
172
/**************************************************************************/
173
/*!
174 463eabf7 Wetmelon
    @brief  Use the external 32.768KHz crystal
175
*/
176 c4f272e1 ladyada
/**************************************************************************/
177
void Adafruit_BNO055::setExtCrystalUse(boolean usextal)
178
{
179 463eabf7 Wetmelon
  adafruit_bno055_opmode_t modeback = _mode;
180
181
  /* Switch to config mode (just in case since this is the default) */
182
  setMode(OPERATION_MODE_CONFIG);
183
  delay(25);
184
  write8(BNO055_PAGE_ID_ADDR, 0);
185
  if (usextal) {
186
    write8(BNO055_SYS_TRIGGER_ADDR, 0x80);
187
  } else {
188
    write8(BNO055_SYS_TRIGGER_ADDR, 0x00);
189
  }
190
  delay(10);
191
  /* Set the requested operating mode (see section 3.3) */
192
  setMode(modeback);
193
  delay(20);
194 c4f272e1 ladyada
}
195
196
197
/**************************************************************************/
198
/*!
199 463eabf7 Wetmelon
    @brief  Gets the latest system status info
200
*/
201 4bc1c0c1 Kevin Townsend
/**************************************************************************/
202 3b2655dc ladyada
void Adafruit_BNO055::getSystemStatus(uint8_t *system_status, uint8_t *self_test_result, uint8_t *system_error)
203 4bc1c0c1 Kevin Townsend
{
204 463eabf7 Wetmelon
  write8(BNO055_PAGE_ID_ADDR, 0);
205
206
  /* System Status (see section 4.3.58)
207
     ---------------------------------
208
     0 = Idle
209
     1 = System Error
210
     2 = Initializing Peripherals
211
     3 = System Iniitalization
212
     4 = Executing Self-Test
213
     5 = Sensor fusio algorithm running
214
     6 = System running without fusion algorithms */
215
216
  if (system_status != 0)
217
    *system_status    = read8(BNO055_SYS_STAT_ADDR);
218
219
  /* Self Test Results (see section )
220
     --------------------------------
221
     1 = test passed, 0 = test failed
222

223
     Bit 0 = Accelerometer self test
224
     Bit 1 = Magnetometer self test
225
     Bit 2 = Gyroscope self test
226
     Bit 3 = MCU self test
227

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