version2
Dependencies: BNO055_fusion mbed
Fork of DEMO2 by
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); }