From e7eeeb0d4e825a175c67cd1c0256c91362517ebb Mon Sep 17 00:00:00 2001
From: neoraider <devnull@localhost>
Date: Wed, 18 Apr 2007 23:00:05 +0000
Subject: Entfernungssensoren in Ordnung gebracht, Hindernisumfahrung
 implementiert.

---
 Srf10.cpp | 20 ++++++++++++++++----
 1 file changed, 16 insertions(+), 4 deletions(-)

(limited to 'Srf10.cpp')

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;
 }
-- 
cgit v1.2.3