Diff: LIDAR.cpp

Revision:

1:a1a7c556e96a

Parent:

0:497d7f448d3e

diff -r 497d7f448d3e -r a1a7c556e96a LIDAR.cpp
--- a/LIDAR.cpp	Fri May 11 12:38:21 2018 +0000
+++ b/LIDAR.cpp	Fri May 11 13:35:55 2018 +0000
@@ -86,10 +86,37 @@
  * It should be called periodically by a low-priority background task.
  */
 void LIDAR::lookForBeacon() {
-    
-    distanceOfBeacon = 0;
-    angleOfBeacon = 0;
     
+    distanceOfBeacon = 0;
+    angleOfBeacon = 0;
+    short beacon_counter;
+    //copy measurement data
+    for (unsigned short i = 0; i < 360; i++)
+    {
+        distances_copy[i] = distances[i];
+    }
+    // search for gap more than 500mm
+    for(int i=1;i<360;i++)
+    {
+        // look for gap of 500mm and distance between 500-2000mm
+        if (abs(distances[i]-distances[i-1])>=500 && distances[i]>=500 && distances[i]<=2000)
+        {
+            printf("Debug: Found Gap at %d \n \r",i);
+            
+            // found edge look now for beacon
+            for(short u=1;u<9;u++) {
+                if(abs(distances[i+u]-distances[i+u-1])<=80 && abs(distances[i+u]-distances[i+u-1])>=60) {
+                    beacon_counter++;
+                    printf("Debug: Possible beacon atp at %d \n \r",i+u);
+                }
+                if(beacon_counter>=2) {
+                    printf("Debug: Found Beagon at %d and Distance %d \n \r",i+u,distances[i]); 
+                    distanceOfBeacon= distances[i];
+                    angleOfBeacon= i;
+                }
+            }
+        }
+    }
     // bitte implementieren!
 }