File content as of revision 16:dc9956bac1a3:

#include "laser.h"

#define LASER_BAUD_RATE 115200

extern DigitalOut disableLRF;
extern DigitalOut nReset;

uint32_t base = 0x40002000;
uint32_t rxOffset = 0x514;
uint32_t txOffset = 0x50c;

uint32_t rx = base + rxOffset;
uint32_t tx = base + txOffset;

uint32_t* prx = (uint32_t*)rx;
uint32_t* ptx = (uint32_t*)tx;

int bufIdx = 0;
bool discard = true;

Laser::Laser(Serial& serial, int n) : timerRunning(false), idx(0), serial(serial), powerOffState (true), busy(false), nSamples(n)
{
   // serial.attach(this, &Laser::processResponse);  // this causes a crash?? do the assignment from the caller
}

void Laser::discardResponse()
{
    discard = true;
}

void Laser::triggerDistanceMeasurement()
{
    if(!busy){
        const int bufSize = 30;
        char cmd[bufSize];
        snprintf(&cmd[0], bufSize, "0 -1 %d doMeasDistExt\n", nSamples);
        //char cmd[] = "0 -1 10 doMeasDistExt\n";   // single reading averaged over 10 measurements
        //char cmd[] = "0 -1 100 doMeasDistExt\n";   // single reading averaged over 100 measurements
        //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
    
        busy = true;
        discard = false;
        sendCommand(cmd);
    }
}

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

void Laser::processRx()
{
    char c = serial.getc();

    if(!discard) {
        if(timerRunning) {
            timer.stop();
            timerRunning = false;
        }

        buf[bufIdx++] =c;

        // TODO sort our timeout
        if(bufIdx == bufSize) {
            // need to reset the LRF module before talking to it again, otherwise it may lock up
            nReset = 0;
            wait_ms(100);
            nReset = 1;
            wait_ms(1000);
            enableMeasurement(true);
            busy = false;
            bufIdx = 0;
        }

        if(c == '\n') {
            buf[bufIdx-1] = 0;        // replace '\n' with string null ternimator
            processResponse();
            discard = false;
             busy = false;
            bufIdx = 0;
        }
    }
}

void Laser::processResponse()
{
    if(!busy) return;

    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");
        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
        char cmd[] = "0 3 0 -1 3 doLaserCmd;\n";
        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;
}

void Laser::turnLaserPowerOn()
{
    if(powerOffState == true) {
        powerOffState = false;

        connectPower();

        enableMeasurement(true);
        setRedDot(1);
    }
}

void Laser::turnLaserPowerOff()
{
    if(powerOffState == false) {
        powerOffState = true;
        setRedDot(0);   // this disables measurements

        removePower();
    }
}

// when connceting power to the laser module ensure that tx and rx pins are
// reassigned to the serial port
void Laser::connectPower()
{
    disableLRF = 0;
    nReset = 1;
    *ptx = 27;  // p27 for tx
    *prx = 26;  // p26 for rx
    wait_ms(1000);
}

// when removing power from the laser module ensure that rx and tx pins are not
// driving voltage into the laser module to avoid hardware damage
void Laser::removePower()
{
    *ptx = 0xffffffff;      // no pin for tx
    *prx = 0xffffffff;      // no pin for rx
    DigitalOut rx(p26);
    DigitalOut tx(p27);
    rx = 0;
    tx = 0;
    nReset = 0;
    disableLRF = 1;
}

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