Revision 312a5b9e

View differences:

Adafruit_BNO055.cpp
15 15
  Written by KTOWN for Adafruit Industries.
16 16

  
17 17
  MIT license, all text above must be included in any redistribution
18
 ***************************************************************************/
18
  ***************************************************************************/
19 19

  
20 20
#if ARDUINO >= 100
21
 #include "Arduino.h"
21
#include "Arduino.h"
22 22
#else
23
 #include "WProgram.h"
23
#include "WProgram.h"
24 24
#endif
25 25

  
26 26
#include <math.h>
......
34 34

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

  
46 46
/***************************************************************************
......
49 49

  
50 50
/**************************************************************************/
51 51
/*!
52
    @brief  Sets up the HW
53
*/
52
	@brief  Sets up the HW
53
	*/
54 54
/**************************************************************************/
55 55
bool Adafruit_BNO055::begin(adafruit_bno055_opmode_t mode)
56 56
{
57
  /* 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
  write8(BNO055_SYS_TRIGGER_ADDR, 0x0);
99
  delay(10);
100
  /* Set the requested operating mode (see section 3.3) */
101
  setMode(mode);
102
  delay(20);
103

  
104
  return true;
57
	/* 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
	write8(BNO055_SYS_TRIGGER_ADDR, 0x0);
99
	delay(10);
100
	/* Set the requested operating mode (see section 3.3) */
101
	setMode(mode);
102
	delay(20);
103

  
104
	return true;
105 105
}
106 106

  
107 107
/**************************************************************************/
108 108
/*!
109
    @brief  Puts the chip in the specified operating mode
110
*/
109
	@brief  Puts the chip in the specified operating mode
110
	*/
111 111
/**************************************************************************/
112 112
void Adafruit_BNO055::setMode(adafruit_bno055_opmode_t mode)
113 113
{
114
  _mode = mode;
115
  write8(BNO055_OPR_MODE_ADDR, _mode);
116
  delay(30);
114
	_mode = mode;
115
	write8(BNO055_OPR_MODE_ADDR, _mode);
116
	delay(30);
117 117
}
118 118

  
119 119
/**************************************************************************/
120 120
/*!
121
    @brief  Use the external 32.768KHz crystal
122
*/
121
	@brief  Use the external 32.768KHz crystal
122
	*/
123 123
/**************************************************************************/
124 124
void Adafruit_BNO055::setExtCrystalUse(boolean usextal)
125 125
{
126
  adafruit_bno055_opmode_t modeback = _mode;
127

  
128
  /* Switch to config mode (just in case since this is the default) */
129
  setMode(OPERATION_MODE_CONFIG);
130
  delay(25);
131
  write8(BNO055_PAGE_ID_ADDR, 0);
132
  if (usextal) {
133
    write8(BNO055_SYS_TRIGGER_ADDR, 0x80);
134
  } else {
135
    write8(BNO055_SYS_TRIGGER_ADDR, 0x00);
136
  }
137
  delay(10);
138
  /* Set the requested operating mode (see section 3.3) */
139
  setMode(modeback);
140
  delay(20);
126
	adafruit_bno055_opmode_t modeback = _mode;
127

  
128
	/* Switch to config mode (just in case since this is the default) */
129
	setMode(OPERATION_MODE_CONFIG);
130
	delay(25);
131
	write8(BNO055_PAGE_ID_ADDR, 0);
132
	if (usextal) {
133
		write8(BNO055_SYS_TRIGGER_ADDR, 0x80);
134
	}
135
	else {
136
		write8(BNO055_SYS_TRIGGER_ADDR, 0x00);
137
	}
138
	delay(10);
139
	/* Set the requested operating mode (see section 3.3) */
140
	setMode(modeback);
141
	delay(20);
141 142
}
142 143

  
143 144

  
144 145
/**************************************************************************/
145 146
/*!
146
    @brief  Gets the latest system status info
147
*/
147
	@brief  Gets the latest system status info
148
	*/
148 149
/**************************************************************************/
149 150
void Adafruit_BNO055::getSystemStatus(uint8_t *system_status, uint8_t *self_test_result, uint8_t *system_error)
150 151
{
151
  write8(BNO055_PAGE_ID_ADDR, 0);
152

  
153
  /* System Status (see section 4.3.58)
154
     ---------------------------------
155
     0 = Idle
156
     1 = System Error
157
     2 = Initializing Peripherals
158
     3 = System Iniitalization
159
     4 = Executing Self-Test
160
     5 = Sensor fusio algorithm running
161
     6 = System running without fusion algorithms */
162

  
163
  if (system_status != 0)
164
    *system_status    = read8(BNO055_SYS_STAT_ADDR);
165

  
166
  /* Self Test Results (see section )
167
     --------------------------------
168
     1 = test passed, 0 = test failed
169

  
170
     Bit 0 = Accelerometer self test
171
     Bit 1 = Magnetometer self test
172
     Bit 2 = Gyroscope self test
173
     Bit 3 = MCU self test
174

  
175
     0x0F = all good! */
176

  
177
  if (self_test_result != 0)
178
    *self_test_result = read8(BNO055_SELFTEST_RESULT_ADDR);
179

  
180
  /* System Error (see section 4.3.59)
181
     ---------------------------------
182
     0 = No error
183
     1 = Peripheral initialization error
184
     2 = System initialization error
185
     3 = Self test result failed
186
     4 = Register map value out of range
187
     5 = Register map address out of range
188
     6 = Register map write error
189
     7 = BNO low power mode not available for selected operat ion mode
190
     8 = Accelerometer power mode not available
191
     9 = Fusion algorithm configuration error
192
     A = Sensor configuration error */
193

  
194
  if (system_error != 0)
195
    *system_error     = read8(BNO055_SYS_ERR_ADDR);
196

  
197
  delay(200);
152
	write8(BNO055_PAGE_ID_ADDR, 0);
153

  
154
	/* System Status (see section 4.3.58)
155
	   ---------------------------------
156
	   0 = Idle
157
	   1 = System Error
158
	   2 = Initializing Peripherals
159
	   3 = System Iniitalization
160
	   4 = Executing Self-Test
161
	   5 = Sensor fusio algorithm running
162
	   6 = System running without fusion algorithms */
163

  
164
	if (system_status != 0)
165
		*system_status = read8(BNO055_SYS_STAT_ADDR);
166

  
167
	/* Self Test Results (see section )
168
	   --------------------------------
169
	   1 = test passed, 0 = test failed
170

  
171
	   Bit 0 = Accelerometer self test
172
	   Bit 1 = Magnetometer self test
173
	   Bit 2 = Gyroscope self test
174
	   Bit 3 = MCU self test
175

  
176
	   0x0F = all good! */
177

  
178
	if (self_test_result != 0)
179
		*self_test_result = read8(BNO055_SELFTEST_RESULT_ADDR);
180

  
181
	/* System Error (see section 4.3.59)
182
	   ---------------------------------
183
	   0 = No error
184
	   1 = Peripheral initialization error
185
	   2 = System initialization error
186
	   3 = Self test result failed
187
	   4 = Register map value out of range
188
	   5 = Register map address out of range
189
	   6 = Register map write error
190
	   7 = BNO low power mode not available for selected operat ion mode
191
	   8 = Accelerometer power mode not available
192
	   9 = Fusion algorithm configuration error
193
	   A = Sensor configuration error */
194

  
195
	if (system_error != 0)
196
		*system_error = read8(BNO055_SYS_ERR_ADDR);
197

  
198
	delay(200);
198 199
}
199 200

  
200 201
/**************************************************************************/
201 202
/*!
202
    @brief  Gets the chip revision numbers
203
*/
203
	@brief  Gets the chip revision numbers
204
	*/
204 205
/**************************************************************************/
205 206
void Adafruit_BNO055::getRevInfo(adafruit_bno055_rev_info_t* info)
206 207
{
207
  uint8_t a, b;
208
	uint8_t a, b;
208 209

  
209
  memset(info, 0, sizeof(adafruit_bno055_rev_info_t));
210
	memset(info, 0, sizeof(adafruit_bno055_rev_info_t));
210 211

  
211
  /* Check the accelerometer revision */
212
  info->accel_rev = read8(BNO055_ACCEL_REV_ID_ADDR);
212
	/* Check the accelerometer revision */
213
	info->accel_rev = read8(BNO055_ACCEL_REV_ID_ADDR);
213 214

  
214
  /* Check the magnetometer revision */
215
  info->mag_rev   = read8(BNO055_MAG_REV_ID_ADDR);
215
	/* Check the magnetometer revision */
216
	info->mag_rev = read8(BNO055_MAG_REV_ID_ADDR);
216 217

  
217
  /* Check the gyroscope revision */
218
  info->gyro_rev  = read8(BNO055_GYRO_REV_ID_ADDR);
218
	/* Check the gyroscope revision */
219
	info->gyro_rev = read8(BNO055_GYRO_REV_ID_ADDR);
219 220

  
220
  /* Check the SW revision */
221
  info->bl_rev    = read8(BNO055_BL_REV_ID_ADDR);
221
	/* Check the SW revision */
222
	info->bl_rev = read8(BNO055_BL_REV_ID_ADDR);
222 223

  
223
  a = read8(BNO055_SW_REV_ID_LSB_ADDR);
224
  b = read8(BNO055_SW_REV_ID_MSB_ADDR);
225
  info->sw_rev = (((uint16_t)b) << 8) | ((uint16_t)a);
224
	a = read8(BNO055_SW_REV_ID_LSB_ADDR);
225
	b = read8(BNO055_SW_REV_ID_MSB_ADDR);
226
	info->sw_rev = (((uint16_t)b) << 8) | ((uint16_t)a);
226 227
}
227 228

  
228 229
/**************************************************************************/
229 230
/*!
230
    @brief  Gets current calibration state.  Each value should be a uint8_t
231
            pointer and it will be set to 0 if not calibrated and 3 if
232
            fully calibrated.
233
*/
231
	@brief  Gets current calibration state.  Each value should be a uint8_t
232
	pointer and it will be set to 0 if not calibrated and 3 if
233
	fully calibrated.
234
	*/
234 235
/**************************************************************************/
235 236
void Adafruit_BNO055::getCalibration(uint8_t* sys, uint8_t* gyro, uint8_t* accel, uint8_t* mag) {
236
  uint8_t calData = read8(BNO055_CALIB_STAT_ADDR);
237
  if (sys != NULL) {
238
    *sys = (calData >> 6) & 0x03;
239
  }
240
  if (gyro != NULL) {
241
    *gyro = (calData >> 4) & 0x03;
242
  }
243
  if (accel != NULL) {
244
    *accel = (calData >> 2) & 0x03;
245
  }
246
  if (mag != NULL) {
247
    *mag = calData & 0x03;
248
  }
237
	uint8_t calData = read8(BNO055_CALIB_STAT_ADDR);
238
	if (sys != NULL) {
239
		*sys = (calData >> 6) & 0x03;
240
	}
241
	if (gyro != NULL) {
242
		*gyro = (calData >> 4) & 0x03;
243
	}
244
	if (accel != NULL) {
245
		*accel = (calData >> 2) & 0x03;
246
	}
247
	if (mag != NULL) {
248
		*mag = calData & 0x03;
249
	}
249 250
}
250 251

  
251 252
/**************************************************************************/
252 253
/*!
253
    @brief  Gets the temperature in degrees celsius
254
*/
254
	@brief  Gets the temperature in degrees celsius
255
	*/
255 256
/**************************************************************************/
256 257
int8_t Adafruit_BNO055::getTemp(void)
257 258
{
258
  int8_t temp = (int8_t)(read8(BNO055_TEMP_ADDR));
259
  return temp;
259
	int8_t temp = (int8_t)(read8(BNO055_TEMP_ADDR));
260
	return temp;
260 261
}
261 262

  
262 263
/**************************************************************************/
263 264
/*!
264
    @brief  Gets a vector reading from the specified source
265
*/
265
	@brief  Gets a vector reading from the specified source
266
	*/
266 267
/**************************************************************************/
267 268
imu::Vector<3> Adafruit_BNO055::getVector(adafruit_vector_type_t vector_type)
268 269
{
269
  imu::Vector<3> xyz;
270
  uint8_t buffer[6];
271
  memset (buffer, 0, 6);
272

  
273
  int16_t x, y, z;
274
  x = y = z = 0;
275

  
276
  /* Read vector data (6 bytes) */
277
  readLen((adafruit_bno055_reg_t)vector_type, buffer, 6);
278

  
279
  x = ((int16_t)buffer[0]) | (((int16_t)buffer[1]) << 8);
280
  y = ((int16_t)buffer[2]) | (((int16_t)buffer[3]) << 8);
281
  z = ((int16_t)buffer[4]) | (((int16_t)buffer[5]) << 8);
282

  
283
  /* Convert the value to an appropriate range (section 3.6.4) */
284
  /* and assign the value to the Vector type */
285
  switch(vector_type)
286
  {
287
    case VECTOR_MAGNETOMETER:
288
      /* 1uT = 16 LSB */
289
      xyz[0] = ((double)x)/16.0;
290
      xyz[1] = ((double)y)/16.0;
291
      xyz[2] = ((double)z)/16.0;
292
      break;
293
    case VECTOR_GYROSCOPE:
294
      /* 1rps = 900 LSB */
295
      xyz[0] = ((double)x)/900.0;
296
      xyz[1] = ((double)y)/900.0;
297
      xyz[2] = ((double)z)/900.0;
298
      break;
299
    case VECTOR_EULER:
300
      /* 1 degree = 16 LSB */
301
      xyz[0] = ((double)x)/16.0;
302
      xyz[1] = ((double)y)/16.0;
303
      xyz[2] = ((double)z)/16.0;
304
      break;
305
    case VECTOR_ACCELEROMETER:
306
    case VECTOR_LINEARACCEL:
307
    case VECTOR_GRAVITY:
308
      /* 1m/s^2 = 100 LSB */
309
      xyz[0] = ((double)x)/100.0;
310
      xyz[1] = ((double)y)/100.0;
311
      xyz[2] = ((double)z)/100.0;
312
      break;
313
  }
314

  
315
  return xyz;
270
	imu::Vector<3> xyz;
271
	uint8_t buffer[6];
272
	memset(buffer, 0, 6);
273

  
274
	int16_t x, y, z;
275
	x = y = z = 0;
276

  
277
	/* Read vector data (6 bytes) */
278
	readLen((adafruit_bno055_reg_t)vector_type, buffer, 6);
279

  
280
	x = ((int16_t)buffer[0]) | (((int16_t)buffer[1]) << 8);
281
	y = ((int16_t)buffer[2]) | (((int16_t)buffer[3]) << 8);
282
	z = ((int16_t)buffer[4]) | (((int16_t)buffer[5]) << 8);
283

  
284
	/* Convert the value to an appropriate range (section 3.6.4) */
285
	/* and assign the value to the Vector type */
286
	switch (vector_type)
287
	{
288
	case VECTOR_MAGNETOMETER:
289
		/* 1uT = 16 LSB */
290
		xyz[0] = ((double)x) / 16.0;
291
		xyz[1] = ((double)y) / 16.0;
292
		xyz[2] = ((double)z) / 16.0;
293
		break;
294
	case VECTOR_GYROSCOPE:
295
		/* 1rps = 900 LSB */
296
		xyz[0] = ((double)x) / 900.0;
297
		xyz[1] = ((double)y) / 900.0;
298
		xyz[2] = ((double)z) / 900.0;
299
		break;
300
	case VECTOR_EULER:
301
		/* 1 degree = 16 LSB */
302
		xyz[0] = ((double)x) / 16.0;
303
		xyz[1] = ((double)y) / 16.0;
304
		xyz[2] = ((double)z) / 16.0;
305
		break;
306
	case VECTOR_ACCELEROMETER:
307
	case VECTOR_LINEARACCEL:
308
	case VECTOR_GRAVITY:
309
		/* 1m/s^2 = 100 LSB */
310
		xyz[0] = ((double)x) / 100.0;
311
		xyz[1] = ((double)y) / 100.0;
312
		xyz[2] = ((double)z) / 100.0;
313
		break;
314
	}
315

  
316
	return xyz;
316 317
}
317 318

  
318 319
/**************************************************************************/
319 320
/*!
320
    @brief  Gets a quaternion reading from the specified source
321
*/
321
	@brief  Gets a quaternion reading from the specified source
322
	*/
322 323
/**************************************************************************/
323 324
imu::Quaternion Adafruit_BNO055::getQuat(void)
324 325
{
325
  uint8_t buffer[8];
326
  memset (buffer, 0, 8);
327

  
328
  int16_t x, y, z, w;
329
  x = y = z = w = 0;
330

  
331
  /* Read quat data (8 bytes) */
332
  readLen(BNO055_QUATERNION_DATA_W_LSB_ADDR, buffer, 8);
333
  w = (((uint16_t)buffer[1]) << 8) | ((uint16_t)buffer[0]);
334
  x = (((uint16_t)buffer[3]) << 8) | ((uint16_t)buffer[2]);
335
  y = (((uint16_t)buffer[5]) << 8) | ((uint16_t)buffer[4]);
336
  z = (((uint16_t)buffer[7]) << 8) | ((uint16_t)buffer[6]);
337

  
338
  /* Assign to Quaternion */
339
  /* See http://ae-bst.resource.bosch.com/media/products/dokumente/bno055/BST_BNO055_DS000_12~1.pdf
340
     3.6.5.5 Orientation (Quaternion)  */
341
  const double scale = (1.0 / (1<<14));
342
  imu::Quaternion quat(scale * w, scale * x, scale * y, scale * z);
343
  return quat;
326
	uint8_t buffer[8];
327
	memset(buffer, 0, 8);
328

  
329
	int16_t x, y, z, w;
330
	x = y = z = w = 0;
331

  
332
	/* Read quat data (8 bytes) */
333
	readLen(BNO055_QUATERNION_DATA_W_LSB_ADDR, buffer, 8);
334
	w = (((uint16_t)buffer[1]) << 8) | ((uint16_t)buffer[0]);
335
	x = (((uint16_t)buffer[3]) << 8) | ((uint16_t)buffer[2]);
336
	y = (((uint16_t)buffer[5]) << 8) | ((uint16_t)buffer[4]);
337
	z = (((uint16_t)buffer[7]) << 8) | ((uint16_t)buffer[6]);
338

  
339
	/* Assign to Quaternion */
340
	/* See http://ae-bst.resource.bosch.com/media/products/dokumente/bno055/BST_BNO055_DS000_12~1.pdf
341
	   3.6.5.5 Orientation (Quaternion)  */
342
	const double scale = (1.0 / (1 << 14));
343
	imu::Quaternion quat(scale * w, scale * x, scale * y, scale * z);
344
	return quat;
344 345
}
345 346

  
346 347
/**************************************************************************/
347 348
/*!
348
    @brief  Provides the sensor_t data for this sensor
349
*/
349
	@brief  Provides the sensor_t data for this sensor
350
	*/
350 351
/**************************************************************************/
351 352
void Adafruit_BNO055::getSensor(sensor_t *sensor)
352 353
{
353
  /* Clear the sensor_t object */
354
  memset(sensor, 0, sizeof(sensor_t));
355

  
356
  /* Insert the sensor name in the fixed length char array */
357
  strncpy (sensor->name, "BNO055", sizeof(sensor->name) - 1);
358
  sensor->name[sizeof(sensor->name)- 1] = 0;
359
  sensor->version     = 1;
360
  sensor->sensor_id   = _sensorID;
361
  sensor->type        = SENSOR_TYPE_ORIENTATION;
362
  sensor->min_delay   = 0;
363
  sensor->max_value   = 0.0F;
364
  sensor->min_value   = 0.0F;
365
  sensor->resolution  = 0.01F;
354
	/* Clear the sensor_t object */
355
	memset(sensor, 0, sizeof(sensor_t));
356

  
357
	/* Insert the sensor name in the fixed length char array */
358
	strncpy(sensor->name, "BNO055", sizeof(sensor->name) - 1);
359
	sensor->name[sizeof(sensor->name) - 1] = 0;
360
	sensor->version = 1;
361
	sensor->sensor_id = _sensorID;
362
	sensor->type = SENSOR_TYPE_ORIENTATION;
363
	sensor->min_delay = 0;
364
	sensor->max_value = 0.0F;
365
	sensor->min_value = 0.0F;
366
	sensor->resolution = 0.01F;
366 367
}
367 368

  
368 369
/**************************************************************************/
369 370
/*!
370
    @brief  Reads the sensor and returns the data as a sensors_event_t
371
*/
371
	@brief  Reads the sensor and returns the data as a sensors_event_t
372
	*/
372 373
/**************************************************************************/
373 374
bool Adafruit_BNO055::getEvent(sensors_event_t *event)
374 375
{
375
  /* Clear the event */
376
  memset(event, 0, sizeof(sensors_event_t));
376
	/* Clear the event */
377
	memset(event, 0, sizeof(sensors_event_t));
377 378

  
378
  event->version   = sizeof(sensors_event_t);
379
  event->sensor_id = _sensorID;
380
  event->type      = SENSOR_TYPE_ORIENTATION;
381
  event->timestamp = millis();
379
	event->version = sizeof(sensors_event_t);
380
	event->sensor_id = _sensorID;
381
	event->type = SENSOR_TYPE_ORIENTATION;
382
	event->timestamp = millis();
382 383

  
383
  /* Get a Euler angle sample for orientation */
384
  imu::Vector<3> euler = getVector(Adafruit_BNO055::VECTOR_EULER);
385
  event->orientation.x = euler.x();
386
  event->orientation.y = euler.y();
387
  event->orientation.z = euler.z();
384
	/* Get a Euler angle sample for orientation */
385
	imu::Vector<3> euler = getVector(Adafruit_BNO055::VECTOR_EULER);
386
	event->orientation.x = euler.x();
387
	event->orientation.y = euler.y();
388
	event->orientation.z = euler.z();
389

  
390
	return true;
391
}
388 392

  
389
  return true;
393
/**************************************************************************/
394
/*!
395
@brief  Reads the sensor's offset registers into a byte array
396
*/
397
/**************************************************************************/
398
bool Adafruit_BNO055::getSensorOffsets(int8_t* calibData)
399
{
400
	if (isFullyCalibrated())
401
	{
402
		adafruit_bno055_opmode_t lastMode = _mode;
403
		setMode(OPERATION_MODE_CONFIG);
404

  
405
		readLen(ACCEL_OFFSET_X_LSB_ADDR, calibData, NUM_BNO055_OFFSET_REGISTERS);
406

  
407
		setMode(lastMode);
408
		return true;
409
	}
410
	return false;
411
}
412

  
413
/**************************************************************************/
414
/*!
415
@brief  Reads the sensor's offset registers into an offset struct
416
*/
417
/**************************************************************************/
418
bool Adafruit_BNO055::getSensorOffsets(adafruit_bno055_offsets_t &offsets_type)
419
{
420
	if (isFullyCalibrated())
421
	{
422
		adafruit_bno055_opmode_t lastMode = _mode;
423
		setMode(OPERATION_MODE_CONFIG);
424
		delay(25);
425

  
426
		offsets_type.accel_offset_x = (read8(ACCEL_OFFSET_X_MSB_ADDR) << 8) | (read8(ACCEL_OFFSET_X_LSB_ADDR));
427
		offsets_type.accel_offset_y = (read8(ACCEL_OFFSET_Y_MSB_ADDR) << 8) | (read8(ACCEL_OFFSET_Y_LSB_ADDR));
428
		offsets_type.accel_offset_z = (read8(ACCEL_OFFSET_Z_MSB_ADDR) << 8) | (read8(ACCEL_OFFSET_Z_LSB_ADDR));
429

  
430
		offsets_type.gyro_offset_x = (read8(GYRO_OFFSET_X_MSB_ADDR) << 8) | (read8(GYRO_OFFSET_X_LSB_ADDR));
431
		offsets_type.gyro_offset_y = (read8(GYRO_OFFSET_Y_MSB_ADDR) << 8) | (read8(GYRO_OFFSET_Y_LSB_ADDR));
432
		offsets_type.gyro_offset_z = (read8(GYRO_OFFSET_Z_MSB_ADDR) << 8) | (read8(GYRO_OFFSET_Z_LSB_ADDR));
433

  
434
		offsets_type.mag_offset_x = (read8(MAG_OFFSET_X_MSB_ADDR) << 8) | (read8(MAG_OFFSET_X_LSB_ADDR));
435
		offsets_type.mag_offset_y = (read8(MAG_OFFSET_Y_MSB_ADDR) << 8) | (read8(MAG_OFFSET_Y_LSB_ADDR));
436
		offsets_type.mag_offset_z = (read8(MAG_OFFSET_Z_MSB_ADDR) << 8) | (read8(MAG_OFFSET_Z_LSB_ADDR));
437

  
438
		offsets_type.accel_radius = (read8(ACCEL_RADIUS_MSB_ADDR) << 8) | (read8(ACCEL_RADIUS_LSB_ADDR));
439
		offsets_type.mag_radius = (read8(MAG_RADIUS_MSB_ADDR) << 8) | (read8(MAG_RADIUS_LSB_ADDR));
440

  
441
		setMode(lastMode);
442
		return true;
443
	}
444
	return false;
445
}
446

  
447

  
448
/**************************************************************************/
449
/*!
450
@brief  Writes an array of calibration values to the sensor's offset registers
451
*/
452
/**************************************************************************/
453
void Adafruit_BNO055::setSensorOffsets(const int8_t* calibData)
454
{
455
	adafruit_bno055_opmode_t lastMode = _mode;
456
	setMode(OPERATION_MODE_CONFIG);
457
	delay(25);
458

  
459
	/* A writeLen() would make this much cleaner */
460
	write8(ACCEL_OFFSET_X_LSB_ADDR, calibData[0]);
461
	write8(ACCEL_OFFSET_X_MSB_ADDR, calibData[1]);
462
	write8(ACCEL_OFFSET_Y_LSB_ADDR, calibData[2]);
463
	write8(ACCEL_OFFSET_Y_MSB_ADDR, calibData[3]);
464
	write8(ACCEL_OFFSET_Z_LSB_ADDR, calibData[4]);
465
	write8(ACCEL_OFFSET_Z_MSB_ADDR, calibData[5]);
466

  
467
	write8(GYRO_OFFSET_X_LSB_ADDR, calibData[6]);
468
	write8(GYRO_OFFSET_X_MSB_ADDR, calibData[7]);
469
	write8(GYRO_OFFSET_Y_LSB_ADDR, calibData[8]);
470
	write8(GYRO_OFFSET_Y_MSB_ADDR, calibData[9]);
471
	write8(GYRO_OFFSET_Z_LSB_ADDR, calibData[10]);
472
	write8(GYRO_OFFSET_Z_MSB_ADDR, calibData[11]);
473

  
474
	write8(MAG_OFFSET_X_LSB_ADDR, calibData[12]);
475
	write8(MAG_OFFSET_X_MSB_ADDR, calibData[13]);
476
	write8(MAG_OFFSET_Y_LSB_ADDR, calibData[14]);
477
	write8(MAG_OFFSET_Y_MSB_ADDR, calibData[15]);
478
	write8(MAG_OFFSET_Z_LSB_ADDR, calibData[16]);
479
	write8(MAG_OFFSET_Z_MSB_ADDR, calibData[17]);
480

  
481
	write8(ACCEL_RADIUS_LSB_ADDR, calibData[18]);
482
	write8(ACCEL_RADIUS_MSB_ADDR, calibData[19]);
483

  
484
	write8(MAG_RADIUS_LSB_ADDR, calibData[20]);
485
	write8(MAG_RADIUS_MSB_ADDR, calibData[21]);
486

  
487
	setMode(lastMode);
390 488
}
391 489

  
490
/**************************************************************************/
491
/*!
492
@brief  Writes to the sensor's offset registers from an offset struct
493
*/
494
/**************************************************************************/
495
void Adafruit_BNO055::setSensorOffsets(const adafruit_bno055_offsets_t &offsets_type)
496
{
497
	adafruit_bno055_opmode_t lastMode = _mode;
498
	setMode(OPERATION_MODE_CONFIG);
499
	delay(25);
500

  
501
	write8(ACCEL_OFFSET_X_LSB_ADDR, (offsets_type.accel_offset_x) & 0x0FF);
502
	write8(ACCEL_OFFSET_X_MSB_ADDR, (offsets_type.accel_offset_x >> 8) & 0x0FF);
503
	write8(ACCEL_OFFSET_Y_LSB_ADDR, (offsets_type.accel_offset_y) & 0x0FF);
504
	write8(ACCEL_OFFSET_Y_MSB_ADDR, (offsets_type.accel_offset_y >> 8) & 0x0FF);
505
	write8(ACCEL_OFFSET_Z_LSB_ADDR, (offsets_type.accel_offset_z) & 0x0FF);
506
	write8(ACCEL_OFFSET_Z_MSB_ADDR, (offsets_type.accel_offset_z >> 8) & 0x0FF);
507

  
508
	write8(GYRO_OFFSET_X_LSB_ADDR, (offsets_type.gyro_offset_x) & 0x0FF);
509
	write8(GYRO_OFFSET_X_MSB_ADDR, (offsets_type.gyro_offset_x >> 8) & 0x0FF);
510
	write8(GYRO_OFFSET_Y_LSB_ADDR, (offsets_type.gyro_offset_y) & 0x0FF);
511
	write8(GYRO_OFFSET_Y_MSB_ADDR, (offsets_type.gyro_offset_y >> 8) & 0x0FF);
512
	write8(GYRO_OFFSET_Z_LSB_ADDR, (offsets_type.gyro_offset_z) & 0x0FF);
513
	write8(GYRO_OFFSET_Z_MSB_ADDR, (offsets_type.gyro_offset_z >> 8) & 0x0FF);
514

  
515
	write8(MAG_OFFSET_X_LSB_ADDR, (offsets_type.mag_offset_x) & 0x0FF);
516
	write8(MAG_OFFSET_X_MSB_ADDR, (offsets_type.mag_offset_x >> 8) & 0x0FF);
517
	write8(MAG_OFFSET_Y_LSB_ADDR, (offsets_type.mag_offset_y) & 0x0FF);
518
	write8(MAG_OFFSET_Y_MSB_ADDR, (offsets_type.mag_offset_y >> 8) & 0x0FF);
519
	write8(MAG_OFFSET_Z_LSB_ADDR, (offsets_type.mag_offset_z) & 0x0FF);
520
	write8(MAG_OFFSET_Z_MSB_ADDR, (offsets_type.mag_offset_z >> 8) & 0x0FF);
521

  
522
	write8(ACCEL_RADIUS_LSB_ADDR, (offsets_type.accel_radius) & 0x0FF);
523
	write8(ACCEL_RADIUS_MSB_ADDR, (offsets_type.accel_radius >> 8) & 0x0FF);
524

  
525
	write8(MAG_RADIUS_LSB_ADDR, (offsets_type.mag_radius) & 0x0FF);
526
	write8(MAG_RADIUS_MSB_ADDR, (offsets_type.mag_radius >> 8) & 0x0FF);
527

  
528
	setMode(lastMode);
529
}
530

  
531
bool Adafruit_BNO055::isFullyCalibrated(void)
532
{
533
	uint8_t system, gyro, accel, mag;
534
	getCalibration(&system, &gyro, &accel, &mag);
535
	if (system < 3 || gyro < 3 || accel < 3 || mag < 3)
536
		return false;
537
	return true;
538
}
539

  
540

  
541

  
392 542
/***************************************************************************
393 543
 PRIVATE FUNCTIONS
394 544
 ***************************************************************************/
395 545

  
396 546
/**************************************************************************/
397 547
/*!
398
    @brief  Writes an 8 bit value over I2C
399
*/
548
	@brief  Writes an 8 bit value over I2C
549
	*/
400 550
/**************************************************************************/
401 551
bool Adafruit_BNO055::write8(adafruit_bno055_reg_t reg, byte value)
402 552
{
403
  Wire.beginTransmission(_address);
404
  #if ARDUINO >= 100
405
    Wire.write((uint8_t)reg);
406
    Wire.write((uint8_t)value);
407
  #else
408
    Wire.send(reg);
409
    Wire.send(value);
410
  #endif
411
  Wire.endTransmission();
412

  
413
  /* ToDo: Check for error! */
414
  return true;
553
	Wire.beginTransmission(_address);
554
#if ARDUINO >= 100
555
	Wire.write((uint8_t)reg);
556
	Wire.write((uint8_t)value);
557
#else
558
	Wire.send(reg);
559
	Wire.send(value);
560
#endif
561
	Wire.endTransmission();
562

  
563
	/* ToDo: Check for error! */
564
	return true;
415 565
}
416 566

  
417 567
/**************************************************************************/
418 568
/*!
419
    @brief  Reads an 8 bit value over I2C
420
*/
569
	@brief  Reads an 8 bit value over I2C
570
	*/
421 571
/**************************************************************************/
422
byte Adafruit_BNO055::read8(adafruit_bno055_reg_t reg )
572
byte Adafruit_BNO055::read8(adafruit_bno055_reg_t reg)
423 573
{
424
  byte value = 0;
425

  
426
  Wire.beginTransmission(_address);
427
  #if ARDUINO >= 100
428
    Wire.write((uint8_t)reg);
429
  #else
430
    Wire.send(reg);
431
  #endif
432
  Wire.endTransmission();
433
  Wire.requestFrom(_address, (byte)1);
434
  #if ARDUINO >= 100
435
    value = Wire.read();
436
  #else
437
    value = Wire.receive();
438
  #endif
439

  
440
  return value;
574
	byte value = 0;
575

  
576
	Wire.beginTransmission(_address);
577
#if ARDUINO >= 100
578
	Wire.write((uint8_t)reg);
579
#else
580
	Wire.send(reg);
581
#endif
582
	Wire.endTransmission();
583
	Wire.requestFrom(_address, (byte)1);
584
#if ARDUINO >= 100
585
	value = Wire.read();
586
#else
587
	value = Wire.receive();
588
#endif
589

  
590
	return value;
441 591
}
442 592

  
443 593
/**************************************************************************/
444 594
/*!
445
    @brief  Reads the specified number of bytes over I2C
446
*/
595
	@brief  Reads the specified number of bytes over I2C
596
	*/
447 597
/**************************************************************************/
448 598
bool Adafruit_BNO055::readLen(adafruit_bno055_reg_t reg, byte * buffer, uint8_t len)
449 599
{
450
  Wire.beginTransmission(_address);
451
  #if ARDUINO >= 100
452
    Wire.write((uint8_t)reg);
453
  #else
454
    Wire.send(reg);
455
  #endif
456
  Wire.endTransmission();
457
  Wire.requestFrom(_address, (byte)len);
458

  
459
  for (uint8_t i = 0; i < len; i++)
460
  {
461
    #if ARDUINO >= 100
462
      buffer[i] = Wire.read();
463
    #else
464
      buffer[i] = Wire.receive();
465
    #endif
466
  }
467

  
468
  /* ToDo: Check for errors! */
469
  return true;
470
}
600
	Wire.beginTransmission(_address);
601
#if ARDUINO >= 100
602
	Wire.write((uint8_t)reg);
603
#else
604
	Wire.send(reg);
605
#endif
606
	Wire.endTransmission();
607
	Wire.requestFrom(_address, (byte)len);
608

  
609
	for (uint8_t i = 0; i < len; i++)
610
	{
611
#if ARDUINO >= 100
612
		buffer[i] = Wire.read();
613
#else
614
		buffer[i] = Wire.receive();
615
#endif
616
	}
617

  
618
	/* ToDo: Check for errors! */
619
	return true;
620
}
Adafruit_BNO055.h
15 15
  Written by KTOWN for Adafruit Industries.
16 16

  
17 17
  MIT license, all text above must be included in any redistribution
18
 ***************************************************************************/
18
  ***************************************************************************/
19 19

  
20 20
#ifndef __ADAFRUIT_BNO055_H__
21 21
#define __ADAFRUIT_BNO055_H__
22 22

  
23 23
#if (ARDUINO >= 100)
24
 #include "Arduino.h"
24
#include "Arduino.h"
25 25
#else
26
 #include "WProgram.h"
26
#include "WProgram.h"
27 27
#endif
28 28

  
29 29
#ifdef __AVR_ATtiny85__
30
 #include <TinyWireM.h>
31
 #define Wire TinyWireM
30
#include <TinyWireM.h>
31
#define Wire TinyWireM
32 32
#else
33
 #include <Wire.h>
33
#include <Wire.h>
34 34
#endif
35 35

  
36 36
#include <Adafruit_Sensor.h>
......
40 40
#define BNO055_ADDRESS_B (0x29)
41 41
#define BNO055_ID        (0xA0)
42 42

  
43
#define NUM_BNO055_OFFSET_REGISTERS (22)
44

  
45
typedef struct
46
{
47
	int16_t accel_offset_x;
48
	int16_t accel_offset_y;
49
	int16_t accel_offset_z;
50
	int16_t gyro_offset_x;
51
	int16_t gyro_offset_y;
52
	int16_t gyro_offset_z;
53
	int16_t mag_offset_x;
54
	int16_t mag_offset_y;
55
	int16_t mag_offset_z;
56

  
57
	int16_t accel_radius;
58
	int16_t mag_radius;
59
} adafruit_bno055_offsets_t;
60

  
43 61
class Adafruit_BNO055 : public Adafruit_Sensor
44 62
{
45
  public:
46
    typedef enum
47
    {
48
      /* Page id register definition */
49
      BNO055_PAGE_ID_ADDR                                     = 0X07,
50

  
51
      /* PAGE0 REGISTER DEFINITION START*/
52
      BNO055_CHIP_ID_ADDR                                     = 0x00,
53
      BNO055_ACCEL_REV_ID_ADDR                                = 0x01,
54
      BNO055_MAG_REV_ID_ADDR                                  = 0x02,
55
      BNO055_GYRO_REV_ID_ADDR                                 = 0x03,
56
      BNO055_SW_REV_ID_LSB_ADDR                               = 0x04,
57
      BNO055_SW_REV_ID_MSB_ADDR                               = 0x05,
58
      BNO055_BL_REV_ID_ADDR                                   = 0X06,
59

  
60
      /* Accel data register */
61
      BNO055_ACCEL_DATA_X_LSB_ADDR                            = 0X08,
62
      BNO055_ACCEL_DATA_X_MSB_ADDR                            = 0X09,
63
      BNO055_ACCEL_DATA_Y_LSB_ADDR                            = 0X0A,
64
      BNO055_ACCEL_DATA_Y_MSB_ADDR                            = 0X0B,
65
      BNO055_ACCEL_DATA_Z_LSB_ADDR                            = 0X0C,
66
      BNO055_ACCEL_DATA_Z_MSB_ADDR                            = 0X0D,
67

  
68
      /* Mag data register */
69
      BNO055_MAG_DATA_X_LSB_ADDR                              = 0X0E,
70
      BNO055_MAG_DATA_X_MSB_ADDR                              = 0X0F,
71
      BNO055_MAG_DATA_Y_LSB_ADDR                              = 0X10,
72
      BNO055_MAG_DATA_Y_MSB_ADDR                              = 0X11,
73
      BNO055_MAG_DATA_Z_LSB_ADDR                              = 0X12,
74
      BNO055_MAG_DATA_Z_MSB_ADDR                              = 0X13,
75

  
76
      /* Gyro data registers */
77
      BNO055_GYRO_DATA_X_LSB_ADDR                             = 0X14,
78
      BNO055_GYRO_DATA_X_MSB_ADDR                             = 0X15,
79
      BNO055_GYRO_DATA_Y_LSB_ADDR                             = 0X16,
80
      BNO055_GYRO_DATA_Y_MSB_ADDR                             = 0X17,
81
      BNO055_GYRO_DATA_Z_LSB_ADDR                             = 0X18,
82
      BNO055_GYRO_DATA_Z_MSB_ADDR                             = 0X19,
83

  
84
      /* Euler data registers */
85
      BNO055_EULER_H_LSB_ADDR                                 = 0X1A,
86
      BNO055_EULER_H_MSB_ADDR                                 = 0X1B,
87
      BNO055_EULER_R_LSB_ADDR                                 = 0X1C,
88
      BNO055_EULER_R_MSB_ADDR                                 = 0X1D,
89
      BNO055_EULER_P_LSB_ADDR                                 = 0X1E,
90
      BNO055_EULER_P_MSB_ADDR                                 = 0X1F,
91

  
92
      /* Quaternion data registers */
93
      BNO055_QUATERNION_DATA_W_LSB_ADDR                       = 0X20,
94
      BNO055_QUATERNION_DATA_W_MSB_ADDR                       = 0X21,
95
      BNO055_QUATERNION_DATA_X_LSB_ADDR                       = 0X22,
96
      BNO055_QUATERNION_DATA_X_MSB_ADDR                       = 0X23,
97
      BNO055_QUATERNION_DATA_Y_LSB_ADDR                       = 0X24,
98
      BNO055_QUATERNION_DATA_Y_MSB_ADDR                       = 0X25,
99
      BNO055_QUATERNION_DATA_Z_LSB_ADDR                       = 0X26,
100
      BNO055_QUATERNION_DATA_Z_MSB_ADDR                       = 0X27,
101

  
102
      /* Linear acceleration data registers */
103
      BNO055_LINEAR_ACCEL_DATA_X_LSB_ADDR                     = 0X28,
104
      BNO055_LINEAR_ACCEL_DATA_X_MSB_ADDR                     = 0X29,
105
      BNO055_LINEAR_ACCEL_DATA_Y_LSB_ADDR                     = 0X2A,
106
      BNO055_LINEAR_ACCEL_DATA_Y_MSB_ADDR                     = 0X2B,
107
      BNO055_LINEAR_ACCEL_DATA_Z_LSB_ADDR                     = 0X2C,
108
      BNO055_LINEAR_ACCEL_DATA_Z_MSB_ADDR                     = 0X2D,
109

  
110
      /* Gravity data registers */
111
      BNO055_GRAVITY_DATA_X_LSB_ADDR                          = 0X2E,
112
      BNO055_GRAVITY_DATA_X_MSB_ADDR                          = 0X2F,
113
      BNO055_GRAVITY_DATA_Y_LSB_ADDR                          = 0X30,
114
      BNO055_GRAVITY_DATA_Y_MSB_ADDR                          = 0X31,
115
      BNO055_GRAVITY_DATA_Z_LSB_ADDR                          = 0X32,
116
      BNO055_GRAVITY_DATA_Z_MSB_ADDR                          = 0X33,
117

  
118
      /* Temperature data register */
119
      BNO055_TEMP_ADDR                                        = 0X34,
120

  
121
      /* Status registers */
122
      BNO055_CALIB_STAT_ADDR                                  = 0X35,
123
      BNO055_SELFTEST_RESULT_ADDR                             = 0X36,
124
      BNO055_INTR_STAT_ADDR                                   = 0X37,
125

  
126
      BNO055_SYS_CLK_STAT_ADDR                                = 0X38,
127
      BNO055_SYS_STAT_ADDR                                    = 0X39,
128
      BNO055_SYS_ERR_ADDR                                     = 0X3A,
129

  
130
      /* Unit selection register */
131
      BNO055_UNIT_SEL_ADDR                                    = 0X3B,
132
      BNO055_DATA_SELECT_ADDR                                 = 0X3C,
133

  
134
      /* Mode registers */
135
      BNO055_OPR_MODE_ADDR                                    = 0X3D,
136
      BNO055_PWR_MODE_ADDR                                    = 0X3E,
137

  
138
      BNO055_SYS_TRIGGER_ADDR                                 = 0X3F,
139
      BNO055_TEMP_SOURCE_ADDR                                 = 0X40,
140

  
141
      /* Axis remap registers */
142
      BNO055_AXIS_MAP_CONFIG_ADDR                             = 0X41,
143
      BNO055_AXIS_MAP_SIGN_ADDR                               = 0X42,
144

  
145
      /* SIC registers */
146
      BNO055_SIC_MATRIX_0_LSB_ADDR                            = 0X43,
147
      BNO055_SIC_MATRIX_0_MSB_ADDR                            = 0X44,
148
      BNO055_SIC_MATRIX_1_LSB_ADDR                            = 0X45,
149
      BNO055_SIC_MATRIX_1_MSB_ADDR                            = 0X46,
150
      BNO055_SIC_MATRIX_2_LSB_ADDR                            = 0X47,
151
      BNO055_SIC_MATRIX_2_MSB_ADDR                            = 0X48,
152
      BNO055_SIC_MATRIX_3_LSB_ADDR                            = 0X49,
153
      BNO055_SIC_MATRIX_3_MSB_ADDR                            = 0X4A,
154
      BNO055_SIC_MATRIX_4_LSB_ADDR                            = 0X4B,
155
      BNO055_SIC_MATRIX_4_MSB_ADDR                            = 0X4C,
156
      BNO055_SIC_MATRIX_5_LSB_ADDR                            = 0X4D,
157
      BNO055_SIC_MATRIX_5_MSB_ADDR                            = 0X4E,
158
      BNO055_SIC_MATRIX_6_LSB_ADDR                            = 0X4F,
159
      BNO055_SIC_MATRIX_6_MSB_ADDR                            = 0X50,
160
      BNO055_SIC_MATRIX_7_LSB_ADDR                            = 0X51,
161
      BNO055_SIC_MATRIX_7_MSB_ADDR                            = 0X52,
162
      BNO055_SIC_MATRIX_8_LSB_ADDR                            = 0X53,
163
      BNO055_SIC_MATRIX_8_MSB_ADDR                            = 0X54,
164

  
165
      /* Accelerometer Offset registers */
166
      ACCEL_OFFSET_X_LSB_ADDR                                 = 0X55,
167
      ACCEL_OFFSET_X_MSB_ADDR                                 = 0X56,
168
      ACCEL_OFFSET_Y_LSB_ADDR                                 = 0X57,
169
      ACCEL_OFFSET_Y_MSB_ADDR                                 = 0X58,
170
      ACCEL_OFFSET_Z_LSB_ADDR                                 = 0X59,
171
      ACCEL_OFFSET_Z_MSB_ADDR                                 = 0X5A,
172

  
173
      /* Magnetometer Offset registers */
174
      MAG_OFFSET_X_LSB_ADDR                                   = 0X5B,
175
      MAG_OFFSET_X_MSB_ADDR                                   = 0X5C,
176
      MAG_OFFSET_Y_LSB_ADDR                                   = 0X5D,
177
      MAG_OFFSET_Y_MSB_ADDR                                   = 0X5E,
178
      MAG_OFFSET_Z_LSB_ADDR                                   = 0X5F,
179
      MAG_OFFSET_Z_MSB_ADDR                                   = 0X60,
180

  
181
      /* Gyroscope Offset register s*/
182
      GYRO_OFFSET_X_LSB_ADDR                                  = 0X61,
183
      GYRO_OFFSET_X_MSB_ADDR                                  = 0X62,
184
      GYRO_OFFSET_Y_LSB_ADDR                                  = 0X63,
185
      GYRO_OFFSET_Y_MSB_ADDR                                  = 0X64,
186
      GYRO_OFFSET_Z_LSB_ADDR                                  = 0X65,
187
      GYRO_OFFSET_Z_MSB_ADDR                                  = 0X66,
188

  
189
      /* Radius registers */
190
      ACCEL_RADIUS_LSB_ADDR                                   = 0X67,
191
      ACCEL_RADIUS_MSB_ADDR                                   = 0X68,
192
      MAG_RADIUS_LSB_ADDR                                     = 0X69,
193
      MAG_RADIUS_MSB_ADDR                                     = 0X6A
194
    } adafruit_bno055_reg_t;
195

  
196
    typedef enum
197
    {
198
      POWER_MODE_NORMAL                                       = 0X00,
199
      POWER_MODE_LOWPOWER                                     = 0X01,
200
      POWER_MODE_SUSPEND                                      = 0X02
201
    } adafruit_bno055_powermode_t;
202

  
203
    typedef enum
204
    {
205
      /* Operation mode settings*/
206
      OPERATION_MODE_CONFIG                                   = 0X00,
207
      OPERATION_MODE_ACCONLY                                  = 0X01,
208
      OPERATION_MODE_MAGONLY                                  = 0X02,
209
      OPERATION_MODE_GYRONLY                                  = 0X03,
210
      OPERATION_MODE_ACCMAG                                   = 0X04,
211
      OPERATION_MODE_ACCGYRO                                  = 0X05,
212
      OPERATION_MODE_MAGGYRO                                  = 0X06,
213
      OPERATION_MODE_AMG                                      = 0X07,
214
      OPERATION_MODE_IMUPLUS                                  = 0X08,
215
      OPERATION_MODE_COMPASS                                  = 0X09,
216
      OPERATION_MODE_M4G                                      = 0X0A,
217
      OPERATION_MODE_NDOF_FMC_OFF                             = 0X0B,
218
      OPERATION_MODE_NDOF                                     = 0X0C
219
    } adafruit_bno055_opmode_t;
220

  
221
    typedef struct
222
    {
223
      uint8_t  accel_rev;
224
      uint8_t  mag_rev;
225
      uint8_t  gyro_rev;
226
      uint16_t sw_rev;
227
      uint8_t  bl_rev;
228
    } adafruit_bno055_rev_info_t;
229

  
230
    typedef enum
231
    {
232
      VECTOR_ACCELEROMETER = BNO055_ACCEL_DATA_X_LSB_ADDR,
233
      VECTOR_MAGNETOMETER  = BNO055_MAG_DATA_X_LSB_ADDR,
234
      VECTOR_GYROSCOPE     = BNO055_GYRO_DATA_X_LSB_ADDR,
235
      VECTOR_EULER         = BNO055_EULER_H_LSB_ADDR,
236
      VECTOR_LINEARACCEL   = BNO055_LINEAR_ACCEL_DATA_X_LSB_ADDR,
237
      VECTOR_GRAVITY       = BNO055_GRAVITY_DATA_X_LSB_ADDR
238
    } adafruit_vector_type_t;
239

  
240
    Adafruit_BNO055 ( int32_t sensorID = -1, uint8_t address = BNO055_ADDRESS_A );
241

  
242
    bool  begin               ( adafruit_bno055_opmode_t mode = OPERATION_MODE_NDOF );
243
    void  setMode             ( adafruit_bno055_opmode_t mode );
244
    void  getRevInfo          ( adafruit_bno055_rev_info_t* );
245
    void  displayRevInfo      ( void );
246
    void  setExtCrystalUse    ( boolean usextal );
247
    void  getSystemStatus     ( uint8_t *system_status,
248
                                uint8_t *self_test_result,
249
                                uint8_t *system_error);
250
    void  displaySystemStatus ( void );
251
    void  getCalibration      ( uint8_t* system, uint8_t* gyro, uint8_t* accel, uint8_t* mag);
252

  
253
    imu::Vector<3>  getVector ( adafruit_vector_type_t vector_type );
254
    imu::Quaternion getQuat   ( void );
255
    int8_t          getTemp   ( void );
256

  
257
    /* Adafruit_Sensor implementation */
258
    bool  getEvent  ( sensors_event_t* );
259
    void  getSensor ( sensor_t* );
260

  
261
  private:
262
    byte  read8   ( adafruit_bno055_reg_t );
263
    bool  readLen ( adafruit_bno055_reg_t, byte* buffer, uint8_t len );
264
    bool  write8  ( adafruit_bno055_reg_t, byte value );
265

  
266
    uint8_t _address;
267
    int32_t _sensorID;
268
    adafruit_bno055_opmode_t _mode;
63
public:
64
	typedef enum
65
	{
66
		/* Page id register definition */
67
		BNO055_PAGE_ID_ADDR = 0X07,
68

  
69
		/* PAGE0 REGISTER DEFINITION START*/
70
		BNO055_CHIP_ID_ADDR = 0x00,
71
		BNO055_ACCEL_REV_ID_ADDR = 0x01,
72
		BNO055_MAG_REV_ID_ADDR = 0x02,
73
		BNO055_GYRO_REV_ID_ADDR = 0x03,
74
		BNO055_SW_REV_ID_LSB_ADDR = 0x04,
75
		BNO055_SW_REV_ID_MSB_ADDR = 0x05,
76
		BNO055_BL_REV_ID_ADDR = 0X06,
77

  
78
		/* Accel data register */
79
		BNO055_ACCEL_DATA_X_LSB_ADDR = 0X08,
80
		BNO055_ACCEL_DATA_X_MSB_ADDR = 0X09,
81
		BNO055_ACCEL_DATA_Y_LSB_ADDR = 0X0A,
82
		BNO055_ACCEL_DATA_Y_MSB_ADDR = 0X0B,
83
		BNO055_ACCEL_DATA_Z_LSB_ADDR = 0X0C,
84
		BNO055_ACCEL_DATA_Z_MSB_ADDR = 0X0D,
85

  
86
		/* Mag data register */
87
		BNO055_MAG_DATA_X_LSB_ADDR = 0X0E,
88
		BNO055_MAG_DATA_X_MSB_ADDR = 0X0F,
89
		BNO055_MAG_DATA_Y_LSB_ADDR = 0X10,
90
		BNO055_MAG_DATA_Y_MSB_ADDR = 0X11,
91
		BNO055_MAG_DATA_Z_LSB_ADDR = 0X12,
92
		BNO055_MAG_DATA_Z_MSB_ADDR = 0X13,
93

  
94
		/* Gyro data registers */
95
		BNO055_GYRO_DATA_X_LSB_ADDR = 0X14,
96
		BNO055_GYRO_DATA_X_MSB_ADDR = 0X15,
97
		BNO055_GYRO_DATA_Y_LSB_ADDR = 0X16,
98
		BNO055_GYRO_DATA_Y_MSB_ADDR = 0X17,
99
		BNO055_GYRO_DATA_Z_LSB_ADDR = 0X18,
100
		BNO055_GYRO_DATA_Z_MSB_ADDR = 0X19,
101

  
102
		/* Euler data registers */
103
		BNO055_EULER_H_LSB_ADDR = 0X1A,
104
		BNO055_EULER_H_MSB_ADDR = 0X1B,
105
		BNO055_EULER_R_LSB_ADDR = 0X1C,
106
		BNO055_EULER_R_MSB_ADDR = 0X1D,
107
		BNO055_EULER_P_LSB_ADDR = 0X1E,
108
		BNO055_EULER_P_MSB_ADDR = 0X1F,
109

  
110
		/* Quaternion data registers */
111
		BNO055_QUATERNION_DATA_W_LSB_ADDR = 0X20,
112
		BNO055_QUATERNION_DATA_W_MSB_ADDR = 0X21,
113
		BNO055_QUATERNION_DATA_X_LSB_ADDR = 0X22,
114
		BNO055_QUATERNION_DATA_X_MSB_ADDR = 0X23,
115
		BNO055_QUATERNION_DATA_Y_LSB_ADDR = 0X24,
116
		BNO055_QUATERNION_DATA_Y_MSB_ADDR = 0X25,
117
		BNO055_QUATERNION_DATA_Z_LSB_ADDR = 0X26,
118
		BNO055_QUATERNION_DATA_Z_MSB_ADDR = 0X27,
119

  
120
		/* Linear acceleration data registers */
121
		BNO055_LINEAR_ACCEL_DATA_X_LSB_ADDR = 0X28,
122
		BNO055_LINEAR_ACCEL_DATA_X_MSB_ADDR = 0X29,
123
		BNO055_LINEAR_ACCEL_DATA_Y_LSB_ADDR = 0X2A,
124
		BNO055_LINEAR_ACCEL_DATA_Y_MSB_ADDR = 0X2B,
125
		BNO055_LINEAR_ACCEL_DATA_Z_LSB_ADDR = 0X2C,
126
		BNO055_LINEAR_ACCEL_DATA_Z_MSB_ADDR = 0X2D,
127

  
128
		/* Gravity data registers */
129
		BNO055_GRAVITY_DATA_X_LSB_ADDR = 0X2E,
130
		BNO055_GRAVITY_DATA_X_MSB_ADDR = 0X2F,
131
		BNO055_GRAVITY_DATA_Y_LSB_ADDR = 0X30,
132
		BNO055_GRAVITY_DATA_Y_MSB_ADDR = 0X31,
133
		BNO055_GRAVITY_DATA_Z_LSB_ADDR = 0X32,
134
		BNO055_GRAVITY_DATA_Z_MSB_ADDR = 0X33,
135

  
136
		/* Temperature data register */
137
		BNO055_TEMP_ADDR = 0X34,
138

  
139
		/* Status registers */
140
		BNO055_CALIB_STAT_ADDR = 0X35,
141
		BNO055_SELFTEST_RESULT_ADDR = 0X36,
142
		BNO055_INTR_STAT_ADDR = 0X37,
143

  
144
		BNO055_SYS_CLK_STAT_ADDR = 0X38,
145
		BNO055_SYS_STAT_ADDR = 0X39,
146
		BNO055_SYS_ERR_ADDR = 0X3A,
147

  
148
		/* Unit selection register */
149
		BNO055_UNIT_SEL_ADDR = 0X3B,
150
		BNO055_DATA_SELECT_ADDR = 0X3C,
151

  
152
		/* Mode registers */
153
		BNO055_OPR_MODE_ADDR = 0X3D,
154
		BNO055_PWR_MODE_ADDR = 0X3E,
155

  
156
		BNO055_SYS_TRIGGER_ADDR = 0X3F,
157
		BNO055_TEMP_SOURCE_ADDR = 0X40,
158

  
159
		/* Axis remap registers */
160
		BNO055_AXIS_MAP_CONFIG_ADDR = 0X41,
161
		BNO055_AXIS_MAP_SIGN_ADDR = 0X42,
162

  
163
		/* SIC registers */
164
		BNO055_SIC_MATRIX_0_LSB_ADDR = 0X43,
165
		BNO055_SIC_MATRIX_0_MSB_ADDR = 0X44,
166
		BNO055_SIC_MATRIX_1_LSB_ADDR = 0X45,
167
		BNO055_SIC_MATRIX_1_MSB_ADDR = 0X46,
168
		BNO055_SIC_MATRIX_2_LSB_ADDR = 0X47,
169
		BNO055_SIC_MATRIX_2_MSB_ADDR = 0X48,
170
		BNO055_SIC_MATRIX_3_LSB_ADDR = 0X49,
171
		BNO055_SIC_MATRIX_3_MSB_ADDR = 0X4A,
172
		BNO055_SIC_MATRIX_4_LSB_ADDR = 0X4B,
173
		BNO055_SIC_MATRIX_4_MSB_ADDR = 0X4C,
174
		BNO055_SIC_MATRIX_5_LSB_ADDR = 0X4D,
175
		BNO055_SIC_MATRIX_5_MSB_ADDR = 0X4E,
176
		BNO055_SIC_MATRIX_6_LSB_ADDR = 0X4F,
177
		BNO055_SIC_MATRIX_6_MSB_ADDR = 0X50,
178
		BNO055_SIC_MATRIX_7_LSB_ADDR = 0X51,
179
		BNO055_SIC_MATRIX_7_MSB_ADDR = 0X52,
180
		BNO055_SIC_MATRIX_8_LSB_ADDR = 0X53,
181
		BNO055_SIC_MATRIX_8_MSB_ADDR = 0X54,
182

  
183
		/* Accelerometer Offset registers */
184
		ACCEL_OFFSET_X_LSB_ADDR = 0X55,
185
		ACCEL_OFFSET_X_MSB_ADDR = 0X56,
186
		ACCEL_OFFSET_Y_LSB_ADDR = 0X57,
187
		ACCEL_OFFSET_Y_MSB_ADDR = 0X58,
188
		ACCEL_OFFSET_Z_LSB_ADDR = 0X59,
189
		ACCEL_OFFSET_Z_MSB_ADDR = 0X5A,
190

  
191
		/* Magnetometer Offset registers */
192
		MAG_OFFSET_X_LSB_ADDR = 0X5B,
193
		MAG_OFFSET_X_MSB_ADDR = 0X5C,
194
		MAG_OFFSET_Y_LSB_ADDR = 0X5D,
195
		MAG_OFFSET_Y_MSB_ADDR = 0X5E,
196
		MAG_OFFSET_Z_LSB_ADDR = 0X5F,
197
		MAG_OFFSET_Z_MSB_ADDR = 0X60,
198

  
199
		/* Gyroscope Offset register s*/
200
		GYRO_OFFSET_X_LSB_ADDR = 0X61,
201
		GYRO_OFFSET_X_MSB_ADDR = 0X62,
202
		GYRO_OFFSET_Y_LSB_ADDR = 0X63,
203
		GYRO_OFFSET_Y_MSB_ADDR = 0X64,
204
		GYRO_OFFSET_Z_LSB_ADDR = 0X65,
205
		GYRO_OFFSET_Z_MSB_ADDR = 0X66,
206

  
207
		/* Radius registers */
208
		ACCEL_RADIUS_LSB_ADDR = 0X67,
209
		ACCEL_RADIUS_MSB_ADDR = 0X68,
210
		MAG_RADIUS_LSB_ADDR = 0X69,
211
		MAG_RADIUS_MSB_ADDR = 0X6A
212
	} adafruit_bno055_reg_t;
213

  
214
	typedef enum
215
	{
216
		POWER_MODE_NORMAL = 0X00,
217
		POWER_MODE_LOWPOWER = 0X01,
218
		POWER_MODE_SUSPEND = 0X02
219
	} adafruit_bno055_powermode_t;
220

  
221
	typedef enum
222
	{
223
		/* Operation mode settings*/
224
		OPERATION_MODE_CONFIG = 0X00,
225
		OPERATION_MODE_ACCONLY = 0X01,
226
		OPERATION_MODE_MAGONLY = 0X02,
227
		OPERATION_MODE_GYRONLY = 0X03,
228
		OPERATION_MODE_ACCMAG = 0X04,
229
		OPERATION_MODE_ACCGYRO = 0X05,
230
		OPERATION_MODE_MAGGYRO = 0X06,
231
		OPERATION_MODE_AMG = 0X07,
232
		OPERATION_MODE_IMUPLUS = 0X08,
233
		OPERATION_MODE_COMPASS = 0X09,
234
		OPERATION_MODE_M4G = 0X0A,
235
		OPERATION_MODE_NDOF_FMC_OFF = 0X0B,
236
		OPERATION_MODE_NDOF = 0X0C
237
	} adafruit_bno055_opmode_t;
238

  
239
	typedef struct
240
	{
241
		uint8_t  accel_rev;
242
		uint8_t  mag_rev;
243
		uint8_t  gyro_rev;
244
		uint16_t sw_rev;
245
		uint8_t  bl_rev;
246
	} adafruit_bno055_rev_info_t;
247

  
248
	typedef enum
249
	{
250
		VECTOR_ACCELEROMETER = BNO055_ACCEL_DATA_X_LSB_ADDR,
251
		VECTOR_MAGNETOMETER = BNO055_MAG_DATA_X_LSB_ADDR,
252
		VECTOR_GYROSCOPE = BNO055_GYRO_DATA_X_LSB_ADDR,
253
		VECTOR_EULER = BNO055_EULER_H_LSB_ADDR,
254
		VECTOR_LINEARACCEL = BNO055_LINEAR_ACCEL_DATA_X_LSB_ADDR,
255
		VECTOR_GRAVITY = BNO055_GRAVITY_DATA_X_LSB_ADDR
256
	} adafruit_vector_type_t;
257

  
258

  
259

  
260
	Adafruit_BNO055(int32_t sensorID = -1, uint8_t address = BNO055_ADDRESS_A);
261

  
262
	bool  begin(adafruit_bno055_opmode_t mode = OPERATION_MODE_NDOF);
263
	void  setMode(adafruit_bno055_opmode_t mode);
264
	void  getRevInfo(adafruit_bno055_rev_info_t*);
265
	void  displayRevInfo(void);
266
	void  setExtCrystalUse(boolean usextal);
267
	void  getSystemStatus(uint8_t *system_status,
268
		uint8_t *self_test_result,
269
		uint8_t *system_error);
270
	void  displaySystemStatus(void);
271
	void  getCalibration(uint8_t* system, uint8_t* gyro, uint8_t* accel, uint8_t* mag);
272

  
273
	imu::Vector<3>  getVector(adafruit_vector_type_t vector_type);
274
	imu::Quaternion getQuat(void);
275
	int8_t          getTemp(void);
276

  
277
	/* Adafruit_Sensor implementation */
278
	bool  getEvent(sensors_event_t*);
279
	void  getSensor(sensor_t*);
280

  
281
	/* Functions to deal with raw calibration data */
282
	bool  getSensorOffsets(int8_t* calibData);
283
	bool  getSensorOffsets(adafruit_bno055_offsets_t &offsets_type);
284
	void  setSensorOffsets(const int8_t* calibData);
285
	void  setSensorOffsets(const adafruit_bno055_offsets_t &offsets_type);
286
	bool  isFullyCalibrated(void);
287

  
288
private:
289
	byte  read8(adafruit_bno055_reg_t);
290
	bool  readLen(adafruit_bno055_reg_t, byte* buffer, uint8_t len);
291
	bool  write8(adafruit_bno055_reg_t, byte value);
292

  
293
	uint8_t _address;
294
	int32_t _sensorID;
295
	adafruit_bno055_opmode_t _mode;
269 296
};
270 297

  
271 298
#endif
examples/restore_offsets/restore_offsets.ino
1
#include <Wire.h>
2
#include <Adafruit_Sensor.h>
3
#include <Adafruit_BNO055.h>
4
#include <utility/imumaths.h>
5
#include <EEPROM.h>
6

  
7
/* This driver uses the Adafruit unified sensor library (Adafruit_Sensor),
8
   which provides a common 'type' for sensor data and some helper functions.
9

  
10
   To use this driver you will also need to download the Adafruit_Sensor
11
   library and include it in your libraries folder.
12

  
13
   You should also assign a unique ID to this sensor for use with
14
   the Adafruit Sensor API so that you can identify this particular
15
   sensor in any data logs, etc.  To assign a unique ID, simply
16
   provide an appropriate value in the constructor below (12345
17
   is used by default in this example).
18

  
19
   Connections
20
   ===========
21
   Connect SCL to analog 5
22
   Connect SDA to analog 4
23
   Connect VDD to 3-5V DC
24
   Connect GROUND to common ground
25

  
26
   History
27
   =======
28
   2015/MAR/03  - First release (KTOWN)
29
   2015/AUG/27  - Added calibration and system status helpers
30
   2015/NOV/13  - Added calibration save and restore
31
   */
32

  
33
/* Set the delay between fresh samples */
34
#define BNO055_SAMPLERATE_DELAY_MS (100)
35

  
36
Adafruit_BNO055 bno = Adafruit_BNO055(55);
37

  
38
/**************************************************************************/
39
/*
40
	Displays some basic information on this sensor from the unified
41
	sensor API sensor_t type (see Adafruit_Sensor for more information)
42
	*/
43
/**************************************************************************/
44
void displaySensorDetails(void)
45
{
46
	sensor_t sensor;
47
	bno.getSensor(&sensor);
... This diff was truncated because it exceeds the maximum size that can be displayed.

Also available in: Unified diff