BNO055 fusion library. This fork adds angular velocity method.
Fork of BNO055_fusion by
Diff: BNO055.cpp
- Revision:
- 5:1c085cc12d85
- Parent:
- 4:9e6fead1e93e
- Child:
- 6:662c92c84505
diff -r 9e6fead1e93e -r 1c085cc12d85 BNO055.cpp --- a/BNO055.cpp Thu Apr 16 10:47:40 2015 +0000 +++ b/BNO055.cpp Mon Apr 10 19:52:37 2017 +0000 @@ -95,6 +95,37 @@ qua->z = dt[7] << 8 | dt[6]; } +void BNO055::get_velocities(BNO055_VEL_TypeDef *el) +{ + uint8_t deg_or_rad; + int16_t x,y,z; + + select_page(0); + dt[0] = BNO055_UNIT_SEL; + _i2c.write(chip_addr, dt, 1, true); + _i2c.read(chip_addr, dt, 1, false); + if (dt[0] & 0x02) { + deg_or_rad = 1; // Radian per sec + } else { + deg_or_rad = 0; // Degree per sec + } + dt[0] = BNO055_GYRO_X_LSB; + _i2c.write(chip_addr, dt, 1, true); + _i2c.read(chip_addr, dt, 6, false); + x = dt[1] << 8 | dt[0]; + y = dt[3] << 8 | dt[2]; + z = dt[5] << 8 | dt[4]; + if (deg_or_rad) { + el->x = (double)x / 900; + el->y = (double)y / 900; + el->z = (double)z / 900; + } else { + el->x = (double)x / 16; + el->y = (double)y / 16; + el->z = (double)z / 16; + } +} + void BNO055::get_linear_accel(BNO055_LIN_ACC_TypeDef *la) { uint8_t ms2_or_mg; @@ -220,7 +251,7 @@ { select_page(0); dt[0] = BNO055_UNIT_SEL; - dt[1] = UNIT_ORI_WIN + UNIT_ACC_MSS + UNIT_GYR_DPS + UNIT_EULER_DEG + UNIT_TEMP_C; + dt[1] = UNIT_ORI_WIN + UNIT_ACC_MSS + UNIT_GYR_RPS + UNIT_EULER_RAD + UNIT_TEMP_C; _i2c.write(chip_addr, dt, 2, false); }