From 5105407b223a3905c148da052bb3e42bd66b3acd Mon Sep 17 00:00:00 2001
From: Bianka Forkel <noreply@fortiss.org>
Date: Tue, 18 Jul 2017 19:35:05 +0000
Subject: [PATCH] LRF: Added timeout refs 7956
---
.../trunk/lib/src/LaserRangeFinder.c | 37 ++++++++++++++-----
1 file changed, 28 insertions(+), 9 deletions(-)
diff --git a/org.fortiss.af3.platform.raspberry/trunk/lib/src/LaserRangeFinder.c b/org.fortiss.af3.platform.raspberry/trunk/lib/src/LaserRangeFinder.c
index f8db0861..a677ad7e 100644
--- a/org.fortiss.af3.platform.raspberry/trunk/lib/src/LaserRangeFinder.c
+++ b/org.fortiss.af3.platform.raspberry/trunk/lib/src/LaserRangeFinder.c
@@ -1,13 +1,23 @@
#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;
}
}
--
GitLab