From b2bce55cb6ec71d479e8d9198f23ddf728f474f0 Mon Sep 17 00:00:00 2001
From: Hoop77 <p.badenhoop@gmx.de>
Date: Thu, 10 May 2018 15:55:33 +0200
Subject: [PATCH] ultrasonic returns MAX_DISTANCE instance of 0
---
.../src/VeloxProtocolLib/test/TerminalControl.cpp | 2 +-
modules/catkin_ws/src/VeloxProtocolLib/test/USS_SRF02.cpp | 6 +++++-
modules/catkin_ws/src/VeloxProtocolLib/test/USS_SRF02.h | 2 ++
modules/catkin_ws/src/car/include/ultrasonic/USS_SRF02.h | 2 ++
modules/catkin_ws/src/car/src/ultrasonic/USS_SRF02.cpp | 8 ++++++--
modules/catkin_ws/src/car/src/ultrasonic/Ultrasonic.cpp | 2 --
6 files changed, 16 insertions(+), 6 deletions(-)
diff --git a/modules/catkin_ws/src/VeloxProtocolLib/test/TerminalControl.cpp b/modules/catkin_ws/src/VeloxProtocolLib/test/TerminalControl.cpp
index 5aa457dc..cbb8c8d0 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 aa0084d5..6419817b 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 698f5bb8..b3fb842e 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 698f5bb8..b3fb842e 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 6cda3f79..6419817b 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 3e8df4b9..1c964212 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");
--
GitLab