File content as of revision 13:a051da83a849:

#include "laser.h"

#define LASER_BAUD_RATE 115200

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

void Laser::discardResponse()
{
   // char c = 0;
    wait_ms(20);        // wait for the response
    while(serial.readable()) {
       serial.getc();
    }
}

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

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;
}

bool Laser::processResponse()
{
    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);
    }

    return true;
}

void Laser::enableMeasurement(bool enable)
{
    if (enable) {
        sendCommand(";\n");
        discardResponse();

    } else {
        sendCommand("switchMeasOff\n");
        discardResponse();
    }
}

void Laser::setRedDot(bool on)
{
    if(on) {
        char cmd[] = "0 -1 5 doMeasDistExt\n";     // doing a measurement turns the redDot on
        sendCommand(cmd);
        wait_ms(120);       // take a while for a response for to measurement command
        discardResponse();
 //       triggerDistanceMeasurement();
    } else {
        sendCommand("switchMeasOff\n");
        discardResponse();
    }
}

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::setDistaceCallback(void (*callback)(float, float))
{
    distanceCallback = callback;
}

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

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




//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;
//    }
//}

//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;
//}

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