Bernard Mentink / Mbed 2 deprecated MCS_LRF_EEP

EEP fORK

Dependencies:   BLE_API mbed nRF51822

Fork of MCS_LRF by Farshad N

laser.cpp@16:dc9956bac1a3, 2017-03-01 (annotated)

Committer:
Farshad
Date:
Wed Mar 01 23:50:26 2017 +0000
Revision:
16:dc9956bac1a3
Parent:
15:bc4f8c597c26 
Serial Interrupt version- Similar problem of occasionally reporting distance with 1 or more decimal points wrong as version 15 in repository.- Revision 15 is polling and been tested more thoroughly and is the preferred version.

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Farshad 7:8a23a257b66a 1
Farshad 7:8a23a257b66a 2 #include "laser.h"
Farshad 7:8a23a257b66a 3
Farshad 8:ed66e7ef8243 4 #define LASER_BAUD_RATE 115200
Farshad 7:8a23a257b66a 5
Farshad 15:bc4f8c597c26 6 extern DigitalOut disableLRF;
Farshad 15:bc4f8c597c26 7 extern DigitalOut nReset;
Farshad 15:bc4f8c597c26 8
Farshad 15:bc4f8c597c26 9 uint32_t base = 0x40002000;
Farshad 15:bc4f8c597c26 10 uint32_t rxOffset = 0x514;
Farshad 15:bc4f8c597c26 11 uint32_t txOffset = 0x50c;
Farshad 15:bc4f8c597c26 12
Farshad 15:bc4f8c597c26 13 uint32_t rx = base + rxOffset;
Farshad 15:bc4f8c597c26 14 uint32_t tx = base + txOffset;
Farshad 15:bc4f8c597c26 15
Farshad 15:bc4f8c597c26 16 uint32_t* prx = (uint32_t*)rx;
Farshad 15:bc4f8c597c26 17 uint32_t* ptx = (uint32_t*)tx;
Farshad 15:bc4f8c597c26 18
Farshad 16:dc9956bac1a3 19 int bufIdx = 0;
Farshad 16:dc9956bac1a3 20 bool discard = true;
Farshad 16:dc9956bac1a3 21
Farshad 15:bc4f8c597c26 22 Laser::Laser(Serial& serial, int n) : timerRunning(false), idx(0), serial(serial), powerOffState (true), busy(false), nSamples(n)
Farshad 7:8a23a257b66a 23 {
Farshad 16:dc9956bac1a3 24 // serial.attach(this, &Laser::processResponse); // this causes a crash?? do the assignment from the caller
Farshad 7:8a23a257b66a 25 }
Farshad 7:8a23a257b66a 26
Farshad 12:cf8af0b4e0d2 27 void Laser::discardResponse()
Farshad 12:cf8af0b4e0d2 28 {
Farshad 16:dc9956bac1a3 29 discard = true;
Farshad 12:cf8af0b4e0d2 30 }
Farshad 12:cf8af0b4e0d2 31
Farshad 8:ed66e7ef8243 32 void Laser::triggerDistanceMeasurement()
Farshad 8:ed66e7ef8243 33 {
Farshad 15:bc4f8c597c26 34 if(!busy){
Farshad 15:bc4f8c597c26 35 const int bufSize = 30;
Farshad 15:bc4f8c597c26 36 char cmd[bufSize];
Farshad 15:bc4f8c597c26 37 snprintf(&cmd[0], bufSize, "0 -1 %d doMeasDistExt\n", nSamples);
Farshad 15:bc4f8c597c26 38 //char cmd[] = "0 -1 10 doMeasDistExt\n"; // single reading averaged over 10 measurements
Farshad 15:bc4f8c597c26 39 //char cmd[] = "0 -1 100 doMeasDistExt\n"; // single reading averaged over 100 measurements
Farshad 15:bc4f8c597c26 40 //char cmd[] = "0 -1 -1 doMeasDistExt\n"; // single reading auto choice of number of averages - This could make the laser to lock up and may need reseting
Farshad 15:bc4f8c597c26 41
Farshad 16:dc9956bac1a3 42 busy = true;
Farshad 16:dc9956bac1a3 43 discard = false;
Farshad 15:bc4f8c597c26 44 sendCommand(cmd);
Farshad 15:bc4f8c597c26 45 }
Farshad 7:8a23a257b66a 46 }
Farshad 7:8a23a257b66a 47
Farshad 7:8a23a257b66a 48 bool Laser::sendCommand(char cmd[])
Farshad 7:8a23a257b66a 49 {
Farshad 8:ed66e7ef8243 50 // start timer before the first of the command is sent to the laser
Farshad 15:bc4f8c597c26 51 timer.reset();
Farshad 8:ed66e7ef8243 52 timer.start();
Farshad 8:ed66e7ef8243 53 timerRunning = true;
Farshad 8:ed66e7ef8243 54
Farshad 7:8a23a257b66a 55 for(int i = 0; i < strlen(cmd); i++) {
Farshad 7:8a23a257b66a 56 serial.putc(cmd[i]);
Farshad 7:8a23a257b66a 57 }
Farshad 7:8a23a257b66a 58
Farshad 7:8a23a257b66a 59 return true;
Farshad 8:ed66e7ef8243 60 }
Farshad 8:ed66e7ef8243 61
Farshad 16:dc9956bac1a3 62 void Laser::processRx()
Farshad 8:ed66e7ef8243 63 {
Farshad 16:dc9956bac1a3 64 char c = serial.getc();
Farshad 16:dc9956bac1a3 65
Farshad 16:dc9956bac1a3 66 if(!discard) {
Farshad 16:dc9956bac1a3 67 if(timerRunning) {
Farshad 16:dc9956bac1a3 68 timer.stop();
Farshad 16:dc9956bac1a3 69 timerRunning = false;
Farshad 8:ed66e7ef8243 70 }
Farshad 8:ed66e7ef8243 71
Farshad 16:dc9956bac1a3 72 buf[bufIdx++] =c;
Farshad 8:ed66e7ef8243 73
Farshad 16:dc9956bac1a3 74 // TODO sort our timeout
Farshad 16:dc9956bac1a3 75 if(bufIdx == bufSize) {
Farshad 16:dc9956bac1a3 76 // need to reset the LRF module before talking to it again, otherwise it may lock up
Farshad 16:dc9956bac1a3 77 nReset = 0;
Farshad 16:dc9956bac1a3 78 wait_ms(100);
Farshad 16:dc9956bac1a3 79 nReset = 1;
Farshad 16:dc9956bac1a3 80 wait_ms(1000);
Farshad 16:dc9956bac1a3 81 enableMeasurement(true);
Farshad 16:dc9956bac1a3 82 busy = false;
Farshad 16:dc9956bac1a3 83 bufIdx = 0;
Farshad 16:dc9956bac1a3 84 }
Farshad 15:bc4f8c597c26 85
Farshad 16:dc9956bac1a3 86 if(c == '\n') {
Farshad 16:dc9956bac1a3 87 buf[bufIdx-1] = 0; // replace '\n' with string null ternimator
Farshad 16:dc9956bac1a3 88 processResponse();
Farshad 16:dc9956bac1a3 89 discard = false;
Farshad 16:dc9956bac1a3 90 busy = false;
Farshad 16:dc9956bac1a3 91 bufIdx = 0;
Farshad 16:dc9956bac1a3 92 }
Farshad 8:ed66e7ef8243 93 }
Farshad 16:dc9956bac1a3 94 }
Farshad 16:dc9956bac1a3 95
Farshad 16:dc9956bac1a3 96 void Laser::processResponse()
Farshad 16:dc9956bac1a3 97 {
Farshad 16:dc9956bac1a3 98 if(!busy) return;
Farshad 8:ed66e7ef8243 99
Farshad 8:ed66e7ef8243 100 float distance = -5;
Farshad 8:ed66e7ef8243 101 vector<char*> v;
Farshad 8:ed66e7ef8243 102
Farshad 8:ed66e7ef8243 103 split(buf, ' ', v);
Farshad 8:ed66e7ef8243 104
Farshad 8:ed66e7ef8243 105 if (v.size() != 6 || atoi(v[1]) != 0 || strcmp(v[5], "Reply") != 0) {
Farshad 8:ed66e7ef8243 106 // there is an error
Farshad 8:ed66e7ef8243 107 distanceCallback(-1.0, 0.0);
Farshad 8:ed66e7ef8243 108 } else {
Farshad 8:ed66e7ef8243 109 float elapsed = (float)(timer.read_us()/1000.0); // elapsed in ms
Farshad 8:ed66e7ef8243 110 distance = atoi(v[2]) / 1000000.0; // distance in m
Farshad 8:ed66e7ef8243 111 distanceCallback(distance, elapsed);
Farshad 8:ed66e7ef8243 112 }
Farshad 7:8a23a257b66a 113 }
Farshad 7:8a23a257b66a 114
Farshad 11:0dafbbb3a686 115 void Laser::enableMeasurement(bool enable)
Farshad 11:0dafbbb3a686 116 {
Farshad 12:cf8af0b4e0d2 117 if (enable) {
Farshad 11:0dafbbb3a686 118 sendCommand(";\n");
Farshad 12:cf8af0b4e0d2 119 discardResponse();
Farshad 12:cf8af0b4e0d2 120
Farshad 12:cf8af0b4e0d2 121 } else {
Farshad 11:0dafbbb3a686 122 sendCommand("switchMeasOff\n");
Farshad 12:cf8af0b4e0d2 123 discardResponse();
Farshad 12:cf8af0b4e0d2 124 }
Farshad 11:0dafbbb3a686 125 }
Farshad 11:0dafbbb3a686 126
Farshad 11:0dafbbb3a686 127 void Laser::setRedDot(bool on)
Farshad 11:0dafbbb3a686 128 {
Farshad 11:0dafbbb3a686 129 if(on) {
Farshad 15:bc4f8c597c26 130 //char cmd[] = "0 -1 5 doMeasDistExt\n"; // doing a measurement turns the redDot on
Farshad 15:bc4f8c597c26 131 char cmd[] = "0 3 0 -1 3 doLaserCmd;\n";
Farshad 11:0dafbbb3a686 132 sendCommand(cmd);
Farshad 12:cf8af0b4e0d2 133 wait_ms(120); // take a while for a response for to measurement command
Farshad 12:cf8af0b4e0d2 134 discardResponse();
Farshad 15:bc4f8c597c26 135 // triggerDistanceMeasurement();
Farshad 12:cf8af0b4e0d2 136 } else {
Farshad 11:0dafbbb3a686 137 sendCommand("switchMeasOff\n");
Farshad 12:cf8af0b4e0d2 138 discardResponse();
Farshad 12:cf8af0b4e0d2 139 }
Farshad 11:0dafbbb3a686 140 }
Farshad 7:8a23a257b66a 141
Farshad 7:8a23a257b66a 142 void Laser::split(char str[], char c, std::vector<char*>& v)
Farshad 7:8a23a257b66a 143 {
Farshad 8:ed66e7ef8243 144 char * pch;
Farshad 8:ed66e7ef8243 145 char limiter[] = {c};
Farshad 8:ed66e7ef8243 146 pch = strtok (str, limiter);
Farshad 8:ed66e7ef8243 147 while (pch != NULL) {
Farshad 8:ed66e7ef8243 148 v.push_back(pch);
Farshad 8:ed66e7ef8243 149 pch = strtok (NULL, limiter);
Farshad 8:ed66e7ef8243 150 }
Farshad 7:8a23a257b66a 151 }
Farshad 7:8a23a257b66a 152
Farshad 8:ed66e7ef8243 153 void Laser::setDistaceCallback(void (*callback)(float, float))
Farshad 8:ed66e7ef8243 154 {
Farshad 8:ed66e7ef8243 155 distanceCallback = callback;
Farshad 8:ed66e7ef8243 156 }
Farshad 8:ed66e7ef8243 157
Farshad 8:ed66e7ef8243 158 void Laser::setDebugCallback(void (*callback)(char*))
Farshad 8:ed66e7ef8243 159 {
Farshad 8:ed66e7ef8243 160 debugCallback = callback;
Farshad 8:ed66e7ef8243 161 }
Farshad 8:ed66e7ef8243 162
Farshad 15:bc4f8c597c26 163 void Laser::turnLaserPowerOn()
Farshad 15:bc4f8c597c26 164 {
Farshad 15:bc4f8c597c26 165 if(powerOffState == true) {
Farshad 15:bc4f8c597c26 166 powerOffState = false;
Farshad 15:bc4f8c597c26 167
Farshad 15:bc4f8c597c26 168 connectPower();
Farshad 15:bc4f8c597c26 169
Farshad 15:bc4f8c597c26 170 enableMeasurement(true);
Farshad 15:bc4f8c597c26 171 setRedDot(1);
Farshad 15:bc4f8c597c26 172 }
Farshad 15:bc4f8c597c26 173 }
Farshad 15:bc4f8c597c26 174
Farshad 15:bc4f8c597c26 175 void Laser::turnLaserPowerOff()
Farshad 15:bc4f8c597c26 176 {
Farshad 15:bc4f8c597c26 177 if(powerOffState == false) {
Farshad 15:bc4f8c597c26 178 powerOffState = true;
Farshad 15:bc4f8c597c26 179 setRedDot(0); // this disables measurements
Farshad 15:bc4f8c597c26 180
Farshad 15:bc4f8c597c26 181 removePower();
Farshad 15:bc4f8c597c26 182 }
Farshad 15:bc4f8c597c26 183 }
Farshad 15:bc4f8c597c26 184
Farshad 15:bc4f8c597c26 185 // when connceting power to the laser module ensure that tx and rx pins are
Farshad 15:bc4f8c597c26 186 // reassigned to the serial port
Farshad 15:bc4f8c597c26 187 void Laser::connectPower()
Farshad 15:bc4f8c597c26 188 {
Farshad 15:bc4f8c597c26 189 disableLRF = 0;
Farshad 15:bc4f8c597c26 190 nReset = 1;
Farshad 15:bc4f8c597c26 191 *ptx = 27; // p27 for tx
Farshad 15:bc4f8c597c26 192 *prx = 26; // p26 for rx
Farshad 15:bc4f8c597c26 193 wait_ms(1000);
Farshad 15:bc4f8c597c26 194 }
Farshad 15:bc4f8c597c26 195
Farshad 15:bc4f8c597c26 196 // when removing power from the laser module ensure that rx and tx pins are not
Farshad 15:bc4f8c597c26 197 // driving voltage into the laser module to avoid hardware damage
Farshad 15:bc4f8c597c26 198 void Laser::removePower()
Farshad 15:bc4f8c597c26 199 {
Farshad 15:bc4f8c597c26 200 *ptx = 0xffffffff; // no pin for tx
Farshad 15:bc4f8c597c26 201 *prx = 0xffffffff; // no pin for rx
Farshad 15:bc4f8c597c26 202 DigitalOut rx(p26);
Farshad 15:bc4f8c597c26 203 DigitalOut tx(p27);
Farshad 15:bc4f8c597c26 204 rx = 0;
Farshad 15:bc4f8c597c26 205 tx = 0;
Farshad 15:bc4f8c597c26 206 nReset = 0;
Farshad 15:bc4f8c597c26 207 disableLRF = 1;
Farshad 15:bc4f8c597c26 208 }
Farshad 15:bc4f8c597c26 209
Farshad 7:8a23a257b66a 210 Laser::~Laser()
Farshad 7:8a23a257b66a 211 {
Farshad 7:8a23a257b66a 212 enableMeasurement(false);
Farshad 7:8a23a257b66a 213 }
Farshad 10:d37cd13dd529 214
Farshad 10:d37cd13dd529 215
Farshad 10:d37cd13dd529 216
Farshad 11:0dafbbb3a686 217
Farshad 11:0dafbbb3a686 218 //void Laser::processRxData(char c)
Farshad 11:0dafbbb3a686 219 //{
Farshad 11:0dafbbb3a686 220 // if(c != '\n') {
Farshad 11:0dafbbb3a686 221 // buf[idx++] = c;
Farshad 11:0dafbbb3a686 222 // if(idx == bufSize) idx = 0; // avoid overflow
Farshad 11:0dafbbb3a686 223 // } else {
Farshad 11:0dafbbb3a686 224 // buf[idx] = 0; // null terminate the string
Farshad 11:0dafbbb3a686 225 // processBuffer();
Farshad 11:0dafbbb3a686 226 // idx = 0;
Farshad 11:0dafbbb3a686 227 // }
Farshad 11:0dafbbb3a686 228 //}
Farshad 11:0dafbbb3a686 229
Farshad 10:d37cd13dd529 230 //float Laser::getDistance()
Farshad 10:d37cd13dd529 231 //{
Farshad 10:d37cd13dd529 232 // float distance = -4.0;
Farshad 10:d37cd13dd529 233 // const int bufSize = 50;
Farshad 10:d37cd13dd529 234 // char data[bufSize];
Farshad 10:d37cd13dd529 235 // int i = 0;
Farshad 10:d37cd13dd529 236 // //char cmd[] = ";\n";
Farshad 10:d37cd13dd529 237 // char cmd[] = "0 -1 10 doMeasDistExt\n"; // single reading averaged over 10 measurements
Farshad 10:d37cd13dd529 238 // //char cmd[] = "0 -1 -1 doMeasDistExt\n"; // single reading of single measurement
Farshad 10:d37cd13dd529 239 //
Farshad 10:d37cd13dd529 240 // if (sendCommand(cmd) == true) {
Farshad 10:d37cd13dd529 241 // // Note: Need to actually read from the serial to clear the RX interrupt
Farshad 10:d37cd13dd529 242 // char c = 0;
Farshad 10:d37cd13dd529 243 // uint16_t count = 0;
Farshad 10:d37cd13dd529 244 // do {
Farshad 10:d37cd13dd529 245 // if(serial.readable()) {
Farshad 10:d37cd13dd529 246 // c = serial.getc();
Farshad 10:d37cd13dd529 247 // data[i++] = c;
Farshad 10:d37cd13dd529 248 // } else {
Farshad 10:d37cd13dd529 249 // wait_us(100);
Farshad 10:d37cd13dd529 250 // }
Farshad 10:d37cd13dd529 251 // } while (c != '\n' && i < bufSize && count++ < 5000); // timeout after about 500ms and ensure no overflow
Farshad 10:d37cd13dd529 252 //
Farshad 10:d37cd13dd529 253 // if (count >= 5000 || i >= bufSize) return -2.0; // timeout or overflow
Farshad 10:d37cd13dd529 254 //
Farshad 10:d37cd13dd529 255 // // now that we have the reply parse it
Farshad 10:d37cd13dd529 256 // data[i - 1] = 0; // null terminate the string
Farshad 10:d37cd13dd529 257 //
Farshad 10:d37cd13dd529 258 // //reply is in this form '2 0 1844364 324 Reply'. 2nd number is error code, 3rd number is distance in um, 4th number is measurement signal
Farshad 10:d37cd13dd529 259 // vector<char*> v;
Farshad 10:d37cd13dd529 260 // split(data, ' ', v);
Farshad 10:d37cd13dd529 261 //
Farshad 10:d37cd13dd529 262 // if (v.size() != 6 || atoi(v[1]) != 0 || strcmp(v[5], "Reply") != 0) {
Farshad 10:d37cd13dd529 263 // // there is an error
Farshad 10:d37cd13dd529 264 // distance = -3.0;
Farshad 10:d37cd13dd529 265 // } else {
Farshad 10:d37cd13dd529 266 // distance = atoi(v[2]) / 1000000.0; // distance in m
Farshad 10:d37cd13dd529 267 // }
Farshad 10:d37cd13dd529 268 // }
Farshad 10:d37cd13dd529 269 //
Farshad 10:d37cd13dd529 270 // return distance;
Farshad 10:d37cd13dd529 271 //}
Farshad 10:d37cd13dd529 272
Farshad 11:0dafbbb3a686 273 //void Laser::processBuffer()
Farshad 11:0dafbbb3a686 274 //{
Farshad 11:0dafbbb3a686 275 // //float distance;
Farshad 11:0dafbbb3a686 276 //// vector<char*> v;
Farshad 11:0dafbbb3a686 277 //// // debugCallback(buf);
Farshad 11:0dafbbb3a686 278 ////
Farshad 11:0dafbbb3a686 279 //// split(buf, ' ', v);
Farshad 11:0dafbbb3a686 280 ////
Farshad 11:0dafbbb3a686 281 //// if (v.size() != 6 || atoi(v[1]) != 0 || strcmp(v[5], "Reply") != 0) {
Farshad 11:0dafbbb3a686 282 //// // there is an error
Farshad 11:0dafbbb3a686 283 //// distanceCallback(-1.0);
Farshad 11:0dafbbb3a686 284 //// } else
Farshad 11:0dafbbb3a686 285 //// {
Farshad 11:0dafbbb3a686 286 //// distance = atoi(v[2]) / 1000000.0; // distance in m
Farshad 11:0dafbbb3a686 287 //// distanceCallback(distance);
Farshad 11:0dafbbb3a686 288 //// }
Farshad 11:0dafbbb3a686 289 //
Farshad 11:0dafbbb3a686 290 //
Farshad 11:0dafbbb3a686 291 // float distance = -5;
Farshad 11:0dafbbb3a686 292 // vector<char*> v;
Farshad 11:0dafbbb3a686 293 //
Farshad 11:0dafbbb3a686 294 // split(buf, ' ', v);
Farshad 11:0dafbbb3a686 295 //
Farshad 11:0dafbbb3a686 296 // if (v.size() != 6 || atoi(v[1]) != 0 || strcmp(v[5], "Reply") != 0) {
Farshad 11:0dafbbb3a686 297 // // there is an error
Farshad 11:0dafbbb3a686 298 // distanceCallback(-1.0, 0.0);
Farshad 11:0dafbbb3a686 299 // } else {
Farshad 11:0dafbbb3a686 300 // float elapsed = (float)(timer.read_us()/1000.0); // elapsed in ms
Farshad 11:0dafbbb3a686 301 // distance = atoi(v[2]) / 1000000.0; // distance in m
Farshad 11:0dafbbb3a686 302 // distanceCallback(distance, elapsed);
Farshad 11:0dafbbb3a686 303 // }
Farshad 11:0dafbbb3a686 304 //}
Farshad 11:0dafbbb3a686 305