File content as of revision 3:a45557a0dcb8:

#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_radian(){
            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);
            
            // 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);
            
            // 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);
            
            return Vec3f(x, y, z);
        }
};

#endif