TeamSurface / Libs

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?

UserRevisionLine numberNew 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 }