Allan Brignoli
gugus
Diff: LIDAR.cpp
- Revision:
- 0:1a0321f1ffbc
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/LIDAR.cpp Fri May 18 12:18:21 2018 +0000
@@ -0,0 +1,237 @@
+/*
+ * 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;
+ }
+ }
+ }
+}
+