Revision 48741e1f Adafruit_BNO055.cpp
| 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 |
/**************************************************************************/ |
Also available in: Unified diff