File content as of revision 2:cf08273cbd94:

#include "LambdaController.h"

LambdaController::LambdaController(PinName tx, PinName rx, PinName hzpin) :
    device(tx, rx), hzjudge(hzpin)
{   
    is_axis_initialized = false;

    buffer_head = -1;
    device.baud(19200);
    device.attach(this, &LambdaController::data_receive_isr, Serial::RxIrq);
    
    hzjudge.mode(PullUp);

    error_count = 0;
    error_id = NoDataError;
                                 
    error_timer.start();
}

void LambdaController::data_receive_isr()
{
    char data, *ptr;
    int i;

    while (device.readable())
    {
        data = device.getc();

        if (data == 0x3A)
        {
            buffer_head = 0;
        }

        if (hzjudge)
        {
            if (buffer_head < 21 && buffer_head != -1)
            {
                if (data != ',')
                {
                    if (buffer_head == 0)
                    {
                        received_buffer[buffer_head] = 0x3A;
                    }   
                    else
                    {
                        if (buffer_head % 2 == 1)
                        {
                            received_buffer[buffer_head / 2 + 1] = strtol(&data, &ptr, 16) << 4;
                        }
                        else if (buffer_head % 2 == 0)
                        {
                            received_buffer[buffer_head / 2] |= strtol(&data, &ptr, 16);
                        }
                    }
                    buffer_head++;
                }
            }
            
            if (buffer_head == 21)
            {
                if (received_buffer[0] == 0x3A)
                {
                    for (i = 0; i < 11; i++)
                    {
                        received_data[i] = received_buffer[i];
                    }
                    
                    if (!is_axis_initialized)
                    {
                        initialize_axis();
                        is_axis_initialized = true;
                    }
    
                    error_count = 0;
                    error_id = NoError;
                    error_timer.reset();
                }
                else
                {
                    buffer_head = -1;
                    error_id = InvalidDataError;
                }
            }   
        }
        else
        {
            if (buffer_head < 11 && buffer_head != -1)
            {
                received_buffer[buffer_head] = data;
                buffer_head++;
            }
            else
            {
                //error_id = InvalidDataError;
                buffer_head = -1;
            }
    
            if (buffer_head == 11)
            {
                if(received_buffer[0] == 0x3A)
                {
                    for (i = 0; i < 11; i++)
                    {
                        received_data[i] = received_buffer[i];
                    }
    
                    if (!is_axis_initialized)
                    {
                        initialize_axis();
                        is_axis_initialized = true;
                    }
    
                    error_count = 0;
                    error_id = NoError;
                    error_timer.reset();
                }
                else
                {
                    buffer_head = -1;
                    error_id = InvalidDataError;
                }
            }
        }

        if(data == EOF)
        {
            error_id = SerialBusyError;
            break;
        }
    }
}

uint16_t LambdaController::get_all_switch()
{
    return (received_data[1] << 8) | received_data[2];
}

bool LambdaController::get_switch(int num)
{
    bool status = false;
    if (num < 11 && num >= 0)
    {
        if (get_all_switch() & (1 << num))
        {
            status = true;
        }
        else
        {
            status = false;
        }
    }
    else
    {
        status = false;
    }

    return status;
}

int LambdaController::get_raw_axis(AxisId id)
{
    uint8_t *data;
    int value;

    if((int)id < 4 && (int)id >= 0)
    {
        data = &received_data[(int) id * 2 + 3];
        value = (data[0] << 8) | (data[1]);
    }
    else
    {
        value = -1;
    }

    return value;
}

double LambdaController::get_axis(AxisId id)
{
    double value = 0;
    int raw;
    raw = get_raw_axis(id);

    if(raw != -1)
    {
        value = (double) (raw - axis_center[(int) id]) / 512.0;

        if (value > -axis_threshold && value < axis_threshold)
        {
            value = 0.0;
        }
        else if (value < -1.0)
        {
            value = -1.0;
        }
        else if (value > 1.0)
        {
            value = 1;
        }
    }
    else
    {
        value = 0.0;
    }

    return value;
}

void LambdaController::initialize_axis(void)
{
    int i;
    for (i = 0; i < 4; i++)
    {
        initialize_axis((AxisId) i);
    }
}

void LambdaController::initialize_axis(AxisId id)
{
    int raw;
    raw = get_raw_axis(id);

    if(raw != -1)
    {
        axis_center[(int) id] = raw;
    }
}

LambdaController::ErrorFactor LambdaController::get_error()
{   
    if (error_timer >= timeout_time)
    {
        error_id = TimeoutError;
    }
    
    return error_id;
}