Diff: LOCALIZE.cpp

Revision:

0:96d6eb224379

Child:

1:8966a48ce8d5

diff -r 000000000000 -r 96d6eb224379 LOCALIZE.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/LOCALIZE.cpp	Tue Mar 01 18:20:45 2016 +0000
@@ -0,0 +1,51 @@
+#include "LOCALIZE.h"
+
+LOCALIZE::LOCALIZE (I2C& y_i2c, I2C& x_imu_i2c, PinName imu_reset):
+    _y_i2c(y_i2c), _x_imu_i2c(x_imu_i2c), _imu_reset(imu_reset), _imu(x_imu_i2c, imu_reset)
+{
+
+}
+
+int LOCALIZE::init (void)
+{
+    if(_imu.chip_ready()==0)
+        return ERROR_IMU;
+    _imu.set_mounting_position(MT_P0);
+    _imu.change_fusion_mode(MODE_IMU);
+    return 0;
+}
+
+void LOCALIZE::get_angle(LOCALIZE_xya *xya)
+{
+    _imu.get_Euler_Angles(&euler);
+    _imu.get_gravity(&gravity);
+    int angle=0;
+    unsigned int quad=0;
+    if(gravity.y>=0)
+        quad&=~1;
+    else
+        quad|=1;
+    if(gravity.x>=0)
+        quad&=~(1<<1);
+    else
+        quad|=(1<<1);
+    quad&=3;
+    switch(quad) {
+        case 0:
+            angle=euler.p;
+            break;
+        case 1:
+            angle=180-euler.p;
+            break;
+        case 2:
+            angle=360+euler.p;
+            break;
+        case 3:
+            angle=180+abs(euler.p);
+            break;
+        default:
+            angle=-1;
+            break;
+    }
+    xya->a=angle;
+}