File content as of revision 2:e27733eaa594:

/*
 * RoboPeak RPLIDAR Driver for Arduino
 * RoboPeak.com
 *
 * Copyright (c) 2014, RoboPeak
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without modification,
 * are permitted provided that the following conditions are met:
 *
 * 1. Redistributions of source code must retain the above copyright notice,
 *    this list of conditions and the following disclaimer.
 *
 * 2. Redistributions in binary form must reproduce the above copyright notice,
 *    this list of conditions and the following disclaimer in the documentation
 *    and/or other materials provided with the distribution.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY
 * EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES
 * OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT
 * SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
 * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT
 * OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
 * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR
 * TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE,
 * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 *
 */

#include "rplidar.h"
//BufferedSerial _bined_serialdev( PA_11,  PA_12); // tx, rx
Timer timers;

/*
    Thread that handles reading the sensor measurements
    
    This thread parses the buffered serial bytes into Lidar measurements.
    If there's space in the mail box it adds a measurement to it. In case there 
    isn't space, it takes 1 element out of the mail box and then puts the new 
    measurement. This makes sure recent data is in the mailbox instead of really 
    old data. Messages are lost anyway but this way the application will read 
    measurements of the current robot position.
    if the mail box still has 100 max messages then this means that the oldest
    measurement was 50ms in a full mail box. 
    Note that since the thread runs every 20ms then there will be measurements 
    as old as 20ms.
*/
void RPLidar::Thread_readSensor(void)
{

    //pc->printf("starting thread\n");
    while(1)
    {   
    
        mutex_measurements.lock();
        while (IS_OK(pollPoint()))
        {    
            
            currentMeasurement_fromThread = getCurrentPoint();
            newMeasurement = 1;
               
          
            /*if(!mail_box.full())
            {            
                struct RPLidarMeasurement current = getCurrentPoint();
                struct RPLidarMeasurement *mail = mail_box.alloc();
                if(mail == NULL)
                {   
                    //there was an error allocating space in heap
                    continue;
                }
                mail->distance = current.distance;
                mail->angle = current.angle;
                mail->quality = current.quality;
                mail->startBit = current.startBit;
                
                mail_box.put(mail);
                
            }
            else
            {
                osEvent evt = mail_box.get();
                if (evt.status == osEventMail) {
                    struct RPLidarMeasurement *mail = (struct RPLidarMeasurement*)evt.value.p;
                    mail_box.free(mail);
                    
                    struct RPLidarMeasurement current = getCurrentPoint();
                    mail = mail_box.alloc();
                    if(mail == NULL)
                    {   
                        //there was an error allocating space in heap
                        continue;
                    }
                    mail->distance = current.distance;
                    mail->angle = current.angle;
                    mail->quality = current.quality;
                    mail->startBit = current.startBit;
                    
                    mail_box.put(mail);
                }
                

            }*/
        }
        mutex_measurements.unlock();    
        wait_us(2000);
    }   

    
}

/*
  Poll for new measurements in the mail box
  Returns: 0 if found data, -1 if not data available.
*/
int32_t RPLidar::pollSensorData(struct RPLidarMeasurement *_data)
{
    
    mutex_measurements.lock();
    if(newMeasurement)
    {
        //currentMeasurement_fromThread = getCurrentPoint();
        _data->distance = currentMeasurement_fromThread.distance;
        _data->angle = currentMeasurement_fromThread.angle;
        _data->quality = currentMeasurement_fromThread.quality;
        _data->startBit = currentMeasurement_fromThread.startBit;
        newMeasurement = 0;
        mutex_measurements.unlock();    
        return 0;
    }
    mutex_measurements.unlock();    
    
    /*osEvent evt = mail_box.get();
    if (evt.status == osEventMail) {
        
        struct RPLidarMeasurement *mail = (struct RPLidarMeasurement*)evt.value.p;
        //struct RPLidarMeasurement data;
        _data->distance = mail->distance;
        _data->angle = mail->angle;
        _data->quality = mail->quality;
        _data->startBit = mail->startBit;
        mail_box.free(mail);
        return 0;

        

    }*/
    
    return -1;
}


RPLidar::RPLidar()
{
    

    _currentMeasurement.distance = 0;
    _currentMeasurement.angle = 0;
    _currentMeasurement.quality = 0;
    _currentMeasurement.startBit = 0;
    
    useThread = 0;
    thread.set_priority(osPriorityAboveNormal);
    //Thread thread(osPriorityAboveNormal, OS_STACK_SIZE, NULL, "RPLidar");
    //thread_p = &thread;
}


RPLidar::~RPLidar()
{
    end();
}

// open the given serial interface and try to connect to the RPLIDAR
/*
bool RPLidar::begin(BufferedSerial &serialobj)
{

    //Serial.begin(115200);

    if (isOpen()) {
      end();
    }
    _bined_serialdev = &serialobj;
  //  _bined_serialdev->end();
    _bined_serialdev->baud(RPLIDAR_SERIAL_BAUDRATE);
}
*/

void RPLidar::begin(BufferedSerial &serialobj)
{
    _bined_serialdev = &serialobj;
    timers.start();
    _bined_serialdev->baud(RPLIDAR_SERIAL_BAUDRATE);
    recvPos_g = 0;
}
// close the currently opened serial interface
void RPLidar::end()
{/*
    if (isOpen()) {
       _bined_serialdev->end();
       _bined_serialdev = NULL;
    }*/
}


// check whether the serial interface is opened
/*
bool RPLidar::isOpen()
{
    return _bined_serialdev?true:false;
}
*/
// ask the RPLIDAR for its health info

u_result RPLidar::getHealth(rplidar_response_device_health_t & healthinfo, _u32 timeout)
{
    _u32 currentTs = timers.read_ms();
    _u32 remainingtime;

    _u8 *infobuf = (_u8 *)&healthinfo;
    _u8 recvPos = 0;

    rplidar_ans_header_t response_header;
    u_result  ans;


  //  if (!isOpen()) return RESULT_OPERATION_FAIL;

    {
        if (IS_FAIL(ans = _sendCommand(RPLIDAR_CMD_GET_DEVICE_HEALTH, NULL, 0))) {
            return ans;
        }

        if (IS_FAIL(ans = _waitResponseHeader(&response_header, timeout))) {
            return ans;
        }

        // verify whether we got a correct header
        if (response_header.type != RPLIDAR_ANS_TYPE_DEVHEALTH) {
            return RESULT_INVALID_DATA;
        }

        if ((response_header.size) < sizeof(rplidar_response_device_health_t)) {
            return RESULT_INVALID_DATA;
        }

        while ((remainingtime=timers.read_ms() - currentTs) <= timeout) {
            int currentbyte = _bined_serialdev->getc();
            if (currentbyte < 0) continue;

            infobuf[recvPos++] = currentbyte;

            if (recvPos == sizeof(rplidar_response_device_health_t)) {
                return RESULT_OK;
            }
        }
    }
    return RESULT_OPERATION_TIMEOUT;
}
// ask the RPLIDAR for its device info like the serial number
u_result RPLidar::getDeviceInfo(rplidar_response_device_info_t & info, _u32 timeout )
{
    _u8  recvPos = 0;
    _u32 currentTs = timers.read_ms();
    _u32 remainingtime;
    _u8 *infobuf = (_u8*)&info;
    rplidar_ans_header_t response_header;
    u_result  ans;

  //  if (!isOpen()) return RESULT_OPERATION_FAIL;

    {
        if (IS_FAIL(ans = _sendCommand(RPLIDAR_CMD_GET_DEVICE_INFO,NULL,0))) {
            return ans;
        }

        if (IS_FAIL(ans = _waitResponseHeader(&response_header, timeout))) {
            return ans;
        }

        // verify whether we got a correct header
        if (response_header.type != RPLIDAR_ANS_TYPE_DEVINFO) {
            return RESULT_INVALID_DATA;
        }

        if (response_header.size < sizeof(rplidar_response_device_info_t)) {
            return RESULT_INVALID_DATA;
        }

        while ((remainingtime=timers.read_ms() - currentTs) <= timeout) {
            int currentbyte = _bined_serialdev->getc();
            if (currentbyte<0) continue;
            infobuf[recvPos++] = currentbyte;

            if (recvPos == sizeof(rplidar_response_device_info_t)) {
                return RESULT_OK;
            }
        }
    }

    return RESULT_OPERATION_TIMEOUT;
}

// stop the measurement operation
u_result RPLidar::stop()
{
//    if (!isOpen()) return RESULT_OPERATION_FAIL;
    u_result ans = _sendCommand(RPLIDAR_CMD_STOP,NULL,0);
    return ans;
}



u_result RPLidar::startThreadScan()
{
    startScan();
    useThread = true;
    thread.start(callback(this, &RPLidar::Thread_readSensor));
        
    return RESULT_OK;
}
// start the measurement operation
/*
 This was altered to also start the thread that parses measurements in the background
*/
u_result RPLidar::startScan(bool force, _u32 timeout)
{
    //pc->printf("RPLidar::startScan\n");
    u_result ans;

//    if (!isOpen()) return RESULT_OPERATION_FAIL;

    stop(); //force the previous operation to stop


        ans = _sendCommand(force?RPLIDAR_CMD_FORCE_SCAN:RPLIDAR_CMD_SCAN, NULL, 0);
        if (IS_FAIL(ans)) return ans;

        // waiting for confirmation
        rplidar_ans_header_t response_header;
        if (IS_FAIL(ans = _waitResponseHeader(&response_header, timeout))) {
            return ans;
        }

        // verify whether we got a correct header
        if (response_header.type != RPLIDAR_ANS_TYPE_MEASUREMENT) {
            return RESULT_INVALID_DATA;
        }

        if (response_header.size < sizeof(rplidar_response_measurement_node_t)) {
            return RESULT_INVALID_DATA;
        }
        

    return RESULT_OK;
}

// wait for one sample point to arrive
/*
    Do not use if startScan was called with starThread == 1!
*/
u_result RPLidar::waitPoint(_u32 timeout)
{
   
   if(useThread)
        return RESULT_OPERATION_NOT_SUPPORT;
        
   _u32 currentTs = timers.read_ms();
   _u32 remainingtime;
   rplidar_response_measurement_node_t node;
   _u8 *nodebuf = (_u8*)&node;

   _u8 recvPos = 0;

   while ((remainingtime = timers.read_ms() - currentTs) <= timeout) {
        int currentbyte = _bined_serialdev->getc();
        if (currentbyte<0) continue;
//Serial.println(currentbyte);
        switch (recvPos) {
            case 0: // expect the sync bit and its reverse in this byte          {
                {
                    _u8 tmp = (currentbyte>>1);
                    if ( (tmp ^ currentbyte) & 0x1 ) {
                        // pass
                    } else {
                        continue;
                    }

                }
                break;
            case 1: // expect the highest bit to be 1
                {
                    if (currentbyte & RPLIDAR_RESP_MEASUREMENT_CHECKBIT) {
                        // pass
                    } else {
                        recvPos = 0;
                        continue;
                    }
                }
                break;
          }
          nodebuf[recvPos++] = currentbyte;
          if (recvPos == sizeof(rplidar_response_measurement_node_t)) {
              // store the data ...
              _currentMeasurement.distance = node.distance_q2/4.0f;
              _currentMeasurement.angle = (node.angle_q6_checkbit >> RPLIDAR_RESP_MEASUREMENT_ANGLE_SHIFT)/64.0f;
              _currentMeasurement.quality = (node.sync_quality>>RPLIDAR_RESP_MEASUREMENT_QUALITY_SHIFT);
              _currentMeasurement.startBit = (node.sync_quality & RPLIDAR_RESP_MEASUREMENT_SYNCBIT);
              ang = _currentMeasurement.angle;
              dis = _currentMeasurement.distance;


              if(ang>=angMin&&ang<=angMax)Data[ang] = dis;

              return RESULT_OK;
          }


   }

   return RESULT_OPERATION_TIMEOUT;
}


/*
    This is very similar to waitPoint and it's only to be used by the thread, 
    hence why it private.
    
    It checks for data in the buffered serial until it finds a message or there
    are no more bytes. 
    If it finds a message it returns RESULT_OK
    If there are no more bytes it returns RESULT_OPERATION_TIMEOUT.
    
    The state of the parsing is saved so it can continue parsing a measurement
    midway. (Note that does not mean it's re-entrant and should only be used
    in the same context)
*/
u_result RPLidar::pollPoint()
{
   
   //_u32 currentTs = timers.read_ms();
   _u32 remainingtime;
   //rplidar_response_measurement_node_t node;
   _u8 *nodebuf = (_u8*)&(node_g);


   int currentbyte;
   while(_bined_serialdev->readable() > 0)
   {
   //while ((remainingtime = timers.read_ms() - currentTs) <= timeout) {
        currentbyte = _bined_serialdev->getc();
        //if (_bined_serialdev->readable() == 0) continue; 
        //Serial.println(currentbyte);
        switch (recvPos_g) {
            case 0: // expect the sync bit and its reverse in this byte          {
                {
                    _u8 tmp = (currentbyte>>1);
                    if ( (tmp ^ currentbyte) & 0x1 ) {
                        // pass
                        
                    } else {
                        continue;
                    }

                }
                break;
            case 1: // expect the highest bit to be 1
                {
                    if (currentbyte & RPLIDAR_RESP_MEASUREMENT_CHECKBIT) {
                        // pass
                    } else {
                        recvPos_g = 0;
                        continue;
                    }
                }
                break;
          }
          nodebuf[recvPos_g++] = currentbyte;
          if (recvPos_g == sizeof(rplidar_response_measurement_node_t)) {
              recvPos_g = 0;
              // store the data ...
              _currentMeasurement.distance = node_g.distance_q2/4.0f;
              _currentMeasurement.angle = (node_g.angle_q6_checkbit >> RPLIDAR_RESP_MEASUREMENT_ANGLE_SHIFT)/64.0f;
              _currentMeasurement.quality = (node_g.sync_quality>>RPLIDAR_RESP_MEASUREMENT_QUALITY_SHIFT);
              _currentMeasurement.startBit = (node_g.sync_quality & RPLIDAR_RESP_MEASUREMENT_SYNCBIT);
              ang = _currentMeasurement.angle;
              dis = _currentMeasurement.distance;


              if(ang>=angMin&&ang<=angMax)Data[ang] = dis;

              return RESULT_OK;
          }


   }//while(_bined_serialdev->readable() > 0);

   return RESULT_OPERATION_TIMEOUT;
}


void RPLidar::setAngle(int min,int max){
  angMin = min;
  angMax = max;
}
void RPLidar::get_lidar(){
  if (!IS_OK(waitPoint())) startScan();
}
u_result RPLidar::_sendCommand(_u8 cmd, const void * payload, size_t payloadsize)
{

    rplidar_cmd_packet_t pkt_header;
    rplidar_cmd_packet_t * header = &pkt_header;
    _u8 checksum = 0;

    if (payloadsize && payload) {
        cmd |= RPLIDAR_CMDFLAG_HAS_PAYLOAD;
    }

    header->syncByte = RPLIDAR_CMD_SYNC_BYTE;
    header->cmd_flag = cmd;

    // send header first
    _bined_serialdev->putc(header->syncByte );
    _bined_serialdev->putc(header->cmd_flag );

  //  _bined_serialdev->write( (uint8_t *)header, 2);

    if (cmd & RPLIDAR_CMDFLAG_HAS_PAYLOAD) {
        checksum ^= RPLIDAR_CMD_SYNC_BYTE;
        checksum ^= cmd;
        checksum ^= (payloadsize & 0xFF);

        // calc checksum
        for (size_t pos = 0; pos < payloadsize; ++pos) {
            checksum ^= ((_u8 *)payload)[pos];
        }

        // send size
        _u8 sizebyte = payloadsize;
        _bined_serialdev->putc(sizebyte);
      //  _bined_serialdev->write((uint8_t *)&sizebyte, 1);

        // send payload
    //    _bined_serialdev.putc((uint8_t )payload);
    //    _bined_serialdev->write((uint8_t *)&payload, sizebyte);

        // send checksum
        _bined_serialdev->putc(checksum);
      //  _bined_serialdev->write((uint8_t *)&checksum, 1);

    }

    return RESULT_OK;
}

u_result RPLidar::_waitResponseHeader(rplidar_ans_header_t * header, _u32 timeout)
{
    _u8  recvPos = 0;
    _u32 currentTs = timers.read_ms();
    _u32 remainingtime;
    _u8 *headerbuf = (_u8*)header;
    while ((remainingtime = timers.read_ms() - currentTs) <= timeout) {

        int currentbyte = _bined_serialdev->getc();
        if (currentbyte<0) continue;
        switch (recvPos) {
        case 0:
            if (currentbyte != RPLIDAR_ANS_SYNC_BYTE1) {
                continue;
            }
            break;
        case 1:
            if (currentbyte != RPLIDAR_ANS_SYNC_BYTE2) {
                recvPos = 0;
                continue;
            }
            break;
        }
        headerbuf[recvPos++] = currentbyte;

        if (recvPos == sizeof(rplidar_ans_header_t)) {
            return RESULT_OK;
        }


    }

    return RESULT_OPERATION_TIMEOUT;
}