-

6fcd2a0a · Hoop77 · lenoelda · 3abcaf8f · 6fcd2a0a
Commit 6fcd2a0a authored 6 years ago by Hoop77 Committed by lenoelda 6 years ago
--- a/modules/catkin_ws/src/car/src/ultrasonic/UltrasonicSensor.cpp
+++ b/modules/catkin_ws/src/car/src/ultrasonic/UltrasonicSensor.cpp
@@ -52,8 +52,9 @@ void UltrasonicSensor::updateSensor(const AsyncState::Ptr & state)
 int UltrasonicSensor::readDistance() const
 {
-    wiringPiI2CWrite(fd, 0x02);
+    //wiringPiI2CWrite(fd, 0x02);
-    int distance = wiringPiI2CRead(fd);
+    int distance = wiringPiI2CReadReg16(fd, RESULT_LOW_BYTE);
+    distance += wiringPiI2CReadReg16(fd, RESULT_HIGH_BYTE) << 8;
    //if (distance == 0)  // 0 means distance too far
        //distance = 255;
    return distance;