ultrasonic returns MAX_DISTANCE instance of 0

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);
 

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;
 }

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();

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();

modules/catkin_ws/src/car/src/ultrasonic/USS_SRF02.cpp

-#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;
 }

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");