File content as of revision 5:957580f33e52:

/*
 * 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;
    lookClockwise = false;
    
    // 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() {
    
    lookClockwise = !lookClockwise;
    
    // make a local copy of scanner data
    
    int16_t distances[360];
    for (uint16_t i = 0; i < 360; i++) distances[i] = (lookClockwise) ? this->distances[360-i-1] : this->distances[i];
    
    // look for beacon
    
    bool foundBeacon = false;
    
    // look for falling edge angle
    
    int16_t fallingEdgeAngle = 1;
    
    while (!foundBeacon && (fallingEdgeAngle < 360)) {
        
        // look for falling edge angle
        
        if ((distances[fallingEdgeAngle] > MIN_DISTANCE) && (distances[fallingEdgeAngle] < MAX_DISTANCE) && (distances[fallingEdgeAngle]+THRESHOLD < distances[fallingEdgeAngle-1])) {
            
            // found falling edge, look for rising edge angle
            
            bool foundRisingEdge = false;
            int16_t risingEdgeAngle = fallingEdgeAngle;
            
            while (!foundRisingEdge && (risingEdgeAngle < 360)) {
                
                if ((distances[risingEdgeAngle-1] > MIN_DISTANCE) && (distances[risingEdgeAngle-1] < MAX_DISTANCE) && (distances[risingEdgeAngle-1]+THRESHOLD < distances[risingEdgeAngle])) {
                    
                    // found rising edge
                    
                    foundRisingEdge = true;
                    
                } else {
                    
                    // didn't find rising edge, continue looking
                    
                    risingEdgeAngle++;
                }
            }
            
            if (foundRisingEdge) {
                
                // rising edge found, now check beacon size and window
                
                int16_t n = risingEdgeAngle-fallingEdgeAngle;
                if ((n >= MIN_SIZE) && (n <= MAX_SIZE)) {
                    
                    int32_t sumOfDistances = 0;
                    for (int16_t angle = fallingEdgeAngle; angle < risingEdgeAngle; angle++) sumOfDistances += distances[angle];
                    int16_t averageDistance = static_cast<int16_t>(sumOfDistances/n);
                    
                    bool distancesInWindow = true;
                    for (int16_t angle = fallingEdgeAngle; angle < risingEdgeAngle; angle++) if (abs(averageDistance-distances[angle]) > WINDOW/2) distancesInWindow = false;
                    
                    if (distancesInWindow) {
                        
                        // calculate distance and angle of beacon
                        
                        distanceOfBeacon = averageDistance+20;
                        angleOfBeacon = (lookClockwise) ? 360-(fallingEdgeAngle+risingEdgeAngle-1)/2 : (fallingEdgeAngle+risingEdgeAngle-1)/2;
                        foundBeacon = true;
                        
                    } else {
                        
                        // distances of beacon are not in window, look for next falling edge
                        
                        fallingEdgeAngle++;
                    }
                    
                } else {
                    
                    // size of beacon is not correct, look for next falling edge
                    
                    fallingEdgeAngle++;
                }
                
            } else {
                
                // didn't find rising edge, stop looking
                
                fallingEdgeAngle = 360;
            }
            
        } else {
            
            // didn't find falling edge, continue looking
            
            fallingEdgeAngle++;
        }
    }
    
    if (!foundBeacon) {
        
        distanceOfBeacon = 0;
        angleOfBeacon = 0;
    }
}

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