Allan Brignoli / Mbed 2 deprecated Rome2_P6

gugus

Dependencies:   mbed

LIDAR.cpp@0:1a0321f1ffbc, 2018-05-18 (annotated)

Committer:
Brignall
Date:
Fri May 18 12:18:21 2018 +0000
Revision:
0:1a0321f1ffbc
lala;

Who changed what in which revision?

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