myBNO055.h

00001 #ifndef _MY_BNO055_H_
00002 #define _MY_BNO055_H_
00003 
00004 #include "BNO055.h"
00005 #include "type.h"
00006 
00007 class My_BNO055 : public BNO055
00008 {
00009     public:
00010         My_BNO055 (PinName p_sda, PinName p_scl, PinName p_reset, uint8_t addr, uint8_t mode)
00011             :BNO055(p_sda, p_scl, p_reset, addr, mode){
00012         }
00013         
00014         Vec4f get_quaternion(){
00015             BNO055_QUATERNION_TypeDef qua;
00016             BNO055::get_quaternion(&qua);
00017             return Vec4f(qua.x / 16384.0, qua.y / 16384.0, qua.z / 16384.0, qua.w / 16384.0);
00018         }
00019         
00020         Vec3f get_radian(){
00021             Vec4f qua = get_quaternion();
00022             float x, y, z;
00023             
00024             double ysqr = qua.y() * qua.y();
00025         
00026             // roll (x-axis rotation)
00027             double t0 = +2.0 * (qua.w() * qua.x() + qua.y() * qua.z());
00028             double t1 = +1.0 - 2.0 * (qua.x() * qua.x() + ysqr);
00029             x = std::atan2(t0, t1);
00030             
00031             // pitch (y-axis rotation)
00032             double t2 = +2.0 * (qua.w() * qua.y() - qua.z() * qua.x());
00033             t2 = t2 > 1.0 ? 1.0 : t2;
00034             t2 = t2 < -1.0 ? -1.0 : t2;
00035             y = std::asin(t2);
00036             
00037             // yaw (z-axis rotation)
00038             double t3 = +2.0 * (qua.w() * qua.z() + qua.x() * qua.y());
00039             double t4 = +1.0 - 2.0 * (ysqr + qua.z() * qua.z());  
00040             z = std::atan2(t3, t4);
00041             
00042             return Vec3f(x, y, z);
00043         }
00044 };
00045 
00046 #endif