File content as of revision 7:8a23a257b66a:

#include "laser.h"


Laser::Laser(Serial& serial) : serial(serial)
{
}


float Laser::getDistance()
{
    float distance = -2.0;
    const int bufSize = 50;
    char data[bufSize];
    int i = 0;
    //char cmd[] = ";\n";
    char cmd[] = "0 -1 10 doMeasDistExt\n";   // single reading averaged over 10 measurements
    //char cmd[] = "0 -1 -1 doMeasDistExt\n";     // single reading of single measurement
    if (sendCommand(cmd) == true) {
        // Note: Need to actually read from the serial to clear the RX interrupt
        char c = 0;
        uint16_t count = 0;
        do {
            if(serial.readable()) {
                c = serial.getc();
                data[i++] = c;
            }
            else{wait_us(100);}
        } while (c != '\n' && count++ < 1000);
        //while (c != '\n');
        if (count >= 1000) return -1;

        // now that we have the reply parse it
        data[i - 1] = 0;        // null terminate the string

        //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
        vector<char*> v;
        split(data, ' ', v);

        if (v.size() != 6 || atoi(v[1]) != 0 || strcmp(v[5], "Reply") != 0) {
            // there is an error
            distance = -1.0;
        } else {
            distance = atoi(v[2]) / 1000000.0; // distance in m
        }
    }

    return distance;
}

void Laser::enableMeasurement(bool enable)
{
    if (enable)
        sendCommand(";\n");
    else
        sendCommand("switchMeasOff");
}

bool Laser::sendCommand(char cmd[])
{
    for(int i = 0; i < strlen(cmd); i++) {
        serial.putc(cmd[i]);
    }

    return true;
}


void Laser::split(char str[], char c, std::vector<char*>& v)
{
  char * pch;
  char limiter[] = {c};
  pch = strtok (str, limiter);
  while (pch != NULL)
  {
     v.push_back(pch);
    pch = strtok (NULL, limiter);
  }
}

Laser::~Laser()
{
    enableMeasurement(false);
}