Diff: LIDAR.cpp

Revision:

0:29be10cb0afc

diff -r 000000000000 -r 29be10cb0afc LIDAR.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/LIDAR.cpp	Fri Apr 27 08:47:34 2018 +0000
@@ -0,0 +1,207 @@
+/*
+ * LIDAR.cpp
+ * Copyright (c) 2018, ZHAW
+ * All rights reserved.
+ */
+
+#include <cmath>
+#include "LIDAR.h"
+
+using namespace std;
+
+/**
+ * Creates a LIDAR object.
+ * @param serial a reference to a serial interface to communicate with the laser scanner.
+ */
+LIDAR::LIDAR(RawSerial& serial) : serial(serial) {
+    
+    // initialize serial interface
+    
+    serial.baud(115200);
+    serial.format(8, SerialBase::None, 1);
+    
+    // initialize local values
+    
+    headerCounter = 0;
+    dataCounter = 0;
+    
+    for (unsigned short i = 0; i < 360; i++) distances[i] = DEFAULT_DISTANCE;
+    distanceOfBeacon = 0;
+    angleOfBeacon = 0;
+    
+    // start serial interrupt
+    
+    serial.attach(callback(this, &LIDAR::receive), RawSerial::RxIrq);
+    
+    // start the continuous operation of the LIDAR
+    
+    serial.putc(START_FLAG);
+    serial.putc(SCAN);
+}
+
+/**
+ * Stops the lidar and deletes this object.
+ */
+LIDAR::~LIDAR() {
+    
+    // stop the LIDAR
+    
+    serial.putc(START_FLAG);
+    serial.putc(STOP);
+}
+
+/**
+ * Returns the distance measurement of the lidar at a given angle.
+ * @param angle the angle, given in [deg] in the range 0..359.
+ * @return the measured distance, given in [mm].
+ */
+short LIDAR::getDistance(short angle) {
+    
+    while (angle < 0) angle += 360;
+    while (angle >= 360) angle -= 360;
+    
+    return distances[angle];
+}
+
+/**
+ * Returns the distance to a detected beacon.
+ * @return the distance to the beacon, given in [mm], or zero, if no beacon was found.
+ */
+short LIDAR::getDistanceOfBeacon() {
+    
+    return distanceOfBeacon;
+}
+
+/**
+ * Returns the angle of a detected beacon.
+ * @return the angle of the beacon, given in [deg] in the range 0..359.
+ */
+short LIDAR::getAngleOfBeacon() {
+    
+    return angleOfBeacon;
+}
+
+/**
+ * This method implements an algorithm that looks for the position of a beacon.
+ * It should be called periodically by a low-priority background task.
+ */
+void LIDAR::lookForBeacon() {
+    
+    distanceOfBeacon = 0;
+    angleOfBeacon = 0;
+    for (i=0;i<360;i++){
+        dist_copy[i] = distances[i];          //local copy for the search of lighthouses
+        }
+    n=0;
+    m=0;
+    changed=false;
+    for (i=0;i<10;i++){
+        dist_diff_start[i]=0;          //
+        dist_diff_stop[i]=0;
+        }
+    for (i=0;i<360;i++){
+        
+        if ((distances[i-1]-distances[i])>500 && distances[i]>500 && distances[i]<2000){
+            /**dist_diff_start[n]=i;
+            n++;
+            
+            printf("  start: %d \r\n",i);*/
+            
+            n=i;
+            changed=true;
+            }
+        if ((distances[i+1]-distances[i])>500&&distances[i]>500&&distances[i]<2000){
+            /**dist_diff_stop[m]=i;
+            m++;
+            printf("  stop: %d \r\n",i);*/
+            m=i;
+            changed=true;
+            }
+        if((m-n)<9&&(m-n)>2&& changed){
+            range=m-n;
+            max=0;
+            min=2000;
+            for(x=0;x<range;x++){
+               if(min> distances[n+x]){
+                   min=distances[n+x];
+                   }
+                else if (max< distances[n+x]){
+                    max=distances[n+x];
+                    }
+            }
+            if((max-min)<75&&(max-min)>0){
+                distanceOfBeacon=(max+min)/2;
+                angleOfBeacon=(n+m)/2;
+                
+           // printf(" Fenster gefunden!! Start:%d  Stop:%d max:%d min:%d  \r \n",n,m,max,min);
+            changed=false;
+               }
+            }
+        }
+    
+ /**   for(i=0;i<10;i++){
+            printf(" %d: %d  ", i,dist_diff_start[i]);
+            }
+    printf("\n\r") ;
+    for(i=0;i<10;i++){
+            printf(" %d: %d  ", i,dist_diff_stop[i]);
+            }*/
+    printf("\n\r") ;
+    printf("\n\r") ;
+    printf("\n\r") ;
+    
+    
+           
+    
+            
+    
+    // bitte implementieren!
+}
+
+/**
+ * This method is called by the serial interrupt service routine.
+ * It handles the reception of measurements from the LIDAR.
+ */
+void LIDAR::receive() {
+    
+    // read received characters while input buffer is full
+    
+    if (serial.readable()) {
+        
+        // read single character from serial interface
+        
+        char c = serial.getc();
+        
+        // add this character to the header or to the data buffer
+        
+        if (headerCounter < HEADER_SIZE) {
+            headerCounter++;
+        } else {
+            if (dataCounter < DATA_SIZE) {
+                data[dataCounter] = c;
+                dataCounter++;
+            }
+            if (dataCounter >= DATA_SIZE) {
+                
+                // data buffer is full, process measurement
+                
+                char quality = data[0] >> 2;
+                short angle = 360-(((unsigned short)data[1] | ((unsigned short)data[2] << 8)) >> 1)/64;
+                int16_t distance = ((unsigned short)data[3] | ((unsigned short)data[4] << 8))/4;
+                
+                if ((quality < QUALITY_THRESHOLD) || (distance < DISTANCE_THRESHOLD)) distance = DEFAULT_DISTANCE;
+                
+                // store distance in [mm] into array of full scan
+                
+                while (angle < 0) angle += 360;
+                while (angle >= 360) angle -= 360;
+                distances[angle] = distance;
+                
+                // reset data counter
+                
+                dataCounter = 0;
+            }
+        }
+    }
+}
+