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