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 |
}
|