LRF: Added timeout

refs 7956

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

 
 #include "inc/LaserRangeFinder.h"
 
+int timeout;
+
 /** Initialize the laser range finder. */
 void lrf_init() {
+    timeout = 0;
+    
+    /*
     char port[] = "/dev/????????";
-
     FILE* fp = popen("dmesg | grep -A 3 \"Arduino Uno\"  | grep -o \"ttyACM.\" | tail -n 1","r");
     fgets(port+5, sizeof(port)-6, fp);
-    printf("Port: %s\n", port);
+    */
+
+    char port[100];
+    FILE* fp = popen("ls -d /dev/serial/by-id/* | grep \"Arduino\" | tr -d \"\n\"", "r");
+    fgets(port, sizeof(port), fp);
+
+    printf("lrf port=%s\n", port);
     pclose(fp);
 
     uart_setup(port);
@@ -26,22 +36,27 @@ GEN_TYPE_boolean lrf_is_noval() {
 /** Get one distance measurement from the laser range finder. */
 GEN_TYPE_int lrf_read() {
 
-    if (uart_filestream == -1)
+    if(timeout>100)
+        lrf_init();
+
+    if (uart_filestream == -1) {
+        timeout++;
         return -1;
+    }
 
     //empty buffer
     uart_flush();
 
     //initialize arrays
     int i;
-    char bytes[6];
-    for (i = 0; i < 6; i++) {
+    char bytes[10];
+    for (i = 0; i < sizeof(bytes) / sizeof(bytes[0]); i++) {
         bytes[i] = 0;
     }
     int n_read = 0;
 
-    char value[4];
-    for (i = 0; i < 4; i++) {
+    char value[10];
+    for (i = 0; i < sizeof(value) / sizeof(value[0]); i++) {
         value[i] = 0;
     }
 
@@ -50,16 +65,20 @@ GEN_TYPE_int lrf_read() {
 
     //receive values
     while (1) {
-        n_read = uart_receive(bytes, 6, 0);
+        n_read = uart_receive(bytes, sizeof(bytes) / sizeof(bytes[0]), 0);
 
-        if(n_read <= 0)
+        if(n_read < 0) {
+            timeout++;
             return -1;
+        }
 
         for (i = 0; i < n_read; i++) {
             if (bytes[i] != 0xd && bytes[i] != 0xa) {
                 value[n++] = bytes[i];
             } else if (atoi(value) != 0) {
                 d = atoi(value);
+                printf("%d\n",d);
+                timeout = 0;
                 return d;
             }
         }