LRF: additional range checks

refs 7956

org.fortiss.af3.platform.raspberry/trunk/lib/src/LaserRangeFinder.c

@@ -36,7 +36,7 @@ GEN_TYPE_boolean lrf_is_noval() {
 /** Get one distance measurement from the laser range finder. */
 GEN_TYPE_int lrf_read() {
 
-    if(timeout>100)
+    if(timeout>1000)
         lrf_init();
 
     if (uart_filestream == -1) {
@@ -49,15 +49,18 @@ GEN_TYPE_int lrf_read() {
     uart_flush();
 
     //initialize arrays
+    int sizeB = 10;
+    int sizeV = 4;
+
     int i;
-    char bytes[10];
-    for (i = 0; i < sizeof(bytes) / sizeof(bytes[0]); i++) {
+    char bytes[sizeB];
+    for (i = 0; i < sizeB; i++) {
         bytes[i] = 0;
     }
     int n_read = 0;
 
-    char value[10];
-    for (i = 0; i < sizeof(value) / sizeof(value[0]); i++) {
+    char value[sizeV];
+    for (i = 0; i < sizeV; i++) {
         value[i] = 0;
     }
 
@@ -66,7 +69,7 @@ GEN_TYPE_int lrf_read() {
 
     //receive values
     while (1) {
-        n_read = uart_receive(bytes, sizeof(bytes) / sizeof(bytes[0]), 0);
+        n_read = uart_receive(bytes, sizeB, 0);
 
         if(n_read <= 0) {
             timeout++;
@@ -76,8 +79,14 @@ GEN_TYPE_int lrf_read() {
 
         for (i = 0; i < n_read; i++) {
             if (bytes[i] != 0xd && bytes[i] != 0xa) {
-                value[n++] = bytes[i];
-            } else if (atoi(value) != 0) {
+                if(n < sizeV)
+                    value[n++] = bytes[i];
+                else {
+                    timeout++;
+                    printf("-1: too many bytes\n");
+                    return -1;
+                }
+            } else {
                 d = atoi(value);
                 printf("%d\n",d);
                 timeout = 0;