EEP fORK
Dependencies: BLE_API mbed nRF51822
Fork of MCS_LRF by
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?
User | Revision | Line number | New 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 |