diff --git a/modules/catkin_ws/src/VeloxProtocolLib/test/TerminalControl.cpp b/modules/catkin_ws/src/VeloxProtocolLib/test/TerminalControl.cpp
index 5aa457dcea22a49ca98cc732bc9a0e3e6380ef4e..cbb8c8d0855c8797f227e3e99a71ead987dc347d 100644
--- a/modules/catkin_ws/src/VeloxProtocolLib/test/TerminalControl.cpp
+++ b/modules/catkin_ws/src/VeloxProtocolLib/test/TerminalControl.cpp
@@ -39,7 +39,7 @@ int main(int argc, char ** argv)
 
             while (running)
             {
-                int distance = uss.getDistance();
+                int distance = uss.getDistance();;
                 if (distance > 20)
                     conn->setSpeed(0);
 
diff --git a/modules/catkin_ws/src/VeloxProtocolLib/test/USS_SRF02.cpp b/modules/catkin_ws/src/VeloxProtocolLib/test/USS_SRF02.cpp
index aa0084d5096200e0219fcef51df24af579b606e0..6419817b12aba8bd940672364478254449031ce3 100644
--- a/modules/catkin_ws/src/VeloxProtocolLib/test/USS_SRF02.cpp
+++ b/modules/catkin_ws/src/VeloxProtocolLib/test/USS_SRF02.cpp
@@ -16,6 +16,7 @@ constexpr int USS_SRF02::RESULT_HIGH_BYTE;
 constexpr int USS_SRF02::RESULT_LOW_BYTE;
 constexpr int USS_SRF02::RANGING_MODE_CM;
 constexpr int USS_SRF02::DELAY;
+constexpr int USS_SRF02::MAX_DISTANCE;
 
 USS_SRF02::USS_SRF02(int devId)
 {
@@ -28,5 +29,8 @@ int USS_SRF02::getDistance()
 {
     wiringPiI2CWriteReg8(fd, COMMAND_REGISTER, RANGING_MODE_CM);
     usleep(DELAY * 1000);
-    return wiringPiI2CReadReg16(fd, RESULT_LOW_BYTE);
+    int distance = wiringPiI2CReadReg16(fd, RESULT_LOW_BYTE);
+    if (distance == 0)  // 0 means distance too far
+        distance = 255;
+    return distance;
 }
diff --git a/modules/catkin_ws/src/VeloxProtocolLib/test/USS_SRF02.h b/modules/catkin_ws/src/VeloxProtocolLib/test/USS_SRF02.h
index 698f5bb8e48f0eb8864955fa012f2b9e3ba82bc5..b3fb842e02a0ba9ae70c058912629283674aebd5 100644
--- a/modules/catkin_ws/src/VeloxProtocolLib/test/USS_SRF02.h
+++ b/modules/catkin_ws/src/VeloxProtocolLib/test/USS_SRF02.h
@@ -23,6 +23,8 @@ public:
 
     static constexpr int DELAY = 70; //70 ms for ranging to finish
 
+    static constexpr int MAX_DISTANCE = 255;
+
     explicit USS_SRF02(int devId);
 
     int getDistance();
diff --git a/modules/catkin_ws/src/car/include/ultrasonic/USS_SRF02.h b/modules/catkin_ws/src/car/include/ultrasonic/USS_SRF02.h
index 698f5bb8e48f0eb8864955fa012f2b9e3ba82bc5..b3fb842e02a0ba9ae70c058912629283674aebd5 100644
--- a/modules/catkin_ws/src/car/include/ultrasonic/USS_SRF02.h
+++ b/modules/catkin_ws/src/car/include/ultrasonic/USS_SRF02.h
@@ -23,6 +23,8 @@ public:
 
     static constexpr int DELAY = 70; //70 ms for ranging to finish
 
+    static constexpr int MAX_DISTANCE = 255;
+
     explicit USS_SRF02(int devId);
 
     int getDistance();
diff --git a/modules/catkin_ws/src/car/src/ultrasonic/USS_SRF02.cpp b/modules/catkin_ws/src/car/src/ultrasonic/USS_SRF02.cpp
index 6cda3f7905a5d7e006a13337aa2e4404b0db2c5c..6419817b12aba8bd940672364478254449031ce3 100644
--- a/modules/catkin_ws/src/car/src/ultrasonic/USS_SRF02.cpp
+++ b/modules/catkin_ws/src/car/src/ultrasonic/USS_SRF02.cpp
@@ -1,4 +1,4 @@
-#include "ultrasonic/USS_SRF02.h"
+#include "USS_SRF02.h"
 #include "wiringPiI2C.h"
 
 #include <iostream>
@@ -16,6 +16,7 @@ constexpr int USS_SRF02::RESULT_HIGH_BYTE;
 constexpr int USS_SRF02::RESULT_LOW_BYTE;
 constexpr int USS_SRF02::RANGING_MODE_CM;
 constexpr int USS_SRF02::DELAY;
+constexpr int USS_SRF02::MAX_DISTANCE;
 
 USS_SRF02::USS_SRF02(int devId)
 {
@@ -28,5 +29,8 @@ int USS_SRF02::getDistance()
 {
     wiringPiI2CWriteReg8(fd, COMMAND_REGISTER, RANGING_MODE_CM);
     usleep(DELAY * 1000);
-    return wiringPiI2CReadReg16(fd, RESULT_LOW_BYTE);
+    int distance = wiringPiI2CReadReg16(fd, RESULT_LOW_BYTE);
+    if (distance == 0)  // 0 means distance too far
+        distance = 255;
+    return distance;
 }
diff --git a/modules/catkin_ws/src/car/src/ultrasonic/Ultrasonic.cpp b/modules/catkin_ws/src/car/src/ultrasonic/Ultrasonic.cpp
index 3e8df4b9bb1b8755396d6c424bc3925dcb178125..1c9642121c5f00ba9413ba76f8e2e89b29ff63d7 100644
--- a/modules/catkin_ws/src/car/src/ultrasonic/Ultrasonic.cpp
+++ b/modules/catkin_ws/src/car/src/ultrasonic/Ultrasonic.cpp
@@ -16,8 +16,6 @@ Ultrasonic::Ultrasonic()
       , messageOStream(nh, "Ultrasonic")
 {}
 
-static std::size_t counter = 0;
-
 void Ultrasonic::onInit()
 {
     messageOStream.write("onInit", "START");