| 31 |
31 |
/***************************************************************************
|
| 32 |
32 |
CONSTRUCTOR
|
| 33 |
33 |
***************************************************************************/
|
| 34 |
|
|
|
34 |
|
| 35 |
35 |
/**************************************************************************/
|
| 36 |
36 |
/*!
|
| 37 |
37 |
@brief Instantiates a new Adafruit_BNO055 class
|
| ... | ... | |
| 78 |
78 |
delay(10);
|
| 79 |
79 |
}
|
| 80 |
80 |
delay(50);
|
| 81 |
|
|
|
81 |
|
| 82 |
82 |
/* Set to normal power mode */
|
| 83 |
83 |
write8(BNO055_PWR_MODE_ADDR, POWER_MODE_NORMAL);
|
| 84 |
84 |
delay(10);
|
| 85 |
85 |
|
| 86 |
86 |
write8(BNO055_PAGE_ID_ADDR, 0);
|
| 87 |
|
|
|
87 |
|
| 88 |
88 |
/* Set the output units */
|
| 89 |
89 |
/*
|
| 90 |
90 |
uint8_t unitsel = (0 << 7) | // Orientation = Android
|
| ... | ... | |
| 156 |
156 |
|
| 157 |
157 |
write8(BNO055_SYS_TRIGGER_ADDR, read8(BNO055_SYS_TRIGGER_ADDR) | 0x1);
|
| 158 |
158 |
delay(1000);
|
| 159 |
|
|
|
159 |
|
| 160 |
160 |
/* System Status (see section 4.3.58)
|
| 161 |
161 |
---------------------------------
|
| 162 |
162 |
0 = Idle
|
| ... | ... | |
| 166 |
166 |
4 = Executing Self-Test
|
| 167 |
167 |
5 = Sensor fusio algorithm running
|
| 168 |
168 |
6 = System running without fusion algorithms */
|
| 169 |
|
|
|
169 |
|
| 170 |
170 |
if (system_status != 0)
|
| 171 |
171 |
*system_status = read8(BNO055_SYS_STAT_ADDR);
|
| 172 |
|
|
|
172 |
|
| 173 |
173 |
/* Self Test Results (see section )
|
| 174 |
174 |
--------------------------------
|
| 175 |
175 |
1 = test passed, 0 = test failed
|
| 176 |
|
|
|
176 |
|
| 177 |
177 |
Bit 0 = Accelerometer self test
|
| 178 |
178 |
Bit 1 = Magnetometer self test
|
| 179 |
179 |
Bit 2 = Gyroscope self test
|
| 180 |
180 |
Bit 3 = MCU self test
|
| 181 |
181 |
|
| 182 |
182 |
0x0F = all good! */
|
| 183 |
|
|
|
183 |
|
| 184 |
184 |
if (self_test_result != 0)
|
| 185 |
185 |
*self_test_result = read8(BNO055_SELFTEST_RESULT_ADDR);
|
| 186 |
186 |
|
| ... | ... | |
| 197 |
197 |
8 = Accelerometer power mode not available
|
| 198 |
198 |
9 = Fusion algorithm configuration error
|
| 199 |
199 |
A = Sensor configuration error */
|
| 200 |
|
|
|
200 |
|
| 201 |
201 |
if (system_error != 0)
|
| 202 |
202 |
*system_error = read8(BNO055_SYS_ERR_ADDR);
|
| 203 |
203 |
|
| ... | ... | |
| 227 |
227 |
|
| 228 |
228 |
/* Check the SW revision */
|
| 229 |
229 |
info->bl_rev = read8(BNO055_BL_REV_ID_ADDR);
|
| 230 |
|
|
|
230 |
|
| 231 |
231 |
a = read8(BNO055_SW_REV_ID_LSB_ADDR);
|
| 232 |
232 |
b = read8(BNO055_SW_REV_ID_MSB_ADDR);
|
| 233 |
233 |
info->sw_rev = (((uint16_t)b) << 8) | ((uint16_t)a);
|
| ... | ... | |
| 235 |
235 |
|
| 236 |
236 |
/**************************************************************************/
|
| 237 |
237 |
/*!
|
| 238 |
|
@brief Gets teh temperature in degrees celsius
|
|
238 |
@brief Gets current calibration state. Each value should be a uint8_t
|
|
239 |
pointer and it will be set to 0 if not calibrated and 3 if
|
|
240 |
fully calibrated.
|
|
241 |
*/
|
|
242 |
/**************************************************************************/
|
|
243 |
void Adafruit_BNO055::getCalibration(uint8_t* sys, uint8_t* gyro, uint8_t* accel, uint8_t* mag) {
|
|
244 |
uint8_t calData = read8(BNO055_CALIB_STAT_ADDR);
|
|
245 |
if (sys != NULL) {
|
|
246 |
*sys = (calData >> 6) & 0x03;
|
|
247 |
}
|
|
248 |
if (gyro != NULL) {
|
|
249 |
*gyro = (calData >> 4) & 0x03;
|
|
250 |
}
|
|
251 |
if (accel != NULL) {
|
|
252 |
*accel = (calData >> 2) & 0x03;
|
|
253 |
}
|
|
254 |
if (mag != NULL) {
|
|
255 |
*mag = calData & 0x03;
|
|
256 |
}
|
|
257 |
}
|
|
258 |
|
|
259 |
/**************************************************************************/
|
|
260 |
/*!
|
|
261 |
@brief Gets the temperature in degrees celsius
|
| 239 |
262 |
*/
|
| 240 |
263 |
/**************************************************************************/
|
| 241 |
264 |
int8_t Adafruit_BNO055::getTemp(void)
|
| ... | ... | |
| 254 |
277 |
imu::Vector<3> xyz;
|
| 255 |
278 |
uint8_t buffer[6];
|
| 256 |
279 |
memset (buffer, 0, 6);
|
| 257 |
|
|
|
280 |
|
| 258 |
281 |
int16_t x, y, z;
|
| 259 |
282 |
x = y = z = 0;
|
| 260 |
|
|
|
283 |
|
| 261 |
284 |
/* Read vector data (6 bytes) */
|
| 262 |
285 |
readLen((adafruit_bno055_reg_t)vector_type, buffer, 6);
|
| 263 |
|
|
|
286 |
|
| 264 |
287 |
x = ((int16_t)buffer[0]) | (((int16_t)buffer[1]) << 8);
|
| 265 |
288 |
y = ((int16_t)buffer[2]) | (((int16_t)buffer[3]) << 8);
|
| 266 |
289 |
z = ((int16_t)buffer[4]) | (((int16_t)buffer[5]) << 8);
|
| ... | ... | |
| 296 |
319 |
xyz[2] = ((double)z)/100.0;
|
| 297 |
320 |
break;
|
| 298 |
321 |
}
|
| 299 |
|
|
|
322 |
|
| 300 |
323 |
return xyz;
|
| 301 |
324 |
}
|
| 302 |
325 |
|
| ... | ... | |
| 309 |
332 |
{
|
| 310 |
333 |
uint8_t buffer[8];
|
| 311 |
334 |
memset (buffer, 0, 8);
|
| 312 |
|
|
|
335 |
|
| 313 |
336 |
int16_t x, y, z, w;
|
| 314 |
337 |
x = y = z = w = 0;
|
| 315 |
|
|
|
338 |
|
| 316 |
339 |
/* Read quat data (8 bytes) */
|
| 317 |
340 |
readLen(BNO055_QUATERNION_DATA_W_LSB_ADDR, buffer, 8);
|
| 318 |
341 |
w = (((uint16_t)buffer[1]) << 8) | ((uint16_t)buffer[0]);
|
| ... | ... | |
| 407 |
430 |
byte Adafruit_BNO055::read8(adafruit_bno055_reg_t reg )
|
| 408 |
431 |
{
|
| 409 |
432 |
byte value = 0;
|
| 410 |
|
|
|
433 |
|
| 411 |
434 |
Wire.beginTransmission(_address);
|
| 412 |
435 |
#if ARDUINO >= 100
|
| 413 |
436 |
Wire.write((uint8_t)reg);
|
| ... | ... | |
| 421 |
444 |
#else
|
| 422 |
445 |
value = Wire.receive();
|
| 423 |
446 |
#endif
|
| 424 |
|
|
|
447 |
|
| 425 |
448 |
return value;
|
| 426 |
449 |
}
|
| 427 |
450 |
|
| ... | ... | |
| 443 |
466 |
|
| 444 |
467 |
/* Wait until data is available */
|
| 445 |
468 |
while (Wire.available() < len);
|
| 446 |
|
|
|
469 |
|
| 447 |
470 |
for (uint8_t i = 0; i < len; i++)
|
| 448 |
471 |
{
|
| 449 |
472 |
#if ARDUINO >= 100
|
| ... | ... | |
| 452 |
475 |
buffer[i] = Wire.receive();
|
| 453 |
476 |
#endif
|
| 454 |
477 |
}
|
| 455 |
|
|
|
478 |
|
| 456 |
479 |
/* ToDo: Check for errors! */
|
| 457 |
480 |
return true;
|
| 458 |
481 |
}
|