File content as of revision 10:d37cd13dd529:

#include "laser.h"

#define LASER_BAUD_RATE 115200

Laser::Laser(Serial& serial) : timerRunning(false), idx(0), serial(serial)
{
}

void Laser::triggerDistanceMeasurement()
{
    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 - This could make the laser to lock up and may need reseting


    sendCommand(cmd);

    processResponse();
//    float distance = -5;
//        vector<char*> v;
//
//        split(buf, ' ', v);
//
//        if (v.size() != 6 || atoi(v[1]) != 0 || strcmp(v[5], "Reply") != 0) {
//            // there is an error
//            distanceCallback(-1.0);
//        } else {
//            distance = atoi(v[2]) / 1000000.0; // distance in m
//            distanceCallback(distance);
//        }
//
//
//  // processBuffer();

//  float distance =  getDistance();
//  distanceCallback(distance);
}

void Laser::processBuffer()
{
    //float distance;
//    vector<char*> v;
//   // debugCallback(buf);
//
//    split(buf, ' ', v);
//
//    if (v.size() != 6 || atoi(v[1]) != 0 || strcmp(v[5], "Reply") != 0) {
//        // there is an error
//        distanceCallback(-1.0);
//    } else
//    {
//        distance = atoi(v[2]) / 1000000.0; // distance in m
//        distanceCallback(distance);
//    }


    float distance = -5;
    vector<char*> v;

    split(buf, ' ', v);

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



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

void Laser::setRedDot(bool on)
{
    if(on) {
        char cmd[] = "0 -1 10 doMeasDistExt\n";     // doing a measurement turns the redDot on
        sendCommand(cmd);
    } else
        sendCommand("switchMeasOff\n");
}

bool Laser::sendCommand(char cmd[])
{
    // start timer before the first of the command is sent to the laser
    timer.reset();    
    timer.start();
    timerRunning = true;

    for(int i = 0; i < strlen(cmd); i++) {
        serial.putc(cmd[i]);
    }

    return true;

    //  return processResponse();
}

bool Laser::processResponse()
{

    // // Note: Need to actually read from the serial to clear the RX interrupt
//        char c = 0;
//        uint16_t count = 0;
//        idx = 0;
//        do {
//            if(serial.readable()) {
//                buf[idx++] = serial.getc();
//            }
//            else{wait_us(100);}
//        } while (c != '\n' && idx < bufSize && count++ < 5000);  // timeout after about 500ms and ensure no overflow
//
//        if (count >= 5000 || idx == 0 || idx >= bufSize) return false;  // timeout or overflow
//
//        // now that we have the reply parse it
//        buf[idx - 1] = 0;        // null terminate the string
//
//       // processBuffer();
//        return true;



    int i = 0;
    char c = 0;
    uint16_t count = 0;
    do {
        if(serial.readable()) {
            // stop timer as soon as we have received the first byte of the response. We need to subtract the time of this receiption
            if(timerRunning) {
                timer.stop();
                timerRunning = false;
            }
            c = serial.getc();
            buf[i++] = c;
        } else {
            wait_us(100);
        }
    } while (c != '\n' && i < bufSize && count++ < 5000);  // timeout after about 500ms and ensure no overflow



    if (count >= 5000 || i >= bufSize) { // timeout or overflow
    } else {
        buf[i -1] = 0;
    }

    float distance = -5;
    vector<char*> v;

    split(buf, ' ', v);

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

//processBuffer();
    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);
    }
}

void Laser::processRxData(char c)
{
    if(c != '\n') {
        buf[idx++] = c;
        if(idx == bufSize) idx = 0;      // avoid overflow
    } else {
        buf[idx] = 0;       // null terminate the string
        processBuffer();
        idx = 0;
    }
}

void Laser::setDistaceCallback(void (*callback)(float, float))
{
    distanceCallback = callback;
}

void Laser::setDebugCallback(void (*callback)(char*))
{
    debugCallback = callback;
}


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



//float Laser::getDistance()
//{
//    float distance = -4.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' && i < bufSize && count++ < 5000);  // timeout after about 500ms and ensure no overflow
//
//        if (count >= 5000 || i >= bufSize) return -2.0;  // timeout or overflow
//
//        // 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 = -3.0;
//        } else {
//            distance = atoi(v[2]) / 1000000.0; // distance in m
//        }
//    }
//
//    return distance;
//}