1 files changed, 16 insertions, 4 deletions

diff --git a/Srf10.cpp b/Srf10.cpp
index 329fc8a..a66828b 100644
--- a/Srf10.cpp
+++ b/Srf10.cpp
@@ -1,6 +1,7 @@
 #include "Srf10.h"
 
 #include "i2c.h"
+#include "timer.h"
 
 
 Srf10::Srf10(uint8_t id)
@@ -13,6 +14,7 @@ Srf10::Srf10(uint8_t id)
 	unit = Inches;
 	
 	distance = 0;
+	has_distance = false;
 	
 	firmware = readFirmware();
 }
@@ -46,15 +48,25 @@ bool Srf10::setRange(uint8_t range) {
 
 long Srf10::updateDistance() {
 	uint8_t data[2] = {0, unit};
+	uint16_t d;
+	
 	
 	if(!I2CSend(id, data, 2)) return -1;
 	
-	while(readFirmware() != 0xFF);
+	do {
+		sleep(1);
+	} while(readFirmware() == 0xFF);
 	
 	if(!I2CSendByte(id, 2)) return -1;
-	if(!I2CRecv(id, data, 2)) return -1;
+	if(I2CRecv(id, data, 2) < 2) return -1;
+	
+	d = (((uint16_t)data[0]) << 8) | data[1];
 	
-	distance = (((uint16_t)data[0]) << 8) | data[1];
+	if(d == 0) has_distance = false;
+	else {
+		has_distance = true;
+		distance = d;
+	}
 	
-	return distance;
+	return d;
 }