File content as of revision 0:1b682aa7d935:

#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_detection_timer.attach(this, &LambdaController::error_detection_isr,
			0.03);
}

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

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

		if (data == 0x3A) {
			buffer_head = 0;
		}
		
		if(data == EOF){
			buffer_head = -1;
			error_id = InvalidDataError;
		}	

		if (buffer_head == 10 && received_buffer[0] == 0x3A) {
			for (i = 0; i < 10; i++) {
				received_data[i] = received_buffer[i];
			}

			if (!is_axis_initialized) {
				initialize_axis();
			}

			error_count = 0;
			error_id = NoError;
		} else {
			buffer_head = -1;
			error_id = InvalidDataError;
		}
	}
}

void LambdaController::error_detection_isr() {
	if (error_count < error_threshold) {
		error_count++;
	} else {
		error_id = TimeoutError;
	}
}

uint8_t LambdaController::get_all_switch() {
	return received_data[1];
}

bool LambdaController::get_switch(int num) {
	bool status = false;
	if (num < 8 && 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) {
	int *data;
	int value;

	data = &received_data[(int) id * 2 + 2];
	value = (data[0] << 8) | (data[1]);

	return value;
}

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

	value = (double) (get_raw_axis(id) - 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;
	}

	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) {
	axis_center[(int) id] = get_raw_axis(id);
}

LambdaController::ErrorFactor LambdaController::get_error() {
	return error_id;
}

void GetCon::debug(const char *format, ...)
{
    va_list arg;

    va_start(arg, format);
    device.vprintf(format, arg);
    va_end(arg);
}