aaa
Dependencies: mbed BNO055_fusion Adafruit_GFX ros_lib_kinetic
odometry/myBNO055.h@12:f726eb78b54c, 2019-08-30 (annotated)
- Committer:
- nakedt555
- Date:
- Fri Aug 30 07:55:38 2019 +0000
- Revision:
- 12:f726eb78b54c
- Parent:
- 3:a45557a0dcb8
aaa
Who changed what in which revision?
User | Revision | Line number | New 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 |