File content as of revision 0:29be10cb0afc:

/*
 * 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;
            }
        }
    }
}