File content as of revision 0:b5c7dc5f1fc8:

#include "LIDAR.h"
#include "mbed.h"

LIDAR::LIDAR(PinName tx, PinName rx, PinName motor_pwm) :
    _lidar(tx, rx), _motor(motor_pwm)
{
    _lidar.baud(115200);
    _motor.period_us(100);
    _motor.write(0.0);
    
    _data_ptr = (char *) _data;
    _speed_ptr = (char *) &_speed;
    
    _fsm_state = 0;
    _fsm_count = 0;
    
    _speed = 0;
    for(int i = 0; i < 360; i++)
        _data[i] = 0;
}

void LIDAR::StartData(void)
{
    _lidar.attach(this, &LIDAR::data_parser, Serial::RxIrq);
}

void LIDAR::StopData(void)
{
    _lidar.attach(NULL, Serial::RxIrq);
}

void LIDAR::SetPWMDuty(float duty)
{
    _motor.write(duty);
}

void LIDAR::SetPWMPeriodUs(int microseconds)
{
    _motor.period_us(microseconds);
}

float LIDAR::GetData(int degree)
{
    if(degree < 360 && degree >= 0)
        return _data[degree]/10.0;
    else
        return -1.0;
}

float LIDAR::GetSpeed(void)
{
    return _speed/64.0;
}

void LIDAR::data_parser(void)
{
    char buffer;
    
    // Insert data to temporary buffer
    buffer = _lidar.getc();

    // State machine for data extraction
    switch(_fsm_state)
    {
        case 0:
        // If start byte found, move to next state
        if(buffer == 0xFA)
        {
            _fsm_count = 0;
            _fsm_state++;
        }
        break;
        
        case 1:
        // Determine the packet number and check packet validity
        _fsm_angle = (buffer - 0xA0) << 3;
        if(_fsm_angle <= 712)
            _fsm_state++;
        else
            _fsm_state = 0;
        break;
        
        case 2:
        // Add the LSB of RPM
        _speed_ptr[0] = buffer;
        _fsm_state++;
        break;
        
        case 3:
        // Add the MSB of RPM
        _speed_ptr[1] = buffer;
        _fsm_state++;
        break;
        
        case 4:
        // Add the LSB of distance
        _data_ptr[718 - _fsm_angle] = buffer;
        _fsm_state++;
        break;
        
        case 5:
        // Add the MSB of distance and check packet validity
        if(buffer & 0x80)
        {
            // Invalid packet is marked by -1
            _data[359 - (_fsm_angle >> 1)] = -1;
        }
        else
        {
            _data_ptr[719 - _fsm_angle] = buffer & 0x3F;
        }
        _fsm_state++;
        break;
        
        case 6:
        // Increment packet counter and angle
        _fsm_count++;
        _fsm_angle += 2;
        _fsm_state++;
        break;
        
        case 7:
        // Check number of data accquired
        // 1 packet should contains 4 data
        if(_fsm_count < 4) _fsm_state = 4;
        else
        {
            _fsm_state = 0;
        }
        break;
        
        default:
        _fsm_state = 0;
        
        }
}