LIDAR.cpp

00001 /*
00002  * LIDAR.cpp
00003  * Copyright (c) 2018, ZHAW
00004  * All rights reserved.
00005  */
00006 
00007 #include <cmath>
00008 #include "LIDAR.h"
00009 
00010 using namespace std;
00011 
00012 /**
00013  * Creates a LIDAR object.
00014  * @param serial a reference to a serial interface to communicate with the laser scanner.
00015  */
00016 LIDAR::LIDAR(RawSerial& serial) : serial(serial) {
00017     
00018     // initialize serial interface
00019     
00020     serial.baud(115200);
00021     serial.format(8, SerialBase::None, 1);
00022     
00023     // initialize local values
00024     
00025     headerCounter = 0;
00026     dataCounter = 0;
00027     
00028     for (unsigned short i = 0; i < 360; i++) distances[i] = DEFAULT_DISTANCE;
00029     distanceOfBeacon = 0;
00030     angleOfBeacon = 0;
00031     lookClockwise = false;
00032     
00033     // start serial interrupt
00034     
00035     serial.attach(callback(this, &LIDAR::receive), RawSerial::RxIrq);
00036     
00037     // start the continuous operation of the LIDAR
00038     
00039     serial.putc(START_FLAG);
00040     serial.putc(SCAN);
00041 }
00042 
00043 /**
00044  * Stops the lidar and deletes this object.
00045  */
00046 LIDAR::~LIDAR() {
00047     
00048     // stop the LIDAR
00049     
00050     serial.putc(START_FLAG);
00051     serial.putc(STOP);
00052 }
00053 
00054 /**
00055  * Returns the distance measurement of the lidar at a given angle.
00056  * @param angle the angle, given in [deg] in the range 0..359.
00057  * @return the measured distance, given in [mm].
00058  */
00059 short LIDAR::getDistance(short angle) {
00060     
00061     while (angle < 0) angle += 360;
00062     while (angle >= 360) angle -= 360;
00063     
00064     return distances[angle];
00065 }
00066 
00067 /**
00068  * Returns the distance to a detected beacon.
00069  * @return the distance to the beacon, given in [mm], or zero, if no beacon was found.
00070  */
00071 short LIDAR::getDistanceOfBeacon() {
00072     
00073     return distanceOfBeacon;
00074 }
00075 
00076 /**
00077  * Returns the angle of a detected beacon.
00078  * @return the angle of the beacon, given in [deg] in the range 0..359.
00079  */
00080 short LIDAR::getAngleOfBeacon() {
00081     
00082     return angleOfBeacon;
00083 }
00084 
00085 /**
00086  * This method implements an algorithm that looks for the position of a beacon.
00087  * It should be called periodically by a low-priority background task.
00088  */
00089 void LIDAR::lookForBeacon() {
00090     
00091     lookClockwise = !lookClockwise;
00092     
00093     // make a local copy of scanner data
00094     
00095     int16_t distances[360];
00096     for (uint16_t i = 0; i < 360; i++) distances[i] = (lookClockwise) ? this->distances[360-i-1] : this->distances[i];
00097     
00098     // look for beacon
00099     
00100     bool foundBeacon = false;
00101     
00102     // look for falling edge angle
00103     
00104     int16_t fallingEdgeAngle = 1;
00105     
00106     while (!foundBeacon && (fallingEdgeAngle < 360)) {
00107         
00108         // look for falling edge angle
00109         
00110         if ((distances[fallingEdgeAngle] > MIN_DISTANCE) && (distances[fallingEdgeAngle] < MAX_DISTANCE) && (distances[fallingEdgeAngle]+THRESHOLD < distances[fallingEdgeAngle-1])) {
00111             
00112             // found falling edge, look for rising edge angle
00113             
00114             bool foundRisingEdge = false;
00115             int16_t risingEdgeAngle = fallingEdgeAngle;
00116             
00117             while (!foundRisingEdge && (risingEdgeAngle < 360)) {
00118                 
00119                 if ((distances[risingEdgeAngle-1] > MIN_DISTANCE) && (distances[risingEdgeAngle-1] < MAX_DISTANCE) && (distances[risingEdgeAngle-1]+THRESHOLD < distances[risingEdgeAngle])) {
00120                     
00121                     // found rising edge
00122                     
00123                     foundRisingEdge = true;
00124                     
00125                 } else {
00126                     
00127                     // didn't find rising edge, continue looking
00128                     
00129                     risingEdgeAngle++;
00130                 }
00131             }
00132             
00133             if (foundRisingEdge) {
00134                 
00135                 // rising edge found, now check beacon size and window
00136                 
00137                 int16_t n = risingEdgeAngle-fallingEdgeAngle;
00138                 if ((n >= MIN_SIZE) && (n <= MAX_SIZE)) {
00139                     
00140                     int32_t sumOfDistances = 0;
00141                     for (int16_t angle = fallingEdgeAngle; angle < risingEdgeAngle; angle++) sumOfDistances += distances[angle];
00142                     int16_t averageDistance = static_cast<int16_t>(sumOfDistances/n);
00143                     
00144                     bool distancesInWindow = true;
00145                     for (int16_t angle = fallingEdgeAngle; angle < risingEdgeAngle; angle++) if (abs(averageDistance-distances[angle]) > WINDOW/2) distancesInWindow = false;
00146                     
00147                     if (distancesInWindow) {
00148                         
00149                         // calculate distance and angle of beacon
00150                         
00151                         distanceOfBeacon = averageDistance+20;
00152                         angleOfBeacon = (lookClockwise) ? 360-(fallingEdgeAngle+risingEdgeAngle-1)/2 : (fallingEdgeAngle+risingEdgeAngle-1)/2;
00153                         foundBeacon = true;
00154                         
00155                     } else {
00156                         
00157                         // distances of beacon are not in window, look for next falling edge
00158                         
00159                         fallingEdgeAngle++;
00160                     }
00161                     
00162                 } else {
00163                     
00164                     // size of beacon is not correct, look for next falling edge
00165                     
00166                     fallingEdgeAngle++;
00167                 }
00168                 
00169             } else {
00170                 
00171                 // didn't find rising edge, stop looking
00172                 
00173                 fallingEdgeAngle = 360;
00174             }
00175             
00176         } else {
00177             
00178             // didn't find falling edge, continue looking
00179             
00180             fallingEdgeAngle++;
00181         }
00182     }
00183     
00184     if (!foundBeacon) {
00185         
00186         distanceOfBeacon = 0;
00187         angleOfBeacon = 0;
00188     }
00189 }
00190 
00191 /**
00192  * This method is called by the serial interrupt service routine.
00193  * It handles the reception of measurements from the LIDAR.
00194  */
00195 void LIDAR::receive() {
00196     
00197     // read received characters while input buffer is full
00198     
00199     if (serial.readable()) {
00200         
00201         // read single character from serial interface
00202         
00203         char c = serial.getc();
00204         
00205         // add this character to the header or to the data buffer
00206         
00207         if (headerCounter < HEADER_SIZE) {
00208             headerCounter++;
00209         } else {
00210             if (dataCounter < DATA_SIZE) {
00211                 data[dataCounter] = c;
00212                 dataCounter++;
00213             }
00214             if (dataCounter >= DATA_SIZE) {
00215                 
00216                 // data buffer is full, process measurement
00217                 
00218                 char quality = data[0] >> 2;
00219                 short angle = 360-(((unsigned short)data[1] | ((unsigned short)data[2] << 8)) >> 1)/64;
00220                 int16_t distance = ((unsigned short)data[3] | ((unsigned short)data[4] << 8))/4;
00221                 
00222                 if ((quality < QUALITY_THRESHOLD) || (distance < DISTANCE_THRESHOLD)) distance = DEFAULT_DISTANCE;
00223                 
00224                 // store distance in [mm] into array of full scan
00225                 
00226                 while (angle < 0) angle += 360;
00227                 while (angle >= 360) angle -= 360;
00228                 distances[angle] = distance;
00229                 
00230                 // reset data counter
00231                 
00232                 dataCounter = 0;
00233             }
00234         }
00235     }
00236 }