Diff: LOCALIZE.cpp

Revision:

8:b36be08c44f8

Parent:

6:0602a9e8118b

Child:

9:4fc5d70c3bab

--- a/LOCALIZE.cpp	Thu Mar 17 23:20:22 2016 +0000
+++ b/LOCALIZE.cpp	Wed Mar 23 06:55:39 2016 +0000
@@ -53,9 +53,10 @@
             _angle=-1;
             break;
     }
-    _xya.a=_angle;
+    //_xya.a=_angle-FRAME_OFF;
     //xya->a=_xya.a;
     xya->a=_euler.h;
+    _xya.a=xya->a;
 }
 
 void LOCALIZE::get_xy(LOCALIZE_xya *xya)
@@ -91,15 +92,15 @@
     char data[]= {0,0,0};
     _x_imu_i2c.read(R_P_ADDR<<1, data, 3);
     wait(R_WAIT);
-    _rx_p=data[1];
+    _rx_n=data[1];
     _x_imu_i2c.read(R_N_ADDR<<1, data, 3);
     wait(R_WAIT);
-    _rx_n=data[1];
+    _rx_p=data[1];
     _y_i2c.read(R_P_ADDR<<1, data, 3);
     wait(R_WAIT);
-    _ry_p=data[1];
+    _ry_n=data[1];
     _y_i2c.read(R_N_ADDR<<1, data, 3);
     wait(R_WAIT);
-    _ry_n=data[1];
+    _ry_p=data[1];
     //printf("%d %d %d %d\n",_rx_p,_rx_n,_ry_p,_ry_n);
 }