File content as of revision 0:da791f233257:

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