Zürcher Eliteeinheit / Mbed 2 deprecated ROME2_P6

rome2_p6 imported

Dependencies:   mbed

LIDAR.cpp@5:957580f33e52, 2018-05-18 (annotated)

Committer:
Appalco
Date:
Fri May 18 13:54:25 2018 +0000
Revision:
5:957580f33e52
Parent:
0:351a2fb21235 
fixed tolerance and wayponts

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Appalco 0:351a2fb21235 1 /*
Appalco 0:351a2fb21235 2 * LIDAR.cpp
Appalco 0:351a2fb21235 3 * Copyright (c) 2018, ZHAW
Appalco 0:351a2fb21235 4 * All rights reserved.
Appalco 0:351a2fb21235 5 */
Appalco 0:351a2fb21235 6
Appalco 0:351a2fb21235 7 #include <cmath>
Appalco 0:351a2fb21235 8 #include "LIDAR.h"
Appalco 0:351a2fb21235 9
Appalco 0:351a2fb21235 10 using namespace std;
Appalco 0:351a2fb21235 11
Appalco 0:351a2fb21235 12 /**
Appalco 0:351a2fb21235 13 * Creates a LIDAR object.
Appalco 0:351a2fb21235 14 * @param serial a reference to a serial interface to communicate with the laser scanner.
Appalco 0:351a2fb21235 15 */
Appalco 0:351a2fb21235 16 LIDAR::LIDAR(RawSerial& serial) : serial(serial) {
Appalco 0:351a2fb21235 17
Appalco 0:351a2fb21235 18 // initialize serial interface
Appalco 0:351a2fb21235 19
Appalco 0:351a2fb21235 20 serial.baud(115200);
Appalco 0:351a2fb21235 21 serial.format(8, SerialBase::None, 1);
Appalco 0:351a2fb21235 22
Appalco 0:351a2fb21235 23 // initialize local values
Appalco 0:351a2fb21235 24
Appalco 0:351a2fb21235 25 headerCounter = 0;
Appalco 0:351a2fb21235 26 dataCounter = 0;
Appalco 0:351a2fb21235 27
Appalco 0:351a2fb21235 28 for (unsigned short i = 0; i < 360; i++) distances[i] = DEFAULT_DISTANCE;
Appalco 0:351a2fb21235 29 distanceOfBeacon = 0;
Appalco 0:351a2fb21235 30 angleOfBeacon = 0;
Appalco 0:351a2fb21235 31 lookClockwise = false;
Appalco 0:351a2fb21235 32
Appalco 0:351a2fb21235 33 // start serial interrupt
Appalco 0:351a2fb21235 34
Appalco 0:351a2fb21235 35 serial.attach(callback(this, &LIDAR::receive), RawSerial::RxIrq);
Appalco 0:351a2fb21235 36
Appalco 0:351a2fb21235 37 // start the continuous operation of the LIDAR
Appalco 0:351a2fb21235 38
Appalco 0:351a2fb21235 39 serial.putc(START_FLAG);
Appalco 0:351a2fb21235 40 serial.putc(SCAN);
Appalco 0:351a2fb21235 41 }
Appalco 0:351a2fb21235 42
Appalco 0:351a2fb21235 43 /**
Appalco 0:351a2fb21235 44 * Stops the lidar and deletes this object.
Appalco 0:351a2fb21235 45 */
Appalco 0:351a2fb21235 46 LIDAR::~LIDAR() {
Appalco 0:351a2fb21235 47
Appalco 0:351a2fb21235 48 // stop the LIDAR
Appalco 0:351a2fb21235 49
Appalco 0:351a2fb21235 50 serial.putc(START_FLAG);
Appalco 0:351a2fb21235 51 serial.putc(STOP);
Appalco 0:351a2fb21235 52 }
Appalco 0:351a2fb21235 53
Appalco 0:351a2fb21235 54 /**
Appalco 0:351a2fb21235 55 * Returns the distance measurement of the lidar at a given angle.
Appalco 0:351a2fb21235 56 * @param angle the angle, given in [deg] in the range 0..359.
Appalco 0:351a2fb21235 57 * @return the measured distance, given in [mm].
Appalco 0:351a2fb21235 58 */
Appalco 0:351a2fb21235 59 short LIDAR::getDistance(short angle) {
Appalco 0:351a2fb21235 60
Appalco 0:351a2fb21235 61 while (angle < 0) angle += 360;
Appalco 0:351a2fb21235 62 while (angle >= 360) angle -= 360;
Appalco 0:351a2fb21235 63
Appalco 0:351a2fb21235 64 return distances[angle];
Appalco 0:351a2fb21235 65 }
Appalco 0:351a2fb21235 66
Appalco 0:351a2fb21235 67 /**
Appalco 0:351a2fb21235 68 * Returns the distance to a detected beacon.
Appalco 0:351a2fb21235 69 * @return the distance to the beacon, given in [mm], or zero, if no beacon was found.
Appalco 0:351a2fb21235 70 */
Appalco 0:351a2fb21235 71 short LIDAR::getDistanceOfBeacon() {
Appalco 0:351a2fb21235 72
Appalco 0:351a2fb21235 73 return distanceOfBeacon;
Appalco 0:351a2fb21235 74 }
Appalco 0:351a2fb21235 75
Appalco 0:351a2fb21235 76 /**
Appalco 0:351a2fb21235 77 * Returns the angle of a detected beacon.
Appalco 0:351a2fb21235 78 * @return the angle of the beacon, given in [deg] in the range 0..359.
Appalco 0:351a2fb21235 79 */
Appalco 0:351a2fb21235 80 short LIDAR::getAngleOfBeacon() {
Appalco 0:351a2fb21235 81
Appalco 0:351a2fb21235 82 return angleOfBeacon;
Appalco 0:351a2fb21235 83 }
Appalco 0:351a2fb21235 84
Appalco 0:351a2fb21235 85 /**
Appalco 0:351a2fb21235 86 * This method implements an algorithm that looks for the position of a beacon.
Appalco 0:351a2fb21235 87 * It should be called periodically by a low-priority background task.
Appalco 0:351a2fb21235 88 */
Appalco 0:351a2fb21235 89 void LIDAR::lookForBeacon() {
Appalco 0:351a2fb21235 90
Appalco 0:351a2fb21235 91 lookClockwise = !lookClockwise;
Appalco 0:351a2fb21235 92
Appalco 0:351a2fb21235 93 // make a local copy of scanner data
Appalco 0:351a2fb21235 94
Appalco 0:351a2fb21235 95 int16_t distances[360];
Appalco 0:351a2fb21235 96 for (uint16_t i = 0; i < 360; i++) distances[i] = (lookClockwise) ? this->distances[360-i-1] : this->distances[i];
Appalco 0:351a2fb21235 97
Appalco 0:351a2fb21235 98 // look for beacon
Appalco 0:351a2fb21235 99
Appalco 0:351a2fb21235 100 bool foundBeacon = false;
Appalco 0:351a2fb21235 101
Appalco 0:351a2fb21235 102 // look for falling edge angle
Appalco 0:351a2fb21235 103
Appalco 0:351a2fb21235 104 int16_t fallingEdgeAngle = 1;
Appalco 0:351a2fb21235 105
Appalco 0:351a2fb21235 106 while (!foundBeacon && (fallingEdgeAngle < 360)) {
Appalco 0:351a2fb21235 107
Appalco 0:351a2fb21235 108 // look for falling edge angle
Appalco 0:351a2fb21235 109
Appalco 0:351a2fb21235 110 if ((distances[fallingEdgeAngle] > MIN_DISTANCE) && (distances[fallingEdgeAngle] < MAX_DISTANCE) && (distances[fallingEdgeAngle]+THRESHOLD < distances[fallingEdgeAngle-1])) {
Appalco 0:351a2fb21235 111
Appalco 0:351a2fb21235 112 // found falling edge, look for rising edge angle
Appalco 0:351a2fb21235 113
Appalco 0:351a2fb21235 114 bool foundRisingEdge = false;
Appalco 0:351a2fb21235 115 int16_t risingEdgeAngle = fallingEdgeAngle;
Appalco 0:351a2fb21235 116
Appalco 0:351a2fb21235 117 while (!foundRisingEdge && (risingEdgeAngle < 360)) {
Appalco 0:351a2fb21235 118
Appalco 0:351a2fb21235 119 if ((distances[risingEdgeAngle-1] > MIN_DISTANCE) && (distances[risingEdgeAngle-1] < MAX_DISTANCE) && (distances[risingEdgeAngle-1]+THRESHOLD < distances[risingEdgeAngle])) {
Appalco 0:351a2fb21235 120
Appalco 0:351a2fb21235 121 // found rising edge
Appalco 0:351a2fb21235 122
Appalco 0:351a2fb21235 123 foundRisingEdge = true;
Appalco 0:351a2fb21235 124
Appalco 0:351a2fb21235 125 } else {
Appalco 0:351a2fb21235 126
Appalco 0:351a2fb21235 127 // didn't find rising edge, continue looking
Appalco 0:351a2fb21235 128
Appalco 0:351a2fb21235 129 risingEdgeAngle++;
Appalco 0:351a2fb21235 130 }
Appalco 0:351a2fb21235 131 }
Appalco 0:351a2fb21235 132
Appalco 0:351a2fb21235 133 if (foundRisingEdge) {
Appalco 0:351a2fb21235 134
Appalco 0:351a2fb21235 135 // rising edge found, now check beacon size and window
Appalco 0:351a2fb21235 136
Appalco 0:351a2fb21235 137 int16_t n = risingEdgeAngle-fallingEdgeAngle;
Appalco 0:351a2fb21235 138 if ((n >= MIN_SIZE) && (n <= MAX_SIZE)) {
Appalco 0:351a2fb21235 139
Appalco 0:351a2fb21235 140 int32_t sumOfDistances = 0;
Appalco 0:351a2fb21235 141 for (int16_t angle = fallingEdgeAngle; angle < risingEdgeAngle; angle++) sumOfDistances += distances[angle];
Appalco 0:351a2fb21235 142 int16_t averageDistance = static_cast<int16_t>(sumOfDistances/n);
Appalco 0:351a2fb21235 143
Appalco 0:351a2fb21235 144 bool distancesInWindow = true;
Appalco 0:351a2fb21235 145 for (int16_t angle = fallingEdgeAngle; angle < risingEdgeAngle; angle++) if (abs(averageDistance-distances[angle]) > WINDOW/2) distancesInWindow = false;
Appalco 0:351a2fb21235 146
Appalco 0:351a2fb21235 147 if (distancesInWindow) {
Appalco 0:351a2fb21235 148
Appalco 0:351a2fb21235 149 // calculate distance and angle of beacon
Appalco 0:351a2fb21235 150
Appalco 0:351a2fb21235 151 distanceOfBeacon = averageDistance+20;
Appalco 0:351a2fb21235 152 angleOfBeacon = (lookClockwise) ? 360-(fallingEdgeAngle+risingEdgeAngle-1)/2 : (fallingEdgeAngle+risingEdgeAngle-1)/2;
Appalco 0:351a2fb21235 153 foundBeacon = true;
Appalco 0:351a2fb21235 154
Appalco 0:351a2fb21235 155 } else {
Appalco 0:351a2fb21235 156
Appalco 0:351a2fb21235 157 // distances of beacon are not in window, look for next falling edge
Appalco 0:351a2fb21235 158
Appalco 0:351a2fb21235 159 fallingEdgeAngle++;
Appalco 0:351a2fb21235 160 }
Appalco 0:351a2fb21235 161
Appalco 0:351a2fb21235 162 } else {
Appalco 0:351a2fb21235 163
Appalco 0:351a2fb21235 164 // size of beacon is not correct, look for next falling edge
Appalco 0:351a2fb21235 165
Appalco 0:351a2fb21235 166 fallingEdgeAngle++;
Appalco 0:351a2fb21235 167 }
Appalco 0:351a2fb21235 168
Appalco 0:351a2fb21235 169 } else {
Appalco 0:351a2fb21235 170
Appalco 0:351a2fb21235 171 // didn't find rising edge, stop looking
Appalco 0:351a2fb21235 172
Appalco 0:351a2fb21235 173 fallingEdgeAngle = 360;
Appalco 0:351a2fb21235 174 }
Appalco 0:351a2fb21235 175
Appalco 0:351a2fb21235 176 } else {
Appalco 0:351a2fb21235 177
Appalco 0:351a2fb21235 178 // didn't find falling edge, continue looking
Appalco 0:351a2fb21235 179
Appalco 0:351a2fb21235 180 fallingEdgeAngle++;
Appalco 0:351a2fb21235 181 }
Appalco 0:351a2fb21235 182 }
Appalco 0:351a2fb21235 183
Appalco 0:351a2fb21235 184 if (!foundBeacon) {
Appalco 0:351a2fb21235 185
Appalco 0:351a2fb21235 186 distanceOfBeacon = 0;
Appalco 0:351a2fb21235 187 angleOfBeacon = 0;
Appalco 0:351a2fb21235 188 }
Appalco 0:351a2fb21235 189 }
Appalco 0:351a2fb21235 190
Appalco 0:351a2fb21235 191 /**
Appalco 0:351a2fb21235 192 * This method is called by the serial interrupt service routine.
Appalco 0:351a2fb21235 193 * It handles the reception of measurements from the LIDAR.
Appalco 0:351a2fb21235 194 */
Appalco 0:351a2fb21235 195 void LIDAR::receive() {
Appalco 0:351a2fb21235 196
Appalco 0:351a2fb21235 197 // read received characters while input buffer is full
Appalco 0:351a2fb21235 198
Appalco 0:351a2fb21235 199 if (serial.readable()) {
Appalco 0:351a2fb21235 200
Appalco 0:351a2fb21235 201 // read single character from serial interface
Appalco 0:351a2fb21235 202
Appalco 0:351a2fb21235 203 char c = serial.getc();
Appalco 0:351a2fb21235 204
Appalco 0:351a2fb21235 205 // add this character to the header or to the data buffer
Appalco 0:351a2fb21235 206
Appalco 0:351a2fb21235 207 if (headerCounter < HEADER_SIZE) {
Appalco 0:351a2fb21235 208 headerCounter++;
Appalco 0:351a2fb21235 209 } else {
Appalco 0:351a2fb21235 210 if (dataCounter < DATA_SIZE) {
Appalco 0:351a2fb21235 211 data[dataCounter] = c;
Appalco 0:351a2fb21235 212 dataCounter++;
Appalco 0:351a2fb21235 213 }
Appalco 0:351a2fb21235 214 if (dataCounter >= DATA_SIZE) {
Appalco 0:351a2fb21235 215
Appalco 0:351a2fb21235 216 // data buffer is full, process measurement
Appalco 0:351a2fb21235 217
Appalco 0:351a2fb21235 218 char quality = data[0] >> 2;
Appalco 0:351a2fb21235 219 short angle = 360-(((unsigned short)data[1] | ((unsigned short)data[2] << 8)) >> 1)/64;
Appalco 0:351a2fb21235 220 int16_t distance = ((unsigned short)data[3] | ((unsigned short)data[4] << 8))/4;
Appalco 0:351a2fb21235 221
Appalco 0:351a2fb21235 222 if ((quality < QUALITY_THRESHOLD) || (distance < DISTANCE_THRESHOLD)) distance = DEFAULT_DISTANCE;
Appalco 0:351a2fb21235 223
Appalco 0:351a2fb21235 224 // store distance in [mm] into array of full scan
Appalco 0:351a2fb21235 225
Appalco 0:351a2fb21235 226 while (angle < 0) angle += 360;
Appalco 0:351a2fb21235 227 while (angle >= 360) angle -= 360;
Appalco 0:351a2fb21235 228 distances[angle] = distance;
Appalco 0:351a2fb21235 229
Appalco 0:351a2fb21235 230 // reset data counter
Appalco 0:351a2fb21235 231
Appalco 0:351a2fb21235 232 dataCounter = 0;
Appalco 0:351a2fb21235 233 }
Appalco 0:351a2fb21235 234 }
Appalco 0:351a2fb21235 235 }
Appalco 0:351a2fb21235 236 }
Appalco 0:351a2fb21235 237