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     
00032     // start serial interrupt
00033     
00034     serial.attach(callback(this, &LIDAR::receive), RawSerial::RxIrq);
00035     
00036     // start the continuous operation of the LIDAR
00037     
00038     serial.putc(START_FLAG);
00039     serial.putc(SCAN);
00040 }
00041 
00042 /**
00043  * Stops the lidar and deletes this object.
00044  */
00045 LIDAR::~LIDAR() {
00046     
00047     // stop the LIDAR
00048     
00049     serial.putc(START_FLAG);
00050     serial.putc(STOP);
00051 }
00052 
00053 /**
00054  * Returns the distance measurement of the lidar at a given angle.
00055  * @param angle the angle, given in [deg] in the range 0..359.
00056  * @return the measured distance, given in [mm].
00057  */
00058 short LIDAR::getDistance(short angle) {
00059     
00060     while (angle < 0) angle += 360;
00061     while (angle >= 360) angle -= 360;
00062     
00063     return distances[angle];
00064 }
00065 
00066 /**
00067  * Returns the distance to a detected beacon.
00068  * @return the distance to the beacon, given in [mm], or zero, if no beacon was found.
00069  */
00070 short LIDAR::getDistanceOfBeacon() {
00071     
00072     return distanceOfBeacon;
00073 }
00074 
00075 /**
00076  * Returns the angle of a detected beacon.
00077  * @return the angle of the beacon, given in [deg] in the range 0..359.
00078  */
00079 short LIDAR::getAngleOfBeacon() {
00080     
00081     return angleOfBeacon;
00082 }
00083 
00084 /**
00085  * This method implements an algorithm that looks for the position of a beacon.
00086  * It should be called periodically by a low-priority background task.
00087  */
00088 void LIDAR::lookForBeacon() {
00089     
00090     distanceOfBeacon = 0;
00091     angleOfBeacon = 0;
00092     for (i=0;i<360;i++){
00093         dist_copy[i] = distances[i];          //local copy for the search of lighthouses
00094         }
00095     n=0;
00096     m=0;
00097     changed=false;
00098     for (i=0;i<10;i++){
00099         dist_diff_start[i]=0;          //
00100         dist_diff_stop[i]=0;
00101         }
00102     for (i=0;i<360;i++){
00103         
00104         if ((distances[i-1]-distances[i])>500 && distances[i]>500 && distances[i]<2000){
00105             /**dist_diff_start[n]=i;
00106             n++;
00107             
00108             printf("  start: %d \r\n",i);*/
00109             
00110             n=i;
00111             changed=true;
00112             }
00113         if ((distances[i+1]-distances[i])>500&&distances[i]>500&&distances[i]<2000){
00114             /**dist_diff_stop[m]=i;
00115             m++;
00116             printf("  stop: %d \r\n",i);*/
00117             m=i;
00118             changed=true;
00119             }
00120         if((m-n)<9&&(m-n)>2&& changed){
00121             range=m-n;
00122             max=0;
00123             min=2000;
00124             for(x=0;x<range;x++){
00125                if(min> distances[n+x]){
00126                    min=distances[n+x];
00127                    }
00128                 else if (max< distances[n+x]){
00129                     max=distances[n+x];
00130                     }
00131             }
00132             if((max-min)<75&&(max-min)>0){
00133                 distanceOfBeacon=(max+min)/2;
00134                 angleOfBeacon=(n+m)/2;
00135                 
00136            // printf(" Fenster gefunden!! Start:%d  Stop:%d max:%d min:%d  \r \n",n,m,max,min);
00137             changed=false;
00138                }
00139             }
00140         }
00141     
00142  /**   for(i=0;i<10;i++){
00143             printf(" %d: %d  ", i,dist_diff_start[i]);
00144             }
00145     printf("\n\r") ;
00146     for(i=0;i<10;i++){
00147             printf(" %d: %d  ", i,dist_diff_stop[i]);
00148             }*/
00149     printf("\n\r") ;
00150     printf("\n\r") ;
00151     printf("\n\r") ;
00152     
00153     
00154            
00155     
00156             
00157     
00158     // bitte implementieren!
00159 }
00160 
00161 /**
00162  * This method is called by the serial interrupt service routine.
00163  * It handles the reception of measurements from the LIDAR.
00164  */
00165 void LIDAR::receive() {
00166     
00167     // read received characters while input buffer is full
00168     
00169     if (serial.readable()) {
00170         
00171         // read single character from serial interface
00172         
00173         char c = serial.getc();
00174         
00175         // add this character to the header or to the data buffer
00176         
00177         if (headerCounter < HEADER_SIZE) {
00178             headerCounter++;
00179         } else {
00180             if (dataCounter < DATA_SIZE) {
00181                 data[dataCounter] = c;
00182                 dataCounter++;
00183             }
00184             if (dataCounter >= DATA_SIZE) {
00185                 
00186                 // data buffer is full, process measurement
00187                 
00188                 char quality = data[0] >> 2;
00189                 short angle = 360-(((unsigned short)data[1] | ((unsigned short)data[2] << 8)) >> 1)/64;
00190                 int16_t distance = ((unsigned short)data[3] | ((unsigned short)data[4] << 8))/4;
00191                 
00192                 if ((quality < QUALITY_THRESHOLD) || (distance < DISTANCE_THRESHOLD)) distance = DEFAULT_DISTANCE;
00193                 
00194                 // store distance in [mm] into array of full scan
00195                 
00196                 while (angle < 0) angle += 360;
00197                 while (angle >= 360) angle -= 360;
00198                 distances[angle] = distance;
00199                 
00200                 // reset data counter
00201                 
00202                 dataCounter = 0;
00203             }
00204         }
00205     }
00206 }
00207