File content as of revision 6:c4e901c6001a:

#include "LambdaController.h"

LambdaController::LambdaController(PinName tx, PinName rx) :
    twe_lite(tx, rx)
{   
    is_axis_initialized = false;

    buffer_head = -1;
    twe_lite.baud(115200);
    twe_lite.attach(this, &LambdaController::data_receive_isr, Serial::RxIrq);

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

void LambdaController::data_receive_isr()
{
    int data;
    int i;

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

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

        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[2] << 8) || received_data[1];
}

bool LambdaController::get_switch(int num)
{
    bool status = false;
    if (num < 16 && 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;
}