-

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

@@ -53,8 +53,8 @@ void UltrasonicSensor::updateSensor(const AsyncState::Ptr & state)
 int UltrasonicSensor::readDistance() const
 {
     //wiringPiI2CWrite(fd, 0x02);
-    int distance = wiringPiI2CReadReg16(fd, RESULT_LOW_BYTE);
-    distance += wiringPiI2CReadReg16(fd, RESULT_HIGH_BYTE) << 8;
+    int distance = wiringPiI2CReadReg8(fd, RESULT_LOW_BYTE);
+    distance += wiringPiI2CReadReg8(fd, RESULT_HIGH_BYTE) << 8;
     //if (distance == 0)  // 0 means distance too far
         //distance = 255;
     return distance;