Nim leo niiiim
Diff: LIDAR.cpp
- Revision:
- 0:da791f233257
diff -r 000000000000 -r da791f233257 LIDAR.cpp --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/LIDAR.cpp Fri May 11 12:21:19 2018 +0000 @@ -0,0 +1,146 @@ +/* + * 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; + } + } + } +}