Revision 48741e1f
Adafruit_BNO055.cpp | ||
---|---|---|
215 | 215 |
|
216 | 216 |
/**************************************************************************/ |
217 | 217 |
/*! |
218 |
@brief Gets a new heading/roll/pitch sample in Euler angles
|
|
218 |
@brief Gets a vector reading from the specified source
|
|
219 | 219 |
*/ |
220 | 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) |
|
221 |
imu::Vector<3> Adafruit_BNO055::getVector(adafruit_vector_type_t vector_type) |
|
250 | 222 |
{ |
251 | 223 |
imu::Vector<3> xyz; |
252 | 224 |
uint8_t buffer[6]; |
... | ... | |
255 | 227 |
int16_t x, y, z; |
256 | 228 |
x = y = z = 0; |
257 | 229 |
|
258 |
/* Read accel data (6 bytes) */
|
|
259 |
readLen(BNO055_ACCEL_DATA_X_LSB_ADDR, buffer, 6);
|
|
230 |
/* Read vector data (6 bytes) */
|
|
231 |
readLen((adafruit_bno055_reg_t)vector_type, buffer, 6);
|
|
260 | 232 |
x = (((uint16_t)buffer[1]) << 8) | ((uint16_t)buffer[0]); |
261 | 233 |
y = (((uint16_t)buffer[3]) << 8) | ((uint16_t)buffer[2]); |
262 | 234 |
z = (((uint16_t)buffer[5]) << 8) | ((uint16_t)buffer[4]); |
... | ... | |
271 | 243 |
|
272 | 244 |
/**************************************************************************/ |
273 | 245 |
/*! |
246 |
@brief Gets a quaternion reading from the specified source |
|
247 |
*/ |
|
248 |
/**************************************************************************/ |
|
249 |
imu::Quaternion Adafruit_BNO055::getQuat(void) |
|
250 |
{ |
|
251 |
uint8_t buffer[8]; |
|
252 |
memset (buffer, 0, 8); |
|
253 |
|
|
254 |
int16_t x, y, z, w; |
|
255 |
x = y = z = w = 0; |
|
256 |
|
|
257 |
/* Read quat data (8 bytes) */ |
|
258 |
readLen(BNO055_QUATERNION_DATA_W_LSB_ADDR, buffer, 8); |
|
259 |
w = (((uint16_t)buffer[1]) << 8) | ((uint16_t)buffer[0]); |
|
260 |
x = (((uint16_t)buffer[3]) << 8) | ((uint16_t)buffer[2]); |
|
261 |
y = (((uint16_t)buffer[5]) << 8) | ((uint16_t)buffer[4]); |
|
262 |
z = (((uint16_t)buffer[7]) << 8) | ((uint16_t)buffer[6]); |
|
263 |
|
|
264 |
/* Assign to Quaternion */ |
|
265 |
imu::Quaternion quat((double)w, (double)x, (double)y, (double)z); |
|
266 |
return quat; |
|
267 |
} |
|
268 |
|
|
269 |
/**************************************************************************/ |
|
270 |
/*! |
|
274 | 271 |
@brief Provides the sensor_t data for this sensor |
275 | 272 |
*/ |
276 | 273 |
/**************************************************************************/ |
Adafruit_BNO055.h | ||
---|---|---|
227 | 227 |
uint8_t self_test_result; |
228 | 228 |
uint8_t system_error; |
229 | 229 |
} adafruit_bno055_system_status_t; |
230 |
|
|
231 |
typedef enum |
|
232 |
{ |
|
233 |
VECTOR_ACCELEROMETER = BNO055_ACCEL_DATA_X_LSB_ADDR, |
|
234 |
VECTOR_MAGNETOMETER = BNO055_MAG_DATA_X_LSB_ADDR, |
|
235 |
VECTOR_GYROSCOPE = BNO055_GYRO_DATA_X_LSB_ADDR, |
|
236 |
VECTOR_EULER = BNO055_EULER_H_LSB_ADDR, |
|
237 |
VECTOR_LINEARACCEL = BNO055_LINEAR_ACCEL_DATA_X_LSB_ADDR, |
|
238 |
VECTOR_GRAVITY = BNO055_GRAVITY_DATA_X_LSB_ADDR |
|
239 |
} adafruit_vector_type_t; |
|
230 | 240 |
|
231 | 241 |
Adafruit_BNO055 ( int32_t sensorID = -1, uint8_t address = BNO055_ADDRESS_A ); |
232 | 242 |
|
... | ... | |
237 | 247 |
void getSystemStatus ( adafruit_bno055_system_status_t* ); |
238 | 248 |
void displaySystemStatus ( void ); |
239 | 249 |
|
240 |
imu::Vector<3> getEuler ( void );
|
|
241 |
imu::Vector<3> getAccel ( void );
|
|
250 |
imu::Vector<3> getVector ( adafruit_vector_type_t vector_type );
|
|
251 |
imu::Quaternion getQuat ( void );
|
|
242 | 252 |
|
243 | 253 |
/* Adafruit_Sensor implementation */ |
244 | 254 |
bool getEvent ( sensors_event_t* ); |
examples/sensorapi/sensorapi.ino | ||
---|---|---|
94 | 94 |
/**************************************************************************/ |
95 | 95 |
void loop(void) |
96 | 96 |
{ |
97 |
imu::Vector<3> euler = bno.getEuler(); |
|
98 |
|
|
97 |
// Possible vector values can be: |
|
98 |
// - VECTOR_ACCELEROMETER |
|
99 |
// - VECTOR_MAGNETOMETER |
|
100 |
// - VECTOR_GYROSCOPE |
|
101 |
// - VECTOR_EULER |
|
102 |
// - VECTOR_LINEARACCEL |
|
103 |
// - VECTOR_GRAVITY |
|
104 |
imu::Vector<3> euler = bno.getVector(Adafruit_BNO055::VECTOR_EULER); |
|
99 | 105 |
Serial.print("X: "); |
100 | 106 |
Serial.println((int)euler.x(), DEC); |
101 | 107 |
Serial.print("Y: "); |
... | ... | |
103 | 109 |
Serial.print("Z: "); |
104 | 110 |
Serial.println((int)euler.z(), DEC); |
105 | 111 |
Serial.println(""); |
112 |
|
|
113 |
imu::Quaternion quat = bno.getQuat(); |
|
114 |
Serial.print("qW: "); |
|
115 |
Serial.println((int)quat.w(), DEC); |
|
116 |
Serial.print("qX: "); |
|
117 |
Serial.println((int)quat.x(), DEC); |
|
118 |
Serial.print("qY: "); |
|
119 |
Serial.println((int)quat.y(), DEC); |
|
120 |
Serial.print("qZ: "); |
|
121 |
Serial.println((int)quat.z(), DEC); |
|
122 |
Serial.println(""); |
|
106 | 123 |
|
107 | 124 |
delay(BNO055_SAMPLERATE_DELAY_MS); |
108 |
} |
|
125 |
} |
Also available in: Unified diff