Diff: LIDAR.cpp

Revision:

0:da791f233257

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/LIDAR.cpp	Fri May 11 12:21:19 2018 +0000
@@ -0,0 +1,146 @@
+/*
+ * 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() {
+    // copy distances to internal array
+    short copy_distances[360];
+    for( uint8_t i = 0; i < 360; i++){
+            copy_distances[i] = this->distances[i];
+    }
+    
+    distanceOfBeacon = 0;
+    angleOfBeacon = 0;
+    
+    // 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;
+            }
+        }
+    }
+}