Antoine Laurens / Mbed 2 deprecated STRAIGHT_DRIVE_NO_SEP

drive down

Dependencies:   BMP280 BNO055_fusion PowerControl mbed

Fork of TEAM_G_FLOW_RIDA by Edwin Cho

Diff: LOCALIZE.cpp

Revision:
20:10a305298e27
Parent:
19:2dd81b864e14
Child:
23:455f7da3dd7a
--- a/LOCALIZE.cpp	Sun Apr 03 05:48:37 2016 +0000
+++ b/LOCALIZE.cpp	Sun Apr 03 06:08:08 2016 +0000
@@ -41,7 +41,7 @@
 {
     _imu.get_Euler_Angles(&_euler);
     _imu.get_gravity(&_gravity);
-    unsigned int quad=0;
+    volatile unsigned int quad=0;
     if(_gravity.y>=0)
         quad&=~1;
     else
@@ -115,6 +115,12 @@
         _rx=_xya.x;
         _ry=_xya.x;
     }
+    if(abs(_rx-_xya.x)>UR_JUMP) {
+        _rx=_xya.x;
+    }
+    if(abs(_ry-_xya.y)>UR_JUMP) {
+        _ry=_xya.y;
+    }
     _xya.x=_rx;
     _xya.y=_ry;
     xya->x=_xya.x;
@@ -138,7 +144,7 @@
     _ry_p=data[1];
 }
 
-void LOCALIZE::ledSw(void)
+inline void LOCALIZE::ledSw(void)
 {
     _led1=!_sw1;
     _led2=!_sw2;