|
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 |
------> http://www.adafruit.com/products
|
|
8 |
|
|
9 |
These sensors use I2C to communicate, 2 pins are required to interface.
|
|
10 |
|
|
11 |
Adafruit invests time and resources providing this open source code,
|
|
12 |
please support Adafruit andopen-source hardware by purchasing products
|
|
13 |
from Adafruit!
|
|
14 |
|
|
15 |
Written by KTOWN for Adafruit Industries.
|
|
16 |
|
|
17 |
MIT license, all text above must be included in any redistribution
|
|
18 |
***************************************************************************/
|
|
19 |
|
|
20 |
#if ARDUINO >= 100
|
|
21 |
#include "Arduino.h"
|
|
22 |
#else
|
|
23 |
#include "WProgram.h"
|
|
24 |
#endif
|
|
25 |
|
|
26 |
#include <math.h>
|
|
27 |
#include <limits.h>
|
|
28 |
|
|
29 |
#include "Adafruit_BNO055.h"
|
|
30 |
|
|
31 |
/***************************************************************************
|
|
32 |
CONSTRUCTOR
|
|
33 |
***************************************************************************/
|
|
34 |
|
|
35 |
/**************************************************************************/
|
|
36 |
/*!
|
|
37 |
@brief Instantiates a new Adafruit_BNO055 class
|
|
38 |
*/
|
|
39 |
/**************************************************************************/
|
|
40 |
Adafruit_BNO055::Adafruit_BNO055(int32_t sensorID, uint8_t address)
|
|
41 |
{
|
|
42 |
_sensorID = sensorID;
|
|
43 |
_address = address;
|
|
44 |
}
|
|
45 |
|
|
46 |
/***************************************************************************
|
|
47 |
PUBLIC FUNCTIONS
|
|
48 |
***************************************************************************/
|
|
49 |
|
|
50 |
/**************************************************************************/
|
|
51 |
/*!
|
|
52 |
@brief Sets up the HW
|
|
53 |
*/
|
|
54 |
/**************************************************************************/
|
|
55 |
bool Adafruit_BNO055::begin(adafruit_bno055_opmode_t mode)
|
|
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 |
return false;
|
|
65 |
}
|
|
66 |
|
|
67 |
/* Switch to config mode (just in case since this is the default) */
|
|
68 |
setMode(OPERATION_MODE_CONFIG);
|
|
69 |
|
|
70 |
/* Set to normal power mode */
|
|
71 |
write8(BNO055_PWR_MODE_ADDR, POWER_MODE_NORMAL);
|
|
72 |
delay(10);
|
|
73 |
|
|
74 |
/* Set the requested operating mode (see section 3.3) */
|
|
75 |
write8(BNO055_OPR_MODE_ADDR, mode);
|
|
76 |
delay(20);
|
|
77 |
|
|
78 |
return true;
|
|
79 |
}
|
|
80 |
|
|
81 |
/**************************************************************************/
|
|
82 |
/*!
|
|
83 |
@brief Puts the chip in the specified operating mode
|
|
84 |
*/
|
|
85 |
/**************************************************************************/
|
|
86 |
void Adafruit_BNO055::setMode(adafruit_bno055_opmode_t mode)
|
|
87 |
{
|
|
88 |
_mode = mode;
|
|
89 |
|
|
90 |
write8(BNO055_OPR_MODE_ADDR, _mode);
|
|
91 |
delay(30);
|
|
92 |
}
|
|
93 |
|
|
94 |
/**************************************************************************/
|
|
95 |
/*!
|
|
96 |
@brief Gets the latest system status info
|
|
97 |
*/
|
|
98 |
/**************************************************************************/
|
|
99 |
void Adafruit_BNO055::getSystemStatus(adafruit_bno055_system_status_t * status)
|
|
100 |
{
|
|
101 |
memset(status, 0, sizeof(adafruit_bno055_system_status_t));
|
|
102 |
|
|
103 |
/* Read the system status register */
|
|
104 |
status->system_status = read8(BNO055_SYS_STAT_ADDR);
|
|
105 |
status->self_test_result = read8(BNO055_SELFTEST_RESULT_ADDR);
|
|
106 |
status->system_error = read8(BNO055_SYS_ERR_ADDR);
|
|
107 |
}
|
|
108 |
|
|
109 |
/**************************************************************************/
|
|
110 |
/*!
|
|
111 |
@brief Displays system status info via Serial.print
|
|
112 |
*/
|
|
113 |
/**************************************************************************/
|
|
114 |
void Adafruit_BNO055::displaySystemStatus(void)
|
|
115 |
{
|
|
116 |
adafruit_bno055_system_status_t status;
|
|
117 |
getSystemStatus(&status);
|
|
118 |
|
|
119 |
/* System Status (see section 4.3.58)
|
|
120 |
---------------------------------
|
|
121 |
0 = Idle
|
|
122 |
1 = System Error
|
|
123 |
2 = Initializing Peripherals
|
|
124 |
3 = System Iniitalization
|
|
125 |
4 = Executing Self-Test
|
|
126 |
5 = Sensor fusio algorithm running
|
|
127 |
6 = System running without fusion algorithms */
|
|
128 |
|
|
129 |
Serial.print("System Status: 0x");
|
|
130 |
Serial.println(status.system_status, HEX);
|
|
131 |
|
|
132 |
/* Self Test Results (see section )
|
|
133 |
--------------------------------
|
|
134 |
1 = test passed, 0 = test failed
|
|
135 |
|
|
136 |
Bit 0 = Accelerometer self test
|
|
137 |
Bit 1 = Magnetometer self test
|
|
138 |
Bit 2 = Gyroscope self test
|
|
139 |
Bit 3 = MCU self test
|
|
140 |
|
|
141 |
0x0F = all good! */
|
|
142 |
|
|
143 |
Serial.print("Self Test Results: 0x");
|
|
144 |
Serial.println(status.self_test_result, HEX);
|
|
145 |
|
|
146 |
/* System Error (see section 4.3.59)
|
|
147 |
---------------------------------
|
|
148 |
0 = No error
|
|
149 |
1 = Peripheral initialization error
|
|
150 |
2 = System initialization error
|
|
151 |
3 = Self test result failed
|
|
152 |
4 = Register map value out of range
|
|
153 |
5 = Register map address out of range
|
|
154 |
6 = Register map write error
|
|
155 |
7 = BNO low power mode not available for selected operat ion mode
|
|
156 |
8 = Accelerometer power mode not available
|
|
157 |
9 = Fusion algorithm configuration error
|
|
158 |
A = Sensor configuration error */
|
|
159 |
|
|
160 |
Serial.print("System Error: 0x");
|
|
161 |
Serial.println(status.system_error, HEX);
|
|
162 |
}
|
|
163 |
|
|
164 |
/**************************************************************************/
|
|
165 |
/*!
|
|
166 |
@brief Gets the chip revision numbers
|
|
167 |
*/
|
|
168 |
/**************************************************************************/
|
|
169 |
void Adafruit_BNO055::getRevInfo(adafruit_bno055_rev_info_t* info)
|
|
170 |
{
|
|
171 |
uint8_t a, b;
|
|
172 |
|
|
173 |
memset(info, 0, sizeof(adafruit_bno055_rev_info_t));
|
|
174 |
|
|
175 |
info->accel_rev = read8(BNO055_ACCEL_REV_ID_ADDR);
|
|
176 |
info->mag_rev = read8(BNO055_MAG_REV_ID_ADDR);
|
|
177 |
info->gyro_rev = read8(BNO055_GYRO_REV_ID_ADDR);
|
|
178 |
info->bl_rev = read8(BNO055_BL_REV_ID_ADDR);
|
|
179 |
|
|
180 |
a = read8(BNO055_SW_REV_ID_LSB_ADDR);
|
|
181 |
b = read8(BNO055_SW_REV_ID_MSB_ADDR);
|
|
182 |
info->sw_rev = (((uint16_t)b) << 8) | ((uint16_t)a);
|
|
183 |
}
|
|
184 |
|
|
185 |
/**************************************************************************/
|
|
186 |
/*!
|
|
187 |
@brief Displays the chip revision numbers via Serial.print
|
|
188 |
*/
|
|
189 |
/**************************************************************************/
|
|
190 |
void Adafruit_BNO055::displayRevInfo(void)
|
|
191 |
{
|
|
192 |
adafruit_bno055_rev_info_t info;
|
|
193 |
getRevInfo(&info);
|
|
194 |
|
|
195 |
/* Check the accelerometer revision */
|
|
196 |
Serial.print("Accelerometer Revision: 0x");
|
|
197 |
Serial.println(info.accel_rev, HEX);
|
|
198 |
|
|
199 |
/* Check the magnetometer revision */
|
|
200 |
Serial.print("Magnetometer Revision: 0x");
|
|
201 |
Serial.println(info.mag_rev, HEX);
|
|
202 |
|
|
203 |
/* Check the gyroscope revision */
|
|
204 |
Serial.print("Gyroscope Revision: 0x");
|
|
205 |
Serial.println(info.gyro_rev, HEX);
|
|
206 |
|
|
207 |
/* Check the SW revision */
|
|
208 |
Serial.print("SW Revision: 0x");
|
|
209 |
Serial.println(info.sw_rev, HEX);
|
|
210 |
|
|
211 |
/* Check the bootloader revision */
|
|
212 |
Serial.print("Bootloader Revision: 0x");
|
|
213 |
Serial.println(info.bl_rev, HEX);
|
|
214 |
}
|
|
215 |
|
|
216 |
/**************************************************************************/
|
|
217 |
/*!
|
|
218 |
@brief Gets a new heading/roll/pitch sample in Euler angles
|
|
219 |
*/
|
|
220 |
/**************************************************************************/
|
|
221 |
imu::Vector<3> Adafruit_BNO055::getEuler(void)
|
|
222 |
{
|
|
223 |
imu::Vector<3> hrp;
|
|
224 |
uint8_t buffer[6];
|
|
225 |
memset (buffer, 0, 6);
|
|
226 |
|
|
227 |
int16_t h, r, p;
|
|
228 |
h = r = p = 0;
|
|
229 |
|
|
230 |
/* Read HRP data (6 bytes) */
|
|
231 |
readLen(BNO055_EULER_H_LSB_ADDR, buffer, 6);
|
|
232 |
h = (((uint16_t)buffer[1]) << 8) | ((uint16_t)buffer[0]);
|
|
233 |
r = (((uint16_t)buffer[3]) << 8) | ((uint16_t)buffer[2]);
|
|
234 |
p = (((uint16_t)buffer[5]) << 8) | ((uint16_t)buffer[4]);
|
|
235 |
|
|
236 |
/* Assign to Vector */
|
|
237 |
hrp[0] = (double)h;
|
|
238 |
hrp[1] = (double)r;
|
|
239 |
hrp[2] = (double)p;
|
|
240 |
|
|
241 |
return hrp;
|
|
242 |
}
|
|
243 |
|
|
244 |
/**************************************************************************/
|
|
245 |
/*!
|
|
246 |
@brief Gets a new accelerometer sample
|
|
247 |
*/
|
|
248 |
/**************************************************************************/
|
|
249 |
imu::Vector<3> Adafruit_BNO055::getAccel(void)
|
|
250 |
{
|
|
251 |
imu::Vector<3> xyz;
|
|
252 |
uint8_t buffer[6];
|
|
253 |
memset (buffer, 0, 6);
|
|
254 |
|
|
255 |
int16_t x, y, z;
|
|
256 |
x = y = z = 0;
|
|
257 |
|
|
258 |
/* Read accel data (6 bytes) */
|
|
259 |
readLen(BNO055_ACCEL_DATA_X_LSB_ADDR, buffer, 6);
|
|
260 |
x = (((uint16_t)buffer[1]) << 8) | ((uint16_t)buffer[0]);
|
|
261 |
y = (((uint16_t)buffer[3]) << 8) | ((uint16_t)buffer[2]);
|
|
262 |
z = (((uint16_t)buffer[5]) << 8) | ((uint16_t)buffer[4]);
|
|
263 |
|
|
264 |
/* Assign to Vector */
|
|
265 |
xyz[0] = (double)x;
|
|
266 |
xyz[1] = (double)y;
|
|
267 |
xyz[2] = (double)z;
|
|
268 |
|
|
269 |
return xyz;
|
|
270 |
}
|
|
271 |
|
|
272 |
/**************************************************************************/
|
|
273 |
/*!
|
|
274 |
@brief Provides the sensor_t data for this sensor
|
|
275 |
*/
|
|
276 |
/**************************************************************************/
|
|
277 |
void Adafruit_BNO055::getSensor(sensor_t *sensor)
|
|
278 |
{
|
|
279 |
/* Clear the sensor_t object */
|
|
280 |
memset(sensor, 0, sizeof(sensor_t));
|
|
281 |
|
|
282 |
/* Insert the sensor name in the fixed length char array */
|
|
283 |
strncpy (sensor->name, "BNO055", sizeof(sensor->name) - 1);
|
|
284 |
sensor->name[sizeof(sensor->name)- 1] = 0;
|
|
285 |
sensor->version = 1;
|
|
286 |
sensor->sensor_id = _sensorID;
|
|
287 |
sensor->type = SENSOR_TYPE_ORIENTATION;
|
|
288 |
sensor->min_delay = 0;
|
|
289 |
sensor->max_value = 0.0F;
|
|
290 |
sensor->min_value = 0.0F;
|
|
291 |
sensor->resolution = 0.01F;
|
|
292 |
}
|
|
293 |
|
|
294 |
/**************************************************************************/
|
|
295 |
/*!
|
|
296 |
@brief Reads the sensor and returns the data as a sensors_event_t
|
|
297 |
*/
|
|
298 |
/**************************************************************************/
|
|
299 |
bool Adafruit_BNO055::getEvent(sensors_event_t *event)
|
|
300 |
{
|
|
301 |
float orientation;
|
|
302 |
|
|
303 |
/* Clear the event */
|
|
304 |
memset(event, 0, sizeof(sensors_event_t));
|
|
305 |
|
|
306 |
event->version = sizeof(sensors_event_t);
|
|
307 |
event->sensor_id = _sensorID;
|
|
308 |
event->type = SENSOR_TYPE_ORIENTATION;
|
|
309 |
event->timestamp = 0;
|
|
310 |
/*
|
|
311 |
getPressure(&pressure_kPa);
|
|
312 |
event->pressure = pressure_kPa / 100.0F;
|
|
313 |
*/
|
|
314 |
|
|
315 |
return true;
|
|
316 |
}
|
|
317 |
|
|
318 |
/***************************************************************************
|
|
319 |
PRIVATE FUNCTIONS
|
|
320 |
***************************************************************************/
|
|
321 |
|
|
322 |
/**************************************************************************/
|
|
323 |
/*!
|
|
324 |
@brief Writes an 8 bit value over I2C
|
|
325 |
*/
|
|
326 |
/**************************************************************************/
|
|
327 |
bool Adafruit_BNO055::write8(adafruit_bno055_reg_t reg, byte value)
|
|
328 |
{
|
|
329 |
Wire.beginTransmission(_address);
|
|
330 |
#if ARDUINO >= 100
|
|
331 |
Wire.write((uint8_t)reg);
|
|
332 |
Wire.write((uint8_t)value);
|
|
333 |
#else
|
|
334 |
Wire.send(reg);
|
|
335 |
Wire.send(value);
|
|
336 |
#endif
|
|
337 |
Wire.endTransmission();
|
|
338 |
|
|
339 |
/* ToDo: Check for error! */
|
|
340 |
return true;
|
|
341 |
}
|
|
342 |
|
|
343 |
/**************************************************************************/
|
|
344 |
/*!
|
|
345 |
@brief Reads an 8 bit value over I2C
|
|
346 |
*/
|
|
347 |
/**************************************************************************/
|
|
348 |
byte Adafruit_BNO055::read8(adafruit_bno055_reg_t reg )
|
|
349 |
{
|
|
350 |
byte value = 0;
|
|
351 |
|
|
352 |
Wire.beginTransmission(_address);
|
|
353 |
#if ARDUINO >= 100
|
|
354 |
Wire.write((uint8_t)reg);
|
|
355 |
#else
|
|
356 |
Wire.send(reg);
|
|
357 |
#endif
|
|
358 |
Wire.endTransmission();
|
|
359 |
Wire.requestFrom(_address, (byte)1);
|
|
360 |
#if ARDUINO >= 100
|
|
361 |
value = Wire.read();
|
|
362 |
#else
|
|
363 |
value = Wire.receive();
|
|
364 |
#endif
|
|
365 |
|
|
366 |
return value;
|
|
367 |
}
|
|
368 |
|
|
369 |
/**************************************************************************/
|
|
370 |
/*!
|
|
371 |
@brief Reads the specified number of bytes over I2C
|
|
372 |
*/
|
|
373 |
/**************************************************************************/
|
|
374 |
bool Adafruit_BNO055::readLen(adafruit_bno055_reg_t reg, byte * buffer, uint8_t len)
|
|
375 |
{
|
|
376 |
Wire.beginTransmission(_address);
|
|
377 |
#if ARDUINO >= 100
|
|
378 |
Wire.write((uint8_t)reg);
|
|
379 |
#else
|
|
380 |
Wire.send(reg);
|
|
381 |
#endif
|
|
382 |
Wire.endTransmission();
|
|
383 |
Wire.requestFrom(_address, (byte)len);
|
|
384 |
|
|
385 |
/* Wait until data is available */
|
|
386 |
while (Wire.available() < len);
|
|
387 |
|
|
388 |
for (uint8_t i = 0; i < len; i++)
|
|
389 |
{
|
|
390 |
#if ARDUINO >= 100
|
|
391 |
buffer[i] = Wire.read();
|
|
392 |
#else
|
|
393 |
buffer[i] = Wire.receive();
|
|
394 |
#endif
|
|
395 |
}
|
|
396 |
|
|
397 |
/* ToDo: Check for errors! */
|
|
398 |
return true;
|
|
399 |
}
|