ROME2 / Mbed 2 deprecated ROME2_P5

ROME_P5

Dependencies:   mbed

LIDAR.cpp@0:29be10cb0afc, 2018-04-27 (annotated)

Committer:
Inaueadr
Date:
Fri Apr 27 08:47:34 2018 +0000
Revision:
0:29be10cb0afc
Hallo

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Inaueadr 0:29be10cb0afc 1 /*
Inaueadr 0:29be10cb0afc 2 * LIDAR.cpp
Inaueadr 0:29be10cb0afc 3 * Copyright (c) 2018, ZHAW
Inaueadr 0:29be10cb0afc 4 * All rights reserved.
Inaueadr 0:29be10cb0afc 5 */
Inaueadr 0:29be10cb0afc 6
Inaueadr 0:29be10cb0afc 7 #include <cmath>
Inaueadr 0:29be10cb0afc 8 #include "LIDAR.h"
Inaueadr 0:29be10cb0afc 9
Inaueadr 0:29be10cb0afc 10 using namespace std;
Inaueadr 0:29be10cb0afc 11
Inaueadr 0:29be10cb0afc 12 /**
Inaueadr 0:29be10cb0afc 13 * Creates a LIDAR object.
Inaueadr 0:29be10cb0afc 14 * @param serial a reference to a serial interface to communicate with the laser scanner.
Inaueadr 0:29be10cb0afc 15 */
Inaueadr 0:29be10cb0afc 16 LIDAR::LIDAR(RawSerial& serial) : serial(serial) {
Inaueadr 0:29be10cb0afc 17
Inaueadr 0:29be10cb0afc 18 // initialize serial interface
Inaueadr 0:29be10cb0afc 19
Inaueadr 0:29be10cb0afc 20 serial.baud(115200);
Inaueadr 0:29be10cb0afc 21 serial.format(8, SerialBase::None, 1);
Inaueadr 0:29be10cb0afc 22
Inaueadr 0:29be10cb0afc 23 // initialize local values
Inaueadr 0:29be10cb0afc 24
Inaueadr 0:29be10cb0afc 25 headerCounter = 0;
Inaueadr 0:29be10cb0afc 26 dataCounter = 0;
Inaueadr 0:29be10cb0afc 27
Inaueadr 0:29be10cb0afc 28 for (unsigned short i = 0; i < 360; i++) distances[i] = DEFAULT_DISTANCE;
Inaueadr 0:29be10cb0afc 29 distanceOfBeacon = 0;
Inaueadr 0:29be10cb0afc 30 angleOfBeacon = 0;
Inaueadr 0:29be10cb0afc 31
Inaueadr 0:29be10cb0afc 32 // start serial interrupt
Inaueadr 0:29be10cb0afc 33
Inaueadr 0:29be10cb0afc 34 serial.attach(callback(this, &LIDAR::receive), RawSerial::RxIrq);
Inaueadr 0:29be10cb0afc 35
Inaueadr 0:29be10cb0afc 36 // start the continuous operation of the LIDAR
Inaueadr 0:29be10cb0afc 37
Inaueadr 0:29be10cb0afc 38 serial.putc(START_FLAG);
Inaueadr 0:29be10cb0afc 39 serial.putc(SCAN);
Inaueadr 0:29be10cb0afc 40 }
Inaueadr 0:29be10cb0afc 41
Inaueadr 0:29be10cb0afc 42 /**
Inaueadr 0:29be10cb0afc 43 * Stops the lidar and deletes this object.
Inaueadr 0:29be10cb0afc 44 */
Inaueadr 0:29be10cb0afc 45 LIDAR::~LIDAR() {
Inaueadr 0:29be10cb0afc 46
Inaueadr 0:29be10cb0afc 47 // stop the LIDAR
Inaueadr 0:29be10cb0afc 48
Inaueadr 0:29be10cb0afc 49 serial.putc(START_FLAG);
Inaueadr 0:29be10cb0afc 50 serial.putc(STOP);
Inaueadr 0:29be10cb0afc 51 }
Inaueadr 0:29be10cb0afc 52
Inaueadr 0:29be10cb0afc 53 /**
Inaueadr 0:29be10cb0afc 54 * Returns the distance measurement of the lidar at a given angle.
Inaueadr 0:29be10cb0afc 55 * @param angle the angle, given in [deg] in the range 0..359.
Inaueadr 0:29be10cb0afc 56 * @return the measured distance, given in [mm].
Inaueadr 0:29be10cb0afc 57 */
Inaueadr 0:29be10cb0afc 58 short LIDAR::getDistance(short angle) {
Inaueadr 0:29be10cb0afc 59
Inaueadr 0:29be10cb0afc 60 while (angle < 0) angle += 360;
Inaueadr 0:29be10cb0afc 61 while (angle >= 360) angle -= 360;
Inaueadr 0:29be10cb0afc 62
Inaueadr 0:29be10cb0afc 63 return distances[angle];
Inaueadr 0:29be10cb0afc 64 }
Inaueadr 0:29be10cb0afc 65
Inaueadr 0:29be10cb0afc 66 /**
Inaueadr 0:29be10cb0afc 67 * Returns the distance to a detected beacon.
Inaueadr 0:29be10cb0afc 68 * @return the distance to the beacon, given in [mm], or zero, if no beacon was found.
Inaueadr 0:29be10cb0afc 69 */
Inaueadr 0:29be10cb0afc 70 short LIDAR::getDistanceOfBeacon() {
Inaueadr 0:29be10cb0afc 71
Inaueadr 0:29be10cb0afc 72 return distanceOfBeacon;
Inaueadr 0:29be10cb0afc 73 }
Inaueadr 0:29be10cb0afc 74
Inaueadr 0:29be10cb0afc 75 /**
Inaueadr 0:29be10cb0afc 76 * Returns the angle of a detected beacon.
Inaueadr 0:29be10cb0afc 77 * @return the angle of the beacon, given in [deg] in the range 0..359.
Inaueadr 0:29be10cb0afc 78 */
Inaueadr 0:29be10cb0afc 79 short LIDAR::getAngleOfBeacon() {
Inaueadr 0:29be10cb0afc 80
Inaueadr 0:29be10cb0afc 81 return angleOfBeacon;
Inaueadr 0:29be10cb0afc 82 }
Inaueadr 0:29be10cb0afc 83
Inaueadr 0:29be10cb0afc 84 /**
Inaueadr 0:29be10cb0afc 85 * This method implements an algorithm that looks for the position of a beacon.
Inaueadr 0:29be10cb0afc 86 * It should be called periodically by a low-priority background task.
Inaueadr 0:29be10cb0afc 87 */
Inaueadr 0:29be10cb0afc 88 void LIDAR::lookForBeacon() {
Inaueadr 0:29be10cb0afc 89
Inaueadr 0:29be10cb0afc 90 distanceOfBeacon = 0;
Inaueadr 0:29be10cb0afc 91 angleOfBeacon = 0;
Inaueadr 0:29be10cb0afc 92 for (i=0;i<360;i++){
Inaueadr 0:29be10cb0afc 93 dist_copy[i] = distances[i]; //local copy for the search of lighthouses
Inaueadr 0:29be10cb0afc 94 }
Inaueadr 0:29be10cb0afc 95 n=0;
Inaueadr 0:29be10cb0afc 96 m=0;
Inaueadr 0:29be10cb0afc 97 changed=false;
Inaueadr 0:29be10cb0afc 98 for (i=0;i<10;i++){
Inaueadr 0:29be10cb0afc 99 dist_diff_start[i]=0; //
Inaueadr 0:29be10cb0afc 100 dist_diff_stop[i]=0;
Inaueadr 0:29be10cb0afc 101 }
Inaueadr 0:29be10cb0afc 102 for (i=0;i<360;i++){
Inaueadr 0:29be10cb0afc 103
Inaueadr 0:29be10cb0afc 104 if ((distances[i-1]-distances[i])>500 && distances[i]>500 && distances[i]<2000){
Inaueadr 0:29be10cb0afc 105 /**dist_diff_start[n]=i;
Inaueadr 0:29be10cb0afc 106 n++;
Inaueadr 0:29be10cb0afc 107
Inaueadr 0:29be10cb0afc 108 printf(" start: %d \r\n",i);*/
Inaueadr 0:29be10cb0afc 109
Inaueadr 0:29be10cb0afc 110 n=i;
Inaueadr 0:29be10cb0afc 111 changed=true;
Inaueadr 0:29be10cb0afc 112 }
Inaueadr 0:29be10cb0afc 113 if ((distances[i+1]-distances[i])>500&&distances[i]>500&&distances[i]<2000){
Inaueadr 0:29be10cb0afc 114 /**dist_diff_stop[m]=i;
Inaueadr 0:29be10cb0afc 115 m++;
Inaueadr 0:29be10cb0afc 116 printf(" stop: %d \r\n",i);*/
Inaueadr 0:29be10cb0afc 117 m=i;
Inaueadr 0:29be10cb0afc 118 changed=true;
Inaueadr 0:29be10cb0afc 119 }
Inaueadr 0:29be10cb0afc 120 if((m-n)<9&&(m-n)>2&& changed){
Inaueadr 0:29be10cb0afc 121 range=m-n;
Inaueadr 0:29be10cb0afc 122 max=0;
Inaueadr 0:29be10cb0afc 123 min=2000;
Inaueadr 0:29be10cb0afc 124 for(x=0;x<range;x++){
Inaueadr 0:29be10cb0afc 125 if(min> distances[n+x]){
Inaueadr 0:29be10cb0afc 126 min=distances[n+x];
Inaueadr 0:29be10cb0afc 127 }
Inaueadr 0:29be10cb0afc 128 else if (max< distances[n+x]){
Inaueadr 0:29be10cb0afc 129 max=distances[n+x];
Inaueadr 0:29be10cb0afc 130 }
Inaueadr 0:29be10cb0afc 131 }
Inaueadr 0:29be10cb0afc 132 if((max-min)<75&&(max-min)>0){
Inaueadr 0:29be10cb0afc 133 distanceOfBeacon=(max+min)/2;
Inaueadr 0:29be10cb0afc 134 angleOfBeacon=(n+m)/2;
Inaueadr 0:29be10cb0afc 135
Inaueadr 0:29be10cb0afc 136 // printf(" Fenster gefunden!! Start:%d Stop:%d max:%d min:%d \r \n",n,m,max,min);
Inaueadr 0:29be10cb0afc 137 changed=false;
Inaueadr 0:29be10cb0afc 138 }
Inaueadr 0:29be10cb0afc 139 }
Inaueadr 0:29be10cb0afc 140 }
Inaueadr 0:29be10cb0afc 141
Inaueadr 0:29be10cb0afc 142 /** for(i=0;i<10;i++){
Inaueadr 0:29be10cb0afc 143 printf(" %d: %d ", i,dist_diff_start[i]);
Inaueadr 0:29be10cb0afc 144 }
Inaueadr 0:29be10cb0afc 145 printf("\n\r") ;
Inaueadr 0:29be10cb0afc 146 for(i=0;i<10;i++){
Inaueadr 0:29be10cb0afc 147 printf(" %d: %d ", i,dist_diff_stop[i]);
Inaueadr 0:29be10cb0afc 148 }*/
Inaueadr 0:29be10cb0afc 149 printf("\n\r") ;
Inaueadr 0:29be10cb0afc 150 printf("\n\r") ;
Inaueadr 0:29be10cb0afc 151 printf("\n\r") ;
Inaueadr 0:29be10cb0afc 152
Inaueadr 0:29be10cb0afc 153
Inaueadr 0:29be10cb0afc 154
Inaueadr 0:29be10cb0afc 155
Inaueadr 0:29be10cb0afc 156
Inaueadr 0:29be10cb0afc 157
Inaueadr 0:29be10cb0afc 158 // bitte implementieren!
Inaueadr 0:29be10cb0afc 159 }
Inaueadr 0:29be10cb0afc 160
Inaueadr 0:29be10cb0afc 161 /**
Inaueadr 0:29be10cb0afc 162 * This method is called by the serial interrupt service routine.
Inaueadr 0:29be10cb0afc 163 * It handles the reception of measurements from the LIDAR.
Inaueadr 0:29be10cb0afc 164 */
Inaueadr 0:29be10cb0afc 165 void LIDAR::receive() {
Inaueadr 0:29be10cb0afc 166
Inaueadr 0:29be10cb0afc 167 // read received characters while input buffer is full
Inaueadr 0:29be10cb0afc 168
Inaueadr 0:29be10cb0afc 169 if (serial.readable()) {
Inaueadr 0:29be10cb0afc 170
Inaueadr 0:29be10cb0afc 171 // read single character from serial interface
Inaueadr 0:29be10cb0afc 172
Inaueadr 0:29be10cb0afc 173 char c = serial.getc();
Inaueadr 0:29be10cb0afc 174
Inaueadr 0:29be10cb0afc 175 // add this character to the header or to the data buffer
Inaueadr 0:29be10cb0afc 176
Inaueadr 0:29be10cb0afc 177 if (headerCounter < HEADER_SIZE) {
Inaueadr 0:29be10cb0afc 178 headerCounter++;
Inaueadr 0:29be10cb0afc 179 } else {
Inaueadr 0:29be10cb0afc 180 if (dataCounter < DATA_SIZE) {
Inaueadr 0:29be10cb0afc 181 data[dataCounter] = c;
Inaueadr 0:29be10cb0afc 182 dataCounter++;
Inaueadr 0:29be10cb0afc 183 }
Inaueadr 0:29be10cb0afc 184 if (dataCounter >= DATA_SIZE) {
Inaueadr 0:29be10cb0afc 185
Inaueadr 0:29be10cb0afc 186 // data buffer is full, process measurement
Inaueadr 0:29be10cb0afc 187
Inaueadr 0:29be10cb0afc 188 char quality = data[0] >> 2;
Inaueadr 0:29be10cb0afc 189 short angle = 360-(((unsigned short)data[1] | ((unsigned short)data[2] << 8)) >> 1)/64;
Inaueadr 0:29be10cb0afc 190 int16_t distance = ((unsigned short)data[3] | ((unsigned short)data[4] << 8))/4;
Inaueadr 0:29be10cb0afc 191
Inaueadr 0:29be10cb0afc 192 if ((quality < QUALITY_THRESHOLD) || (distance < DISTANCE_THRESHOLD)) distance = DEFAULT_DISTANCE;
Inaueadr 0:29be10cb0afc 193
Inaueadr 0:29be10cb0afc 194 // store distance in [mm] into array of full scan
Inaueadr 0:29be10cb0afc 195
Inaueadr 0:29be10cb0afc 196 while (angle < 0) angle += 360;
Inaueadr 0:29be10cb0afc 197 while (angle >= 360) angle -= 360;
Inaueadr 0:29be10cb0afc 198 distances[angle] = distance;
Inaueadr 0:29be10cb0afc 199
Inaueadr 0:29be10cb0afc 200 // reset data counter
Inaueadr 0:29be10cb0afc 201
Inaueadr 0:29be10cb0afc 202 dataCounter = 0;
Inaueadr 0:29be10cb0afc 203 }
Inaueadr 0:29be10cb0afc 204 }
Inaueadr 0:29be10cb0afc 205 }
Inaueadr 0:29be10cb0afc 206 }
Inaueadr 0:29be10cb0afc 207