aaa
Dependencies: mbed BNO055_fusion Adafruit_GFX ros_lib_kinetic
Diff: myBNO055.h
- Revision:
- 2:086272a2da1c
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/myBNO055.h Fri Dec 07 22:34:31 2018 +0000 @@ -0,0 +1,46 @@ +#ifndef _MY_BNO055_H_ +#define _MY_BNO055_H_ + +#include "BNO055.h" +#include "type.h" + +class My_BNO055 : public BNO055 +{ + public: + My_BNO055 (PinName p_sda, PinName p_scl, PinName p_reset, uint8_t addr, uint8_t mode) + :BNO055(p_sda, p_scl, p_reset, addr, mode){ + } + + Vec4f get_quaternion(){ + BNO055_QUATERNION_TypeDef qua; + BNO055::get_quaternion(&qua); + return Vec4f(qua.x / 16384.0, qua.y / 16384.0, qua.z / 16384.0, qua.w / 16384.0); + } + + Vec3f get_angle(){ + Vec4f qua = get_quaternion(); + float x, y, z; + + double ysqr = qua.y() * qua.y(); + + // roll (x-axis rotation) + double t0 = +2.0 * (qua.w() * qua.x() + qua.y() * qua.z()); + double t1 = +1.0 - 2.0 * (qua.x() * qua.x() + ysqr); + x = std::atan2(t0, t1) * 57.2957795; + + // pitch (y-axis rotation) + double t2 = +2.0 * (qua.w() * qua.y() - qua.z() * qua.x()); + t2 = t2 > 1.0 ? 1.0 : t2; + t2 = t2 < -1.0 ? -1.0 : t2; + y = std::asin(t2) * 57.2957795; + + // yaw (z-axis rotation) + double t3 = +2.0 * (qua.w() * qua.z() + qua.x() * qua.y()); + double t4 = +1.0 - 2.0 * (ysqr + qua.z() * qua.z()); + z = std::atan2(t3, t4) * 57.2957795; + + return Vec3f(x, y, z); + } +}; + +#endif \ No newline at end of file