駿 高世 / Mbed 2 deprecated abu2019_mr1

aaa

Dependencies:   mbed BNO055_fusion Adafruit_GFX ros_lib_kinetic

odometry/myBNO055.h@3:a45557a0dcb8, 2018-12-11 (annotated)

Committer:
nakedt555
Date:
Tue Dec 11 17:51:47 2018 +0000
Revision:
3:a45557a0dcb8
Parent:
myBNO055.h@2:086272a2da1c 
12/10iikanji

Who changed what in which revision?

UserRevisionLine numberNew contents of line
nakedt555 2:086272a2da1c 1 #ifndef _MY_BNO055_H_
nakedt555 2:086272a2da1c 2 #define _MY_BNO055_H_
nakedt555 2:086272a2da1c 3
nakedt555 2:086272a2da1c 4 #include "BNO055.h"
nakedt555 2:086272a2da1c 5 #include "type.h"
nakedt555 2:086272a2da1c 6
nakedt555 2:086272a2da1c 7 class My_BNO055 : public BNO055
nakedt555 2:086272a2da1c 8 {
nakedt555 2:086272a2da1c 9 public:
nakedt555 2:086272a2da1c 10 My_BNO055 (PinName p_sda, PinName p_scl, PinName p_reset, uint8_t addr, uint8_t mode)
nakedt555 2:086272a2da1c 11 :BNO055(p_sda, p_scl, p_reset, addr, mode){
nakedt555 2:086272a2da1c 12 }
nakedt555 2:086272a2da1c 13
nakedt555 2:086272a2da1c 14 Vec4f get_quaternion(){
nakedt555 2:086272a2da1c 15 BNO055_QUATERNION_TypeDef qua;
nakedt555 2:086272a2da1c 16 BNO055::get_quaternion(&qua);
nakedt555 2:086272a2da1c 17 return Vec4f(qua.x / 16384.0, qua.y / 16384.0, qua.z / 16384.0, qua.w / 16384.0);
nakedt555 2:086272a2da1c 18 }
nakedt555 2:086272a2da1c 19
nakedt555 3:a45557a0dcb8 20 Vec3f get_radian(){
nakedt555 2:086272a2da1c 21 Vec4f qua = get_quaternion();
nakedt555 2:086272a2da1c 22 float x, y, z;
nakedt555 2:086272a2da1c 23
nakedt555 2:086272a2da1c 24 double ysqr = qua.y() * qua.y();
nakedt555 2:086272a2da1c 25
nakedt555 2:086272a2da1c 26 // roll (x-axis rotation)
nakedt555 2:086272a2da1c 27 double t0 = +2.0 * (qua.w() * qua.x() + qua.y() * qua.z());
nakedt555 2:086272a2da1c 28 double t1 = +1.0 - 2.0 * (qua.x() * qua.x() + ysqr);
nakedt555 3:a45557a0dcb8 29 x = std::atan2(t0, t1);
nakedt555 2:086272a2da1c 30
nakedt555 2:086272a2da1c 31 // pitch (y-axis rotation)
nakedt555 2:086272a2da1c 32 double t2 = +2.0 * (qua.w() * qua.y() - qua.z() * qua.x());
nakedt555 2:086272a2da1c 33 t2 = t2 > 1.0 ? 1.0 : t2;
nakedt555 2:086272a2da1c 34 t2 = t2 < -1.0 ? -1.0 : t2;
nakedt555 3:a45557a0dcb8 35 y = std::asin(t2);
nakedt555 2:086272a2da1c 36
nakedt555 2:086272a2da1c 37 // yaw (z-axis rotation)
nakedt555 2:086272a2da1c 38 double t3 = +2.0 * (qua.w() * qua.z() + qua.x() * qua.y());
nakedt555 2:086272a2da1c 39 double t4 = +1.0 - 2.0 * (ysqr + qua.z() * qua.z());
nakedt555 3:a45557a0dcb8 40 z = std::atan2(t3, t4);
nakedt555 2:086272a2da1c 41
nakedt555 2:086272a2da1c 42 return Vec3f(x, y, z);
nakedt555 2:086272a2da1c 43 }
nakedt555 2:086272a2da1c 44 };
nakedt555 2:086272a2da1c 45
nakedt555 2:086272a2da1c 46 #endif