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 |
|
#include <math.h>
|
27 |
26 |
#include <limits.h>
|
|
27 |
#include <math.h>
|
28 |
28 |
|
29 |
29 |
#include "Adafruit_BNO055.h"
|
30 |
30 |
|
... | ... | |
35 |
35 |
/**************************************************************************/
|
36 |
36 |
/*!
|
37 |
37 |
@brief Instantiates a new Adafruit_BNO055 class
|
|
38 |
@param sensorID
|
|
39 |
@param address
|
38 |
40 |
*/
|
39 |
41 |
/**************************************************************************/
|
40 |
|
Adafruit_BNO055::Adafruit_BNO055(int32_t sensorID, uint8_t address)
|
41 |
|
{
|
|
42 |
Adafruit_BNO055::Adafruit_BNO055(int32_t sensorID, uint8_t address) {
|
42 |
43 |
_sensorID = sensorID;
|
43 |
44 |
_address = address;
|
44 |
45 |
}
|
... | ... | |
50 |
51 |
/**************************************************************************/
|
51 |
52 |
/*!
|
52 |
53 |
@brief Sets up the HW
|
|
54 |
@param mode
|
|
55 |
@return true if process is sucessfull
|
53 |
56 |
*/
|
54 |
57 |
/**************************************************************************/
|
55 |
|
bool Adafruit_BNO055::begin(adafruit_bno055_opmode_t mode)
|
56 |
|
{
|
|
58 |
bool Adafruit_BNO055::begin(adafruit_bno055_opmode_t mode) {
|
57 |
59 |
/* Enable I2C */
|
58 |
60 |
Wire.begin();
|
59 |
61 |
|
... | ... | |
64 |
66 |
|
65 |
67 |
/* Make sure we have the right device */
|
66 |
68 |
uint8_t id = read8(BNO055_CHIP_ID_ADDR);
|
67 |
|
if(id != BNO055_ID)
|
68 |
|
{
|
|
69 |
if (id != BNO055_ID) {
|
69 |
70 |
delay(1000); // hold on for boot
|
70 |
71 |
id = read8(BNO055_CHIP_ID_ADDR);
|
71 |
|
if(id != BNO055_ID) {
|
72 |
|
return false; // still not? ok bail
|
|
72 |
if (id != BNO055_ID) {
|
|
73 |
return false; // still not? ok bail
|
73 |
74 |
}
|
74 |
75 |
}
|
75 |
76 |
|
... | ... | |
78 |
79 |
|
79 |
80 |
/* Reset */
|
80 |
81 |
write8(BNO055_SYS_TRIGGER_ADDR, 0x20);
|
81 |
|
while (read8(BNO055_CHIP_ID_ADDR) != BNO055_ID)
|
82 |
|
{
|
|
82 |
while (read8(BNO055_CHIP_ID_ADDR) != BNO055_ID) {
|
83 |
83 |
delay(10);
|
84 |
84 |
}
|
85 |
85 |
delay(50);
|
... | ... | |
120 |
120 |
/**************************************************************************/
|
121 |
121 |
/*!
|
122 |
122 |
@brief Puts the chip in the specified operating mode
|
|
123 |
@param mode
|
123 |
124 |
*/
|
124 |
125 |
/**************************************************************************/
|
125 |
|
void Adafruit_BNO055::setMode(adafruit_bno055_opmode_t mode)
|
126 |
|
{
|
|
126 |
void Adafruit_BNO055::setMode(adafruit_bno055_opmode_t mode) {
|
127 |
127 |
_mode = mode;
|
128 |
128 |
write8(BNO055_OPR_MODE_ADDR, _mode);
|
129 |
129 |
delay(30);
|
... | ... | |
132 |
132 |
/**************************************************************************/
|
133 |
133 |
/*!
|
134 |
134 |
@brief Changes the chip's axis remap
|
|
135 |
@param remapcode
|
135 |
136 |
*/
|
136 |
137 |
/**************************************************************************/
|
137 |
|
void Adafruit_BNO055::setAxisRemap( adafruit_bno055_axis_remap_config_t remapcode )
|
138 |
|
{
|
|
138 |
void Adafruit_BNO055::setAxisRemap(
|
|
139 |
adafruit_bno055_axis_remap_config_t remapcode) {
|
139 |
140 |
adafruit_bno055_opmode_t modeback = _mode;
|
140 |
141 |
|
141 |
142 |
setMode(OPERATION_MODE_CONFIG);
|
... | ... | |
150 |
151 |
/**************************************************************************/
|
151 |
152 |
/*!
|
152 |
153 |
@brief Changes the chip's axis signs
|
|
154 |
@param remapsign
|
153 |
155 |
*/
|
154 |
156 |
/**************************************************************************/
|
155 |
|
void Adafruit_BNO055::setAxisSign( adafruit_bno055_axis_remap_sign_t remapsign )
|
156 |
|
{
|
|
157 |
void Adafruit_BNO055::setAxisSign(adafruit_bno055_axis_remap_sign_t remapsign) {
|
157 |
158 |
adafruit_bno055_opmode_t modeback = _mode;
|
158 |
159 |
|
159 |
160 |
setMode(OPERATION_MODE_CONFIG);
|
... | ... | |
165 |
166 |
delay(20);
|
166 |
167 |
}
|
167 |
168 |
|
168 |
|
|
169 |
169 |
/**************************************************************************/
|
170 |
170 |
/*!
|
171 |
171 |
@brief Use the external 32.768KHz crystal
|
|
172 |
@param usextal boolean
|
172 |
173 |
*/
|
173 |
174 |
/**************************************************************************/
|
174 |
|
void Adafruit_BNO055::setExtCrystalUse(boolean usextal)
|
175 |
|
{
|
|
175 |
void Adafruit_BNO055::setExtCrystalUse(boolean usextal) {
|
176 |
176 |
adafruit_bno055_opmode_t modeback = _mode;
|
177 |
177 |
|
178 |
178 |
/* Switch to config mode (just in case since this is the default) */
|
... | ... | |
190 |
190 |
delay(20);
|
191 |
191 |
}
|
192 |
192 |
|
193 |
|
|
194 |
193 |
/**************************************************************************/
|
195 |
194 |
/*!
|
196 |
195 |
@brief Gets the latest system status info
|
|
196 |
@param system_status
|
|
197 |
@param self_test_result
|
|
198 |
@param system_error
|
197 |
199 |
*/
|
198 |
200 |
/**************************************************************************/
|
199 |
|
void Adafruit_BNO055::getSystemStatus(uint8_t *system_status, uint8_t *self_test_result, uint8_t *system_error)
|
200 |
|
{
|
|
201 |
void Adafruit_BNO055::getSystemStatus(uint8_t *system_status,
|
|
202 |
uint8_t *self_test_result,
|
|
203 |
uint8_t *system_error) {
|
201 |
204 |
write8(BNO055_PAGE_ID_ADDR, 0);
|
202 |
205 |
|
203 |
206 |
/* System Status (see section 4.3.58)
|
... | ... | |
211 |
214 |
6 = System running without fusion algorithms */
|
212 |
215 |
|
213 |
216 |
if (system_status != 0)
|
214 |
|
*system_status = read8(BNO055_SYS_STAT_ADDR);
|
|
217 |
*system_status = read8(BNO055_SYS_STAT_ADDR);
|
215 |
218 |
|
216 |
219 |
/* Self Test Results (see section )
|
217 |
220 |
--------------------------------
|
... | ... | |
242 |
245 |
A = Sensor configuration error */
|
243 |
246 |
|
244 |
247 |
if (system_error != 0)
|
245 |
|
*system_error = read8(BNO055_SYS_ERR_ADDR);
|
|
248 |
*system_error = read8(BNO055_SYS_ERR_ADDR);
|
246 |
249 |
|
247 |
250 |
delay(200);
|
248 |
251 |
}
|
... | ... | |
252 |
255 |
@brief Gets the chip revision numbers
|
253 |
256 |
*/
|
254 |
257 |
/**************************************************************************/
|
255 |
|
void Adafruit_BNO055::getRevInfo(adafruit_bno055_rev_info_t* info)
|
256 |
|
{
|
|
258 |
void Adafruit_BNO055::getRevInfo(adafruit_bno055_rev_info_t *info) {
|
257 |
259 |
uint8_t a, b;
|
258 |
260 |
|
259 |
261 |
memset(info, 0, sizeof(adafruit_bno055_rev_info_t));
|
... | ... | |
262 |
264 |
info->accel_rev = read8(BNO055_ACCEL_REV_ID_ADDR);
|
263 |
265 |
|
264 |
266 |
/* Check the magnetometer revision */
|
265 |
|
info->mag_rev = read8(BNO055_MAG_REV_ID_ADDR);
|
|
267 |
info->mag_rev = read8(BNO055_MAG_REV_ID_ADDR);
|
266 |
268 |
|
267 |
269 |
/* Check the gyroscope revision */
|
268 |
|
info->gyro_rev = read8(BNO055_GYRO_REV_ID_ADDR);
|
|
270 |
info->gyro_rev = read8(BNO055_GYRO_REV_ID_ADDR);
|
269 |
271 |
|
270 |
272 |
/* Check the SW revision */
|
271 |
|
info->bl_rev = read8(BNO055_BL_REV_ID_ADDR);
|
|
273 |
info->bl_rev = read8(BNO055_BL_REV_ID_ADDR);
|
272 |
274 |
|
273 |
275 |
a = read8(BNO055_SW_REV_ID_LSB_ADDR);
|
274 |
276 |
b = read8(BNO055_SW_REV_ID_MSB_ADDR);
|
... | ... | |
280 |
282 |
@brief Gets current calibration state. Each value should be a uint8_t
|
281 |
283 |
pointer and it will be set to 0 if not calibrated and 3 if
|
282 |
284 |
fully calibrated.
|
|
285 |
@param sys
|
|
286 |
@param gyro
|
|
287 |
@param accel
|
|
288 |
@param mag
|
283 |
289 |
*/
|
284 |
290 |
/**************************************************************************/
|
285 |
|
void Adafruit_BNO055::getCalibration(uint8_t* sys, uint8_t* gyro, uint8_t* accel, uint8_t* mag) {
|
|
291 |
void Adafruit_BNO055::getCalibration(uint8_t *sys, uint8_t *gyro,
|
|
292 |
uint8_t *accel, uint8_t *mag) {
|
286 |
293 |
uint8_t calData = read8(BNO055_CALIB_STAT_ADDR);
|
287 |
294 |
if (sys != NULL) {
|
288 |
295 |
*sys = (calData >> 6) & 0x03;
|
... | ... | |
301 |
308 |
/**************************************************************************/
|
302 |
309 |
/*!
|
303 |
310 |
@brief Gets the temperature in degrees celsius
|
|
311 |
@return temperature in degrees celsius
|
304 |
312 |
*/
|
305 |
313 |
/**************************************************************************/
|
306 |
|
int8_t Adafruit_BNO055::getTemp(void)
|
307 |
|
{
|
|
314 |
int8_t Adafruit_BNO055::getTemp() {
|
308 |
315 |
int8_t temp = (int8_t)(read8(BNO055_TEMP_ADDR));
|
309 |
316 |
return temp;
|
310 |
317 |
}
|
311 |
318 |
|
312 |
319 |
/**************************************************************************/
|
313 |
320 |
/*!
|
314 |
|
@brief Gets a vector reading from the specified source
|
|
321 |
@brief Gets a vector reading from the specified source
|
|
322 |
@param vector_type
|
|
323 |
@return vector from specified source
|
315 |
324 |
*/
|
316 |
325 |
/**************************************************************************/
|
317 |
|
imu::Vector<3> Adafruit_BNO055::getVector(adafruit_vector_type_t vector_type)
|
318 |
|
{
|
|
326 |
imu::Vector<3> Adafruit_BNO055::getVector(adafruit_vector_type_t vector_type) {
|
319 |
327 |
imu::Vector<3> xyz;
|
320 |
328 |
uint8_t buffer[6];
|
321 |
|
memset (buffer, 0, 6);
|
|
329 |
memset(buffer, 0, 6);
|
322 |
330 |
|
323 |
331 |
int16_t x, y, z;
|
324 |
332 |
x = y = z = 0;
|
... | ... | |
332 |
340 |
|
333 |
341 |
/* Convert the value to an appropriate range (section 3.6.4) */
|
334 |
342 |
/* 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 |
|
/* 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 |
|
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;
|
|
343 |
switch (vector_type) {
|
|
344 |
case VECTOR_MAGNETOMETER:
|
|
345 |
/* 1uT = 16 LSB */
|
|
346 |
xyz[0] = ((double)x) / 16.0;
|
|
347 |
xyz[1] = ((double)y) / 16.0;
|
|
348 |
xyz[2] = ((double)z) / 16.0;
|
|
349 |
break;
|
|
350 |
case VECTOR_GYROSCOPE:
|
|
351 |
/* 1dps = 16 LSB */
|
|
352 |
xyz[0] = ((double)x) / 16.0;
|
|
353 |
xyz[1] = ((double)y) / 16.0;
|
|
354 |
xyz[2] = ((double)z) / 16.0;
|
|
355 |
break;
|
|
356 |
case VECTOR_EULER:
|
|
357 |
/* 1 degree = 16 LSB */
|
|
358 |
xyz[0] = ((double)x) / 16.0;
|
|
359 |
xyz[1] = ((double)y) / 16.0;
|
|
360 |
xyz[2] = ((double)z) / 16.0;
|
|
361 |
break;
|
|
362 |
case VECTOR_ACCELEROMETER:
|
|
363 |
case VECTOR_LINEARACCEL:
|
|
364 |
case VECTOR_GRAVITY:
|
|
365 |
/* 1m/s^2 = 100 LSB */
|
|
366 |
xyz[0] = ((double)x) / 100.0;
|
|
367 |
xyz[1] = ((double)y) / 100.0;
|
|
368 |
xyz[2] = ((double)z) / 100.0;
|
|
369 |
break;
|
363 |
370 |
}
|
364 |
371 |
|
365 |
372 |
return xyz;
|
... | ... | |
368 |
375 |
/**************************************************************************/
|
369 |
376 |
/*!
|
370 |
377 |
@brief Gets a quaternion reading from the specified source
|
|
378 |
@return quaternion reading
|
371 |
379 |
*/
|
372 |
380 |
/**************************************************************************/
|
373 |
|
imu::Quaternion Adafruit_BNO055::getQuat(void)
|
374 |
|
{
|
|
381 |
imu::Quaternion Adafruit_BNO055::getQuat() {
|
375 |
382 |
uint8_t buffer[8];
|
376 |
|
memset (buffer, 0, 8);
|
|
383 |
memset(buffer, 0, 8);
|
377 |
384 |
|
378 |
385 |
int16_t x, y, z, w;
|
379 |
386 |
x = y = z = w = 0;
|
... | ... | |
386 |
393 |
z = (((uint16_t)buffer[7]) << 8) | ((uint16_t)buffer[6]);
|
387 |
394 |
|
388 |
395 |
/* Assign to Quaternion */
|
389 |
|
/* See http://ae-bst.resource.bosch.com/media/products/dokumente/bno055/BST_BNO055_DS000_12~1.pdf
|
|
396 |
/* See
|
|
397 |
http://ae-bst.resource.bosch.com/media/products/dokumente/bno055/BST_BNO055_DS000_12~1.pdf
|
390 |
398 |
3.6.5.5 Orientation (Quaternion) */
|
391 |
|
const double scale = (1.0 / (1<<14));
|
|
399 |
const double scale = (1.0 / (1 << 14));
|
392 |
400 |
imu::Quaternion quat(scale * w, scale * x, scale * y, scale * z);
|
393 |
401 |
return quat;
|
394 |
402 |
}
|
... | ... | |
396 |
404 |
/**************************************************************************/
|
397 |
405 |
/*!
|
398 |
406 |
@brief Provides the sensor_t data for this sensor
|
|
407 |
@param sensor
|
399 |
408 |
*/
|
400 |
409 |
/**************************************************************************/
|
401 |
|
void Adafruit_BNO055::getSensor(sensor_t *sensor)
|
402 |
|
{
|
|
410 |
void Adafruit_BNO055::getSensor(sensor_t *sensor) {
|
403 |
411 |
/* Clear the sensor_t object */
|
404 |
412 |
memset(sensor, 0, sizeof(sensor_t));
|
405 |
413 |
|
406 |
414 |
/* 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;
|
|
415 |
strncpy(sensor->name, "BNO055", sizeof(sensor->name) - 1);
|
|
416 |
sensor->name[sizeof(sensor->name) - 1] = 0;
|
|
417 |
sensor->version = 1;
|
|
418 |
sensor->sensor_id = _sensorID;
|
|
419 |
sensor->type = SENSOR_TYPE_ORIENTATION;
|
|
420 |
sensor->min_delay = 0;
|
|
421 |
sensor->max_value = 0.0F;
|
|
422 |
sensor->min_value = 0.0F;
|
|
423 |
sensor->resolution = 0.01F;
|
416 |
424 |
}
|
417 |
425 |
|
418 |
426 |
/**************************************************************************/
|
419 |
427 |
/*!
|
420 |
428 |
@brief Reads the sensor and returns the data as a sensors_event_t
|
|
429 |
@param event
|
|
430 |
@return always returns true
|
421 |
431 |
*/
|
422 |
432 |
/**************************************************************************/
|
423 |
|
bool Adafruit_BNO055::getEvent(sensors_event_t *event)
|
424 |
|
{
|
|
433 |
bool Adafruit_BNO055::getEvent(sensors_event_t *event) {
|
425 |
434 |
/* Clear the event */
|
426 |
435 |
memset(event, 0, sizeof(sensors_event_t));
|
427 |
436 |
|
428 |
|
event->version = sizeof(sensors_event_t);
|
|
437 |
event->version = sizeof(sensors_event_t);
|
429 |
438 |
event->sensor_id = _sensorID;
|
430 |
|
event->type = SENSOR_TYPE_ORIENTATION;
|
|
439 |
event->type = SENSOR_TYPE_ORIENTATION;
|
431 |
440 |
event->timestamp = millis();
|
432 |
441 |
|
433 |
442 |
/* Get a Euler angle sample for orientation */
|
... | ... | |
441 |
450 |
|
442 |
451 |
/**************************************************************************/
|
443 |
452 |
/*!
|
444 |
|
@brief Reads the sensor's offset registers into a byte array
|
|
453 |
@brief Reads the sensor's offset registers into a byte array
|
|
454 |
@param calibData
|
|
455 |
@return true if read is successful
|
445 |
456 |
*/
|
446 |
457 |
/**************************************************************************/
|
447 |
|
bool Adafruit_BNO055::getSensorOffsets(uint8_t* calibData)
|
448 |
|
{
|
449 |
|
if (isFullyCalibrated())
|
450 |
|
{
|
451 |
|
adafruit_bno055_opmode_t lastMode = _mode;
|
452 |
|
setMode(OPERATION_MODE_CONFIG);
|
|
458 |
bool Adafruit_BNO055::getSensorOffsets(uint8_t *calibData) {
|
|
459 |
if (isFullyCalibrated()) {
|
|
460 |
adafruit_bno055_opmode_t lastMode = _mode;
|
|
461 |
setMode(OPERATION_MODE_CONFIG);
|
453 |
462 |
|
454 |
|
readLen(ACCEL_OFFSET_X_LSB_ADDR, calibData, NUM_BNO055_OFFSET_REGISTERS);
|
|
463 |
readLen(ACCEL_OFFSET_X_LSB_ADDR, calibData, NUM_BNO055_OFFSET_REGISTERS);
|
455 |
464 |
|
456 |
|
setMode(lastMode);
|
457 |
|
return true;
|
458 |
|
}
|
459 |
|
return false;
|
|
465 |
setMode(lastMode);
|
|
466 |
return true;
|
|
467 |
}
|
|
468 |
return false;
|
460 |
469 |
}
|
461 |
470 |
|
462 |
471 |
/**************************************************************************/
|
463 |
472 |
/*!
|
464 |
|
@brief Reads the sensor's offset registers into an offset struct
|
|
473 |
@brief Reads the sensor's offset registers into an offset struct
|
|
474 |
@param offsets_type
|
|
475 |
@return true if read is successful
|
465 |
476 |
*/
|
466 |
477 |
/**************************************************************************/
|
467 |
|
bool Adafruit_BNO055::getSensorOffsets(adafruit_bno055_offsets_t &offsets_type)
|
468 |
|
{
|
469 |
|
if (isFullyCalibrated())
|
470 |
|
{
|
471 |
|
adafruit_bno055_opmode_t lastMode = _mode;
|
472 |
|
setMode(OPERATION_MODE_CONFIG);
|
473 |
|
delay(25);
|
474 |
|
|
475 |
|
/* 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 |
|
offsets_type.accel_offset_x = (read8(ACCEL_OFFSET_X_MSB_ADDR) << 8) | (read8(ACCEL_OFFSET_X_LSB_ADDR));
|
481 |
|
|