Nim leo niiiim
LIDAR.cpp@0:da791f233257, 2018-05-11 (annotated)
- Committer:
- Kiwicjam
- Date:
- Fri May 11 12:21:19 2018 +0000
- Revision:
- 0:da791f233257
start of rome2 p5;
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
Kiwicjam | 0:da791f233257 | 1 | /* |
Kiwicjam | 0:da791f233257 | 2 | * LIDAR.cpp |
Kiwicjam | 0:da791f233257 | 3 | * Copyright (c) 2018, ZHAW |
Kiwicjam | 0:da791f233257 | 4 | * All rights reserved. |
Kiwicjam | 0:da791f233257 | 5 | */ |
Kiwicjam | 0:da791f233257 | 6 | |
Kiwicjam | 0:da791f233257 | 7 | #include <cmath> |
Kiwicjam | 0:da791f233257 | 8 | #include "LIDAR.h" |
Kiwicjam | 0:da791f233257 | 9 | |
Kiwicjam | 0:da791f233257 | 10 | using namespace std; |
Kiwicjam | 0:da791f233257 | 11 | |
Kiwicjam | 0:da791f233257 | 12 | /** |
Kiwicjam | 0:da791f233257 | 13 | * Creates a LIDAR object. |
Kiwicjam | 0:da791f233257 | 14 | * @param serial a reference to a serial interface to communicate with the laser scanner. |
Kiwicjam | 0:da791f233257 | 15 | */ |
Kiwicjam | 0:da791f233257 | 16 | LIDAR::LIDAR(RawSerial& serial) : serial(serial) { |
Kiwicjam | 0:da791f233257 | 17 | |
Kiwicjam | 0:da791f233257 | 18 | // initialize serial interface |
Kiwicjam | 0:da791f233257 | 19 | |
Kiwicjam | 0:da791f233257 | 20 | serial.baud(115200); |
Kiwicjam | 0:da791f233257 | 21 | serial.format(8, SerialBase::None, 1); |
Kiwicjam | 0:da791f233257 | 22 | |
Kiwicjam | 0:da791f233257 | 23 | // initialize local values |
Kiwicjam | 0:da791f233257 | 24 | |
Kiwicjam | 0:da791f233257 | 25 | headerCounter = 0; |
Kiwicjam | 0:da791f233257 | 26 | dataCounter = 0; |
Kiwicjam | 0:da791f233257 | 27 | |
Kiwicjam | 0:da791f233257 | 28 | for (unsigned short i = 0; i < 360; i++) distances[i] = DEFAULT_DISTANCE; |
Kiwicjam | 0:da791f233257 | 29 | distanceOfBeacon = 0; |
Kiwicjam | 0:da791f233257 | 30 | angleOfBeacon = 0; |
Kiwicjam | 0:da791f233257 | 31 | |
Kiwicjam | 0:da791f233257 | 32 | // start serial interrupt |
Kiwicjam | 0:da791f233257 | 33 | |
Kiwicjam | 0:da791f233257 | 34 | serial.attach(callback(this, &LIDAR::receive), RawSerial::RxIrq); |
Kiwicjam | 0:da791f233257 | 35 | |
Kiwicjam | 0:da791f233257 | 36 | // start the continuous operation of the LIDAR |
Kiwicjam | 0:da791f233257 | 37 | |
Kiwicjam | 0:da791f233257 | 38 | serial.putc(START_FLAG); |
Kiwicjam | 0:da791f233257 | 39 | serial.putc(SCAN); |
Kiwicjam | 0:da791f233257 | 40 | } |
Kiwicjam | 0:da791f233257 | 41 | |
Kiwicjam | 0:da791f233257 | 42 | /** |
Kiwicjam | 0:da791f233257 | 43 | * Stops the lidar and deletes this object. |
Kiwicjam | 0:da791f233257 | 44 | */ |
Kiwicjam | 0:da791f233257 | 45 | LIDAR::~LIDAR() { |
Kiwicjam | 0:da791f233257 | 46 | |
Kiwicjam | 0:da791f233257 | 47 | // stop the LIDAR |
Kiwicjam | 0:da791f233257 | 48 | |
Kiwicjam | 0:da791f233257 | 49 | serial.putc(START_FLAG); |
Kiwicjam | 0:da791f233257 | 50 | serial.putc(STOP); |
Kiwicjam | 0:da791f233257 | 51 | } |
Kiwicjam | 0:da791f233257 | 52 | |
Kiwicjam | 0:da791f233257 | 53 | /** |
Kiwicjam | 0:da791f233257 | 54 | * Returns the distance measurement of the lidar at a given angle. |
Kiwicjam | 0:da791f233257 | 55 | * @param angle the angle, given in [deg] in the range 0..359. |
Kiwicjam | 0:da791f233257 | 56 | * @return the measured distance, given in [mm]. |
Kiwicjam | 0:da791f233257 | 57 | */ |
Kiwicjam | 0:da791f233257 | 58 | short LIDAR::getDistance(short angle) { |
Kiwicjam | 0:da791f233257 | 59 | |
Kiwicjam | 0:da791f233257 | 60 | while (angle < 0) angle += 360; |
Kiwicjam | 0:da791f233257 | 61 | while (angle >= 360) angle -= 360; |
Kiwicjam | 0:da791f233257 | 62 | |
Kiwicjam | 0:da791f233257 | 63 | return distances[angle]; |
Kiwicjam | 0:da791f233257 | 64 | } |
Kiwicjam | 0:da791f233257 | 65 | |
Kiwicjam | 0:da791f233257 | 66 | /** |
Kiwicjam | 0:da791f233257 | 67 | * Returns the distance to a detected beacon. |
Kiwicjam | 0:da791f233257 | 68 | * @return the distance to the beacon, given in [mm], or zero, if no beacon was found. |
Kiwicjam | 0:da791f233257 | 69 | */ |
Kiwicjam | 0:da791f233257 | 70 | short LIDAR::getDistanceOfBeacon() { |
Kiwicjam | 0:da791f233257 | 71 | |
Kiwicjam | 0:da791f233257 | 72 | return distanceOfBeacon; |
Kiwicjam | 0:da791f233257 | 73 | } |
Kiwicjam | 0:da791f233257 | 74 | |
Kiwicjam | 0:da791f233257 | 75 | /** |
Kiwicjam | 0:da791f233257 | 76 | * Returns the angle of a detected beacon. |
Kiwicjam | 0:da791f233257 | 77 | * @return the angle of the beacon, given in [deg] in the range 0..359. |
Kiwicjam | 0:da791f233257 | 78 | */ |
Kiwicjam | 0:da791f233257 | 79 | short LIDAR::getAngleOfBeacon() { |
Kiwicjam | 0:da791f233257 | 80 | |
Kiwicjam | 0:da791f233257 | 81 | return angleOfBeacon; |
Kiwicjam | 0:da791f233257 | 82 | } |
Kiwicjam | 0:da791f233257 | 83 | |
Kiwicjam | 0:da791f233257 | 84 | /** |
Kiwicjam | 0:da791f233257 | 85 | * This method implements an algorithm that looks for the position of a beacon. |
Kiwicjam | 0:da791f233257 | 86 | * It should be called periodically by a low-priority background task. |
Kiwicjam | 0:da791f233257 | 87 | */ |
Kiwicjam | 0:da791f233257 | 88 | void LIDAR::lookForBeacon() { |
Kiwicjam | 0:da791f233257 | 89 | // copy distances to internal array |
Kiwicjam | 0:da791f233257 | 90 | short copy_distances[360]; |
Kiwicjam | 0:da791f233257 | 91 | for( uint8_t i = 0; i < 360; i++){ |
Kiwicjam | 0:da791f233257 | 92 | copy_distances[i] = this->distances[i]; |
Kiwicjam | 0:da791f233257 | 93 | } |
Kiwicjam | 0:da791f233257 | 94 | |
Kiwicjam | 0:da791f233257 | 95 | distanceOfBeacon = 0; |
Kiwicjam | 0:da791f233257 | 96 | angleOfBeacon = 0; |
Kiwicjam | 0:da791f233257 | 97 | |
Kiwicjam | 0:da791f233257 | 98 | // bitte implementieren! |
Kiwicjam | 0:da791f233257 | 99 | } |
Kiwicjam | 0:da791f233257 | 100 | |
Kiwicjam | 0:da791f233257 | 101 | /** |
Kiwicjam | 0:da791f233257 | 102 | * This method is called by the serial interrupt service routine. |
Kiwicjam | 0:da791f233257 | 103 | * It handles the reception of measurements from the LIDAR. |
Kiwicjam | 0:da791f233257 | 104 | */ |
Kiwicjam | 0:da791f233257 | 105 | void LIDAR::receive() { |
Kiwicjam | 0:da791f233257 | 106 | |
Kiwicjam | 0:da791f233257 | 107 | // read received characters while input buffer is full |
Kiwicjam | 0:da791f233257 | 108 | |
Kiwicjam | 0:da791f233257 | 109 | if (serial.readable()) { |
Kiwicjam | 0:da791f233257 | 110 | |
Kiwicjam | 0:da791f233257 | 111 | // read single character from serial interface |
Kiwicjam | 0:da791f233257 | 112 | |
Kiwicjam | 0:da791f233257 | 113 | char c = serial.getc(); |
Kiwicjam | 0:da791f233257 | 114 | |
Kiwicjam | 0:da791f233257 | 115 | // add this character to the header or to the data buffer |
Kiwicjam | 0:da791f233257 | 116 | |
Kiwicjam | 0:da791f233257 | 117 | if (headerCounter < HEADER_SIZE) { |
Kiwicjam | 0:da791f233257 | 118 | headerCounter++; |
Kiwicjam | 0:da791f233257 | 119 | } else { |
Kiwicjam | 0:da791f233257 | 120 | if (dataCounter < DATA_SIZE) { |
Kiwicjam | 0:da791f233257 | 121 | data[dataCounter] = c; |
Kiwicjam | 0:da791f233257 | 122 | dataCounter++; |
Kiwicjam | 0:da791f233257 | 123 | } |
Kiwicjam | 0:da791f233257 | 124 | if (dataCounter >= DATA_SIZE) { |
Kiwicjam | 0:da791f233257 | 125 | |
Kiwicjam | 0:da791f233257 | 126 | // data buffer is full, process measurement |
Kiwicjam | 0:da791f233257 | 127 | |
Kiwicjam | 0:da791f233257 | 128 | char quality = data[0] >> 2; |
Kiwicjam | 0:da791f233257 | 129 | short angle = 360-(((unsigned short)data[1] | ((unsigned short)data[2] << 8)) >> 1)/64; |
Kiwicjam | 0:da791f233257 | 130 | int16_t distance = ((unsigned short)data[3] | ((unsigned short)data[4] << 8))/4; |
Kiwicjam | 0:da791f233257 | 131 | |
Kiwicjam | 0:da791f233257 | 132 | if ((quality < QUALITY_THRESHOLD) || (distance < DISTANCE_THRESHOLD)) distance = DEFAULT_DISTANCE; |
Kiwicjam | 0:da791f233257 | 133 | |
Kiwicjam | 0:da791f233257 | 134 | // store distance in [mm] into array of full scan |
Kiwicjam | 0:da791f233257 | 135 | |
Kiwicjam | 0:da791f233257 | 136 | while (angle < 0) angle += 360; |
Kiwicjam | 0:da791f233257 | 137 | while (angle >= 360) angle -= 360; |
Kiwicjam | 0:da791f233257 | 138 | distances[angle] = distance; |
Kiwicjam | 0:da791f233257 | 139 | |
Kiwicjam | 0:da791f233257 | 140 | // reset data counter |
Kiwicjam | 0:da791f233257 | 141 | |
Kiwicjam | 0:da791f233257 | 142 | dataCounter = 0; |
Kiwicjam | 0:da791f233257 | 143 | } |
Kiwicjam | 0:da791f233257 | 144 | } |
Kiwicjam | 0:da791f233257 | 145 | } |
Kiwicjam | 0:da791f233257 | 146 | } |