Diff: odometry/myBNO055.h

Revision:

3:a45557a0dcb8

Parent:

2:086272a2da1c

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/odometry/myBNO055.h	Tue Dec 11 17:51:47 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_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
\ No newline at end of file