Revision 5d8d461e

View differences:

Adafruit_BNO055.cpp
1
/***************************************************************************
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
  ------> https://www.adafruit.com/product/2472
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
 ***************************************************************************/
1
/*!
2
 * @file Adafruit_BNO055.cpp
3
 *
4
 *  @mainpage Adafruit BNO055 Orientation Sensor
5
 * 
6
 *  @section intro_sec Introduction
7
 *
8
 *    This is a library for the BNO055 orientation sensor
9
 *
10
 *    Designed specifically to work with the Adafruit BNO055 Breakout.
11
 *
12
 *    Pick one up today in the adafruit shop!
13
 *    ------> https://www.adafruit.com/product/2472
14
 *
15
 *    These sensors use I2C to communicate, 2 pins are required to interface.
16
 *
17
 *    Adafruit invests time and resources providing this open source code,
18
 *    please support Adafruit andopen-source hardware by purchasing products
19
 *    from Adafruit!
20
 *
21
 *  @section author Author
22
 *
23
 *  K.Townsend (Adafruit Industries)
24
 *
25
 *    @section license License
26
 *    MIT license, all text above must be included in any redistribution
27
 */
19 28

  
20 29
#if ARDUINO >= 100
21 30
#include "Arduino.h"
......
28 37

  
29 38
#include "Adafruit_BNO055.h"
30 39

  
31
/***************************************************************************
32
 CONSTRUCTOR
33
 ***************************************************************************/
34

  
35
/**************************************************************************/
36 40
/*!
37
    @brief  Instantiates a new Adafruit_BNO055 class
38
    @param  sensorID
39
    @param  address
40
*/
41
/**************************************************************************/
41
 *  @brief  Instantiates a new Adafruit_BNO055 class
42
 *  @param  sensorID
43
 *  @param  address
44
 */
42 45
Adafruit_BNO055::Adafruit_BNO055(int32_t sensorID, uint8_t address) {
43 46
  _sensorID = sensorID;
44 47
  _address = address;
45 48
}
46 49

  
47
/***************************************************************************
48
 PUBLIC FUNCTIONS
49
 ***************************************************************************/
50

  
51
/**************************************************************************/
52 50
/*!
53
    @brief  Sets up the HW
54
    @param  mode
55
    @return true if process is sucessfull
56
*/
57
/**************************************************************************/
51
 *  @brief  Sets up the HW
52
 *  @param  mode
53
 *  @return true if process is sucessfull
54
 */
58 55
bool Adafruit_BNO055::begin(adafruit_bno055_opmode_t mode) {
59 56
  /* Enable I2C */
60 57
  Wire.begin();
61 58

  
62
  // BNO055 clock stretches for 500us or more!
59
  /* BNO055 clock stretches for 500us or more! */
63 60
#ifdef ESP8266
64
  Wire.setClockStretchLimit(1000); // Allow for 1000us of clock stretching
61
  /* Allow for 1000us of clock stretching */
62
  Wire.setClockStretchLimit(1000); 
65 63
#endif
66 64

  
67 65
  /* Make sure we have the right device */
68 66
  uint8_t id = read8(BNO055_CHIP_ID_ADDR);
69 67
  if (id != BNO055_ID) {
70
    delay(1000); // hold on for boot
68
    /* hold on for boot */
69
    delay(1000); 
71 70
    id = read8(BNO055_CHIP_ID_ADDR);
72 71
    if (id != BNO055_ID) {
73
      return false; // still not? ok bail
72
      /* still not? ok bail */
73
      return false;
74 74
    }
75 75
  }
76 76

  
......
90 90

  
91 91
  write8(BNO055_PAGE_ID_ADDR, 0);
92 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
  */
93
  /* Set the output units 
94
     uint8_t unitsel = (0 << 7) | // Orientation = Android
95
                       (0 << 4) | // Temperature = Celsius
96
                       (0 << 2) | // Euler = Degrees
97
                       (1 << 1) | // Gyro = Rads
98
                       (0 << 0);  // Accelerometer = m/s^2
99
     write8(BNO055_UNIT_SEL_ADDR, unitsel);
100
   */
102 101

  
103 102
  /* 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 103
  write8(BNO055_SYS_TRIGGER_ADDR, 0x0);
112 104
  delay(10);
113 105
  /* Set the requested operating mode (see section 3.3) */
......
117 109
  return true;
118 110
}
119 111

  
120
/**************************************************************************/
121 112
/*!
122
    @brief  Puts the chip in the specified operating mode
123
    @param  mode
124
*/
125
/**************************************************************************/
113
 *  @brief  Puts the chip in the specified operating mode
114
 *  @param  mode
115
 */
126 116
void Adafruit_BNO055::setMode(adafruit_bno055_opmode_t mode) {
127 117
  _mode = mode;
128 118
  write8(BNO055_OPR_MODE_ADDR, _mode);
129 119
  delay(30);
130 120
}
131 121

  
132
/**************************************************************************/
133 122
/*!
134
    @brief  Changes the chip's axis remap
135
    @param  remapcode
136
*/
137
/**************************************************************************/
123
 *  @brief  Changes the chip's axis remap
124
 *  @param  remapcode
125
 */
138 126
void Adafruit_BNO055::setAxisRemap(
139 127
    adafruit_bno055_axis_remap_config_t remapcode) {
140 128
  adafruit_bno055_opmode_t modeback = _mode;
......
148 136
  delay(20);
149 137
}
150 138

  
151
/**************************************************************************/
152 139
/*!
153
    @brief  Changes the chip's axis signs
154
    @param  remapsign
155
*/
156
/**************************************************************************/
140
 *  @brief  Changes the chip's axis signs
141
 *  @param  remapsign
142
 */
157 143
void Adafruit_BNO055::setAxisSign(adafruit_bno055_axis_remap_sign_t remapsign) {
158 144
  adafruit_bno055_opmode_t modeback = _mode;
159 145

  
......
166 152
  delay(20);
167 153
}
168 154

  
169
/**************************************************************************/
170 155
/*!
171
    @brief  Use the external 32.768KHz crystal
172
    @param  usextal boolean
173
*/
174
/**************************************************************************/
156
 *  @brief  Use the external 32.768KHz crystal
157
 *  @param  usextal boolean
158
 */
175 159
void Adafruit_BNO055::setExtCrystalUse(boolean usextal) {
176 160
  adafruit_bno055_opmode_t modeback = _mode;
177 161

  
......
190 174
  delay(20);
191 175
}
192 176

  
193
/**************************************************************************/
194 177
/*!
195
    @brief  Gets the latest system status info
196
    @param  system_status
197
    @param  self_test_result
198
    @param  system_error
199
*/
200
/**************************************************************************/
178
 *   @brief  Gets the latest system status info
179
 *   @param  system_status
180
 *   @param  self_test_result
181
 *   @param  system_error
182
 */
201 183
void Adafruit_BNO055::getSystemStatus(uint8_t *system_status,
202 184
                                      uint8_t *self_test_result,
203 185
                                      uint8_t *system_error) {
204 186
  write8(BNO055_PAGE_ID_ADDR, 0);
205 187

  
206 188
  /* System Status (see section 4.3.58)
207
     ---------------------------------
208 189
     0 = Idle
209 190
     1 = System Error
210 191
     2 = Initializing Peripherals
211 192
     3 = System Iniitalization
212 193
     4 = Executing Self-Test
213 194
     5 = Sensor fusio algorithm running
214
     6 = System running without fusion algorithms */
195
     6 = System running without fusion algorithms 
196
   */
215 197

  
216 198
  if (system_status != 0)
217 199
    *system_status = read8(BNO055_SYS_STAT_ADDR);
218 200

  
219
  /* Self Test Results (see section )
220
     --------------------------------
201
  /* Self Test Results
221 202
     1 = test passed, 0 = test failed
222

  
203
   
223 204
     Bit 0 = Accelerometer self test
224 205
     Bit 1 = Magnetometer self test
225 206
     Bit 2 = Gyroscope self test
226 207
     Bit 3 = MCU self test
227

  
228
     0x0F = all good! */
208
   
209
     0x0F = all good! 
210
   */
229 211

  
230 212
  if (self_test_result != 0)
231 213
    *self_test_result = read8(BNO055_SELFTEST_RESULT_ADDR);
232 214

  
233 215
  /* System Error (see section 4.3.59)
234
     ---------------------------------
235 216
     0 = No error
236 217
     1 = Peripheral initialization error
237 218
     2 = System initialization error
......
242 223
     7 = BNO low power mode not available for selected operat ion mode
243 224
     8 = Accelerometer power mode not available
244 225
     9 = Fusion algorithm configuration error
245
     A = Sensor configuration error */
226
     A = Sensor configuration error 
227
   */
246 228

  
247 229
  if (system_error != 0)
248 230
    *system_error = read8(BNO055_SYS_ERR_ADDR);
......
250 232
  delay(200);
251 233
}
252 234

  
253
/**************************************************************************/
254 235
/*!
255
    @brief  Gets the chip revision numbers
256
*/
257
/**************************************************************************/
236
 *  @brief  Gets the chip revision numbers
237
 */
258 238
void Adafruit_BNO055::getRevInfo(adafruit_bno055_rev_info_t *info) {
259 239
  uint8_t a, b;
260 240

  
......
277 257
  info->sw_rev = (((uint16_t)b) << 8) | ((uint16_t)a);
278 258
}
279 259

  
280
/**************************************************************************/
281 260
/*!
282
    @brief  Gets current calibration state.  Each value should be a uint8_t
283
            pointer and it will be set to 0 if not calibrated and 3 if
284
            fully calibrated.
285
    @param  sys
286
    @param  gyro
287
    @param  accel
288
    @param  mag
289
*/
290
/**************************************************************************/
261
 *  @brief  Gets current calibration state.  Each value should be a uint8_t
262
 *          pointer and it will be set to 0 if not calibrated and 3 if
263
 *          fully calibrated.
264
 *  @param  sys
265
 *  @param  gyro
266
 *  @param  accel
267
 *  @param  mag
268
 */
291 269
void Adafruit_BNO055::getCalibration(uint8_t *sys, uint8_t *gyro,
292 270
                                     uint8_t *accel, uint8_t *mag) {
293 271
  uint8_t calData = read8(BNO055_CALIB_STAT_ADDR);
......
305 283
  }
306 284
}
307 285

  
308
/**************************************************************************/
309 286
/*!
310
    @brief  Gets the temperature in degrees celsius
311
    @return temperature in degrees celsius
312
*/
313
/**************************************************************************/
287
 *  @brief  Gets the temperature in degrees celsius
288
 *  @return temperature in degrees celsius
289
 */
314 290
int8_t Adafruit_BNO055::getTemp() {
315 291
  int8_t temp = (int8_t)(read8(BNO055_TEMP_ADDR));
316 292
  return temp;
317 293
}
318 294

  
319
/**************************************************************************/
320 295
/*!
321
    @brief   Gets a vector reading from the specified source
322
    @param   vector_type
323
    @return  vector from specified source
324
*/
325
/**************************************************************************/
296
 *  @brief   Gets a vector reading from the specified source
297
 *  @param   vector_type
298
 *  @return  vector from specified source
299
 */
326 300
imu::Vector<3> Adafruit_BNO055::getVector(adafruit_vector_type_t vector_type) {
327 301
  imu::Vector<3> xyz;
328 302
  uint8_t buffer[6];
......
338 312
  y = ((int16_t)buffer[2]) | (((int16_t)buffer[3]) << 8);
339 313
  z = ((int16_t)buffer[4]) | (((int16_t)buffer[5]) << 8);
340 314

  
341
  /* Convert the value to an appropriate range (section 3.6.4) */
342
  /* and assign the value to the Vector type */
315
  /*! 
316
   * Convert the value to an appropriate range (section 3.6.4)
317
   * and assign the value to the Vector type 
318
   */
343 319
  switch (vector_type) {
344 320
  case VECTOR_MAGNETOMETER:
345 321
    /* 1uT = 16 LSB */
......
372 348
  return xyz;
373 349
}
374 350

  
375
/**************************************************************************/
376 351
/*!
377
    @brief  Gets a quaternion reading from the specified source
378
    @return quaternion reading
379
*/
380
/**************************************************************************/
352
 *  @brief  Gets a quaternion reading from the specified source
353
 *  @return quaternion reading
354
 */
381 355
imu::Quaternion Adafruit_BNO055::getQuat() {
382 356
  uint8_t buffer[8];
383 357
  memset(buffer, 0, 8);
......
392 366
  y = (((uint16_t)buffer[5]) << 8) | ((uint16_t)buffer[4]);
393 367
  z = (((uint16_t)buffer[7]) << 8) | ((uint16_t)buffer[6]);
394 368

  
395
  /* Assign to Quaternion */
396
  /* See
397
     http://ae-bst.resource.bosch.com/media/products/dokumente/bno055/BST_BNO055_DS000_12~1.pdf
398
     3.6.5.5 Orientation (Quaternion)  */
369
  /*!
370
   * Assign to Quaternion
371
   * See
372
   * http://ae-bst.resource.bosch.com/media/products/dokumente/bno055/BST_BNO055_DS000_12~1.pdf
373
   * 3.6.5.5 Orientation (Quaternion)  
374
   */
399 375
  const double scale = (1.0 / (1 << 14));
400 376
  imu::Quaternion quat(scale * w, scale * x, scale * y, scale * z);
401 377
  return quat;
402 378
}
403 379

  
404
/**************************************************************************/
405 380
/*!
406
    @brief  Provides the sensor_t data for this sensor
407
    @param  sensor
381
 *  @brief  Provides the sensor_t data for this sensor
382
 *  @param  sensor
408 383
*/
409
/**************************************************************************/
410 384
void Adafruit_BNO055::getSensor(sensor_t *sensor) {
411 385
  /* Clear the sensor_t object */
412 386
  memset(sensor, 0, sizeof(sensor_t));
......
423 397
  sensor->resolution = 0.01F;
424 398
}
425 399

  
426
/**************************************************************************/
427 400
/*!
428
    @brief  Reads the sensor and returns the data as a sensors_event_t
429
    @param  event
430
    @return always returns true
431
*/
432
/**************************************************************************/
401
 *  @brief  Reads the sensor and returns the data as a sensors_event_t
402
 *  @param  event
403
 *  @return always returns true
404
 */
433 405
bool Adafruit_BNO055::getEvent(sensors_event_t *event) {
434 406
  /* Clear the event */
435 407
  memset(event, 0, sizeof(sensors_event_t));
......
448 420
  return true;
449 421
}
450 422

  
451
/**************************************************************************/
452 423
/*!
453
    @brief  Reads the sensor's offset registers into a byte array
454
    @param  calibData
455
    @return true if read is successful
456
*/
457
/**************************************************************************/
424
 *  @brief  Reads the sensor's offset registers into a byte array
425
 *  @param  calibData
426
 *  @return true if read is successful
427
 */
458 428
bool Adafruit_BNO055::getSensorOffsets(uint8_t *calibData) {
459 429
  if (isFullyCalibrated()) {
460 430
    adafruit_bno055_opmode_t lastMode = _mode;
......
468 438
  return false;
469 439
}
470 440

  
471
/**************************************************************************/
472 441
/*!
473
    @brief  Reads the sensor's offset registers into an offset struct
474
    @param  offsets_type
475
    @return true if read is successful
476
*/
477
/**************************************************************************/
442
 *  @brief  Reads the sensor's offset registers into an offset struct
443
 *  @param  offsets_type
444
 *  @return true if read is successful
445
 */
478 446
bool Adafruit_BNO055::getSensorOffsets(
479 447
    adafruit_bno055_offsets_t &offsets_type) {
480 448
  if (isFullyCalibrated()) {
......
530 498
  return false;
531 499
}
532 500

  
533
/**************************************************************************/
534 501
/*!
535
  @brief  Writes an array of calibration values to the sensor's offset registers
536
  @param  calibData
537
*/
538
/**************************************************************************/
502
 *  @brief  Writes an array of calibration values to the sensor's offset registers
503
 *  @param  calibData
504
 */
539 505
void Adafruit_BNO055::setSensorOffsets(const uint8_t *calibData) {
540 506
  adafruit_bno055_opmode_t lastMode = _mode;
541 507
  setMode(OPERATION_MODE_CONFIG);
......
577 543
  setMode(lastMode);
578 544
}
579 545

  
580
/**************************************************************************/
581 546
/*!
582
@brief  Writes to the sensor's offset registers from an offset struct
583
@param  offsets_type
547
 *  @brief  Writes to the sensor's offset registers from an offset struct
548
 *  @param  offsets_type
584 549
*/
585
/**************************************************************************/
586 550
void Adafruit_BNO055::setSensorOffsets(
587 551
    const adafruit_bno055_offsets_t &offsets_type) {
588 552
  adafruit_bno055_opmode_t lastMode = _mode;
......
624 588
  setMode(lastMode);
625 589
}
626 590

  
627
/**************************************************************************/
628 591
/*!
629
    @brief  Checks of all cal status values are set to 3 (fully calibrated)
630
    @return status of calibration
631
*/
632
/**************************************************************************/
592
 *  @brief  Checks of all cal status values are set to 3 (fully calibrated)
593
 *  @return status of calibration
594
 */
633 595
bool Adafruit_BNO055::isFullyCalibrated() {
634 596
  uint8_t system, gyro, accel, mag;
635 597
  getCalibration(&system, &gyro, &accel, &mag);
......
638 600
  return true;
639 601
}
640 602

  
641
/***************************************************************************
642
 PRIVATE FUNCTIONS
643
 ***************************************************************************/
644

  
645
/**************************************************************************/
646 603
/*!
647
    @brief  Writes an 8 bit value over I2C
648
*/
649
/**************************************************************************/
604
 *  @brief  Writes an 8 bit value over I2C
605
 */
650 606
bool Adafruit_BNO055::write8(adafruit_bno055_reg_t reg, byte value) {
651 607
  Wire.beginTransmission(_address);
652 608
#if ARDUINO >= 100
......
662 618
  return true;
663 619
}
664 620

  
665
/**************************************************************************/
666 621
/*!
667
    @brief  Reads an 8 bit value over I2C
668
*/
669
/**************************************************************************/
622
 *  @brief  Reads an 8 bit value over I2C
623
 */
670 624
byte Adafruit_BNO055::read8(adafruit_bno055_reg_t reg) {
671 625
  byte value = 0;
672 626

  
......
687 641
  return value;
688 642
}
689 643

  
690
/**************************************************************************/
691 644
/*!
692
    @brief  Reads the specified number of bytes over I2C
693
*/
694
/**************************************************************************/
645
 *  @brief  Reads the specified number of bytes over I2C
646
 */
695 647
bool Adafruit_BNO055::readLen(adafruit_bno055_reg_t reg, byte *buffer,
696 648
                              uint8_t len) {
697 649
  Wire.beginTransmission(_address);
Adafruit_BNO055.h
1 1
/*!
2
 *  @file Adafruit_BNO055.h
2 3
 *
3
 *  This is a library for the BNO055 orientation sensor
4
 *    This is a library for the BNO055 orientation sensor
4 5
 *
5
 *  Designed specifically to work with the Adafruit BNO055 Breakout.
6
 *    Designed specifically to work with the Adafruit BNO055 Breakout.
6 7
 *
7
 *  Pick one up today in the adafruit shop!
8
 *  ------> https://www.adafruit.com/product/2472
8
 *    Pick one up today in the adafruit shop!
9
 *    ------> https://www.adafruit.com/product/2472
9 10
 *
10
 *  These sensors use I2C to communicate, 2 pins are required to interface.
11
 *    These sensors use I2C to communicate, 2 pins are required to interface.
11 12
 *
12
 *  Adafruit invests time and resources providing this open source code,
13
 *  please support Adafruit andopen-source hardware by purchasing products
14
 *  from Adafruit!
13
 *    Adafruit invests time and resources providing this open source code,
14
 *    please support Adafruit andopen-source hardware by purchasing products
15
 *    from Adafruit!
15 16
 *
16
 *  Written by KTOWN for Adafruit Industries.
17
 *    K.Townsend (Adafruit Industries)*
17 18
 *
18
 *  MIT license, all text above must be included in any redistribution
19
 *    MIT license, all text above must be included in any redistribution
19 20
 */
20 21

  
21 22
#ifndef __ADAFRUIT_BNO055_H__
......
37 38
#include <Adafruit_Sensor.h>
38 39
#include <utility/imumaths.h>
39 40

  
41
/** BNO055 Address A **/
40 42
#define BNO055_ADDRESS_A (0x28)
43
/** BNO055 Address B **/
41 44
#define BNO055_ADDRESS_B (0x29)
45
/** BNO055 ID **/
42 46
#define BNO055_ID (0xA0)
43 47

  
48
/** Offsets registers **/
44 49
#define NUM_BNO055_OFFSET_REGISTERS (22)
45 50

  
46 51
/** A structure to represent offsets **/

Also available in: Unified diff