Statistics
| Branch: | Revision:

adafruit_bno055 / Adafruit_BNO055.cpp @ a6b8a32b

History | View | Annotate | Download (22.242 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 a6b8a32b Limor "Ladyada" Fried
  ------> https://www.adafruit.com/product/2472
8 4bc1c0c1 Kevin Townsend

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 a97e0fe1 kA®0šhî
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 a97e0fe1 kA®0šhî
    @brief  Changes the chip's axis remap
135
*/
136
/**************************************************************************/
137
void Adafruit_BNO055::setAxisRemap( adafruit_bno055_axis_remap_config_t remapcode )
138
{
139
  adafruit_bno055_opmode_t modeback = _mode;
140
141
  setMode(OPERATION_MODE_CONFIG);
142
  delay(25);
143
  write8(BNO055_AXIS_MAP_CONFIG_ADDR, remapcode);
144
  delay(10);
145
  /* Set the requested operating mode (see section 3.3) */
146
  setMode(modeback);
147
  delay(20);
148
}
149
150
/**************************************************************************/
151
/*!
152
    @brief  Changes the chip's axis signs
153
*/
154
/**************************************************************************/
155
void Adafruit_BNO055::setAxisSign( adafruit_bno055_axis_remap_sign_t remapsign )
156
{
157
  adafruit_bno055_opmode_t modeback = _mode;
158
159
  setMode(OPERATION_MODE_CONFIG);
160
  delay(25);
161
  write8(BNO055_AXIS_MAP_SIGN_ADDR, remapsign);
162
  delay(10);
163
  /* Set the requested operating mode (see section 3.3) */
164
  setMode(modeback);
165
  delay(20);
166
}
167
168
169
/**************************************************************************/
170
/*!
171 463eabf7 Wetmelon
    @brief  Use the external 32.768KHz crystal
172
*/
173 c4f272e1 ladyada
/**************************************************************************/
174
void Adafruit_BNO055::setExtCrystalUse(boolean usextal)
175
{
176 463eabf7 Wetmelon
  adafruit_bno055_opmode_t modeback = _mode;
177
178
  /* Switch to config mode (just in case since this is the default) */
179
  setMode(OPERATION_MODE_CONFIG);
180
  delay(25);
181
  write8(BNO055_PAGE_ID_ADDR, 0);
182
  if (usextal) {
183
    write8(BNO055_SYS_TRIGGER_ADDR, 0x80);
184
  } else {
185
    write8(BNO055_SYS_TRIGGER_ADDR, 0x00);
186
  }
187
  delay(10);
188
  /* Set the requested operating mode (see section 3.3) */
189
  setMode(modeback);
190
  delay(20);
191 c4f272e1 ladyada
}
192
193
194
/**************************************************************************/
195
/*!
196 463eabf7 Wetmelon
    @brief  Gets the latest system status info
197
*/
198 4bc1c0c1 Kevin Townsend
/**************************************************************************/
199 3b2655dc ladyada
void Adafruit_BNO055::getSystemStatus(uint8_t *system_status, uint8_t *self_test_result, uint8_t *system_error)
200 4bc1c0c1 Kevin Townsend
{
201 463eabf7 Wetmelon
  write8(BNO055_PAGE_ID_ADDR, 0);
202
203
  /* System Status (see section 4.3.58)
204
     ---------------------------------
205
     0 = Idle
206
     1 = System Error
207
     2 = Initializing Peripherals
208
     3 = System Iniitalization
209
     4 = Executing Self-Test
210
     5 = Sensor fusio algorithm running
211
     6 = System running without fusion algorithms */
212
213
  if (system_status != 0)
214
    *system_status    = read8(BNO055_SYS_STAT_ADDR);
215
216
  /* Self Test Results (see section )
217
     --------------------------------
218
     1 = test passed, 0 = test failed
219

220
     Bit 0 = Accelerometer self test
221
     Bit 1 = Magnetometer self test
222
     Bit 2 = Gyroscope self test
223
     Bit 3 = MCU self test
224

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