File content as of revision 6:ff3c66f7372b:

/* University of York Robotics Laboratory PsiSwarm Library: Serial Control Source File
 *
 * File: serial.cpp
 *
 * (C) Dept. Electronics & Computer Science, University of York
 * James Hilder, Alan Millard, Homero Elizondo, Jon Timmis
 *
 * PsiSwarm Library Version: 0.3
 *
 * October 2015
 *
 */

#include "psiswarm.h"

static float command_timeout_period = 0.1f;     //If a complete command message is not received in 0.1s then consider it a user message
char pc_command_message_started = 0;
char pc_command_message_byte = 0;
char pc_command_message[3];
char bt_command_message_started = 0;
char bt_command_message_byte = 0;
char bt_command_message[3];



char allow_commands = 1;
char allow_requests = 1;

Timeout pc_command_timeout;
Timeout bt_command_timeout;



// A predefined message structure for command messages is as follows:
// [Byte 0][Byte 1][Byte 2][Byte 3][Byte 4]
// Byte 0 and Byte 4 must be equal to COMMAND_MESSAGE_BYTE [in psiswarm.h] or message is treated as a user message


void handle_user_serial_message(char * message, char length, char interface)
{
    //This is where user code for handling a (non-system) serial message should go
    //By default, nothing is done
    //
    //message = pointer to message char array
    //length = length of message
    //interface = 0 for PC serial connection, 1 for Bluetooth
}

void IF_handle_user_serial_message(char * message, char length, char interface)
{
    char buffer[255];
    sprintf(buffer,message,length);
    for(int i=0; i<length; i++) {
        buffer[i]=message[i];
    }
    buffer[length]=0;
    if(interface) debug("Received BT message:%s [%d chars]\n",buffer,length);
    else debug("Received USB message:%s [%d chars]\n",buffer,length);
    handle_user_serial_message(message,length,interface);
}


void IF_handle_command_serial_message(char message[3], char interface)
{
    char iface [4];
    if(interface) strcpy(iface,"BT");
    else strcpy(iface,"USB");
    char command [26];
    char subcommand[30];
    float dec;
    float l_dec;
    float r_dec;
    int irp_delay;
    char colour_string[7];
    char ret_message[50];
    char send_message = 0;
    char command_status = 0;
    // command_status values:
    // 0 - unrecognised command
    // 1 - command actioned
    // 2 - command blocked
    // 3 - invalid parameters

    subcommand[0]=0;
    command[0]=0;
    switch(message[0]) {

            // MOTOR COMMANDS

        case 1:
            strcpy(command,"SET LEFT MOTOR");
            dec = IF_decode_float(message[1],message[2]);
            sprintf(subcommand,"%1.5f",dec);
            if(allow_commands) {
                command_status = 1;
                set_left_motor_speed(dec);
            } else command_status = 2;
            break;
        case 2:
            strcpy(command,"SET RIGHT MOTOR");
            dec = IF_decode_float(message[1],message[2]);
            sprintf(subcommand,"%1.5f",dec);
            if(allow_commands) {
                set_right_motor_speed(dec);
                command_status = 1;
            } else command_status = 2;
            break;
        case 3:
            strcpy(command,"SET BOTH MOTORS");
            dec = IF_decode_float(message[1],message[2]);
            sprintf(subcommand,"%1.5f",dec);
            if(allow_commands) {
                command_status = 1;
                forward(dec);
            } else command_status = 2;
            break;
        case 4:
            strcpy(command,"BRAKE LEFT MOTOR");
            sprintf(subcommand,"");
            if(allow_commands) {
                command_status = 1;
                brake_left_motor();
            } else command_status = 2;
            break;
        case 5:
            strcpy(command,"BRAKE RIGHT MOTOR");
            sprintf(subcommand,"");
            if(allow_commands) {
                command_status = 1;
                brake_right_motor();
            } else command_status = 2;
            break;
        case 6:
            strcpy(command,"BRAKE BOTH MOTORS");
            sprintf(subcommand,"");
            if(allow_commands) {
                command_status = 1;
                brake();
            } else command_status = 2;
            break;
        case 7:
            strcpy(command,"STOP BOTH MOTORS");
            sprintf(subcommand,"");
            if(allow_commands) {
                command_status = 1;
                stop();
            } else command_status = 2;
            break;
        case 8:
            strcpy(command,"TURN ON SPOT");
            dec = IF_decode_float(message[1],message[2]);
            sprintf(subcommand,"%1.5f",dec);
            if(allow_commands) {
                command_status = 1;
                turn(dec);
            } else command_status = 2;
            break;
        case 9:
            strcpy(command,"SET EACH MOTOR");
            l_dec = IF_decode_float(message[1]);
            r_dec = IF_decode_float(message[2]);
            sprintf(subcommand,"L=%1.3f R=%1.3f",l_dec,r_dec);
            if(allow_commands) {
                command_status = 1;
                
                set_left_motor_speed(l_dec);
                set_right_motor_speed(r_dec);
            } else command_status = 2;
            break;
            // LED COMMANDS

        case 10:
            strcpy(command,"SET LED STATES");
            sprintf(subcommand,"G:%s R:%s",IF_char_to_binary_char(message[1]), IF_char_to_binary_char(message[2]));
            if(allow_commands) {
                command_status = 1;
                set_leds(message[1],message[2]);
            } else command_status = 2;
            break;
        case 11:
            strcpy(command,"SET RED LED STATES");
            sprintf(subcommand,"%s",IF_char_to_binary_char(message[1]));
            if(allow_commands) {
                command_status = 1;
                set_red_leds(message[1]);
            } else command_status = 2;
            break;
        case 12:
            strcpy(command,"SET GREEN LED STATES");
            sprintf(subcommand,"%s",IF_char_to_binary_char(message[1]));
            if(allow_commands) {
                command_status = 1;
                set_green_leds(message[1]);
            } else command_status = 2;
            break;
        case 13:
            strcpy(command,"SET LED");
            switch(message[2]) {
                case 1:
                    strcpy(colour_string,"RED");
                    break;
                case 2:
                    strcpy(colour_string,"GREEN");
                    break;
                case 3:
                    strcpy(colour_string,"BOTH");
                    break;
                case 0:
                    strcpy(colour_string,"OFF");
                    break;
            }
            if(message[1] < 8 && message[2] < 4) {
                sprintf(subcommand,"%d %s",message[1],colour_string);
                if(allow_commands) {
                    command_status = 1;
                    set_led(message[1],message[2]);
                } else command_status = 2;
            } else {
                sprintf(subcommand,"[INVALID CODE]");
                command_status = 3;
            }
            break;
        case 14:
            strcpy(command,"SET CENTER LED STATE");
            switch(message[1]) {
                case 1:
                    strcpy(colour_string,"RED");
                    break;
                case 2:
                    strcpy(colour_string,"GREEN");
                    break;
                case 3:
                    strcpy(colour_string,"BOTH");
                    break;
                case 0:
                    strcpy(colour_string,"OFF");
                    break;
            }
            if(message[1] < 4) {
                sprintf(subcommand,"%s",colour_string);
                if(allow_commands) {
                    command_status = 1;
                    set_center_led(message[1]);
                } else command_status = 2;
            } else {
                sprintf(subcommand,"[INVALID CODE]");
                command_status = 3;
            }
            break;
        case 15:
            strcpy(command,"SET C.LED BRIGHTNESS");
            dec = IF_decode_unsigned_float(message[1],message[2]);
            sprintf(subcommand,"%1.5f",dec);
            if(allow_commands) {
                command_status = 1;
                set_center_led_brightness(dec);
            } else command_status = 2;
            break;
        case 16:
            strcpy(command,"SET MBED LEDS");
            sprintf(subcommand,"%s",IF_nibble_to_binary_char(message[1]));
            if(allow_commands) {
                command_status = 1;
                mbed_led1 = (message[1] & 128) >> 7;
                mbed_led2 = (message[1] & 64) >> 6;
                mbed_led3 = (message[1] & 32) >> 5;
                mbed_led4 = (message[1] & 16) >> 4;
            } else command_status = 2;
            break;
        case 17:
            strcpy(command,"BLINK OUTER LEDS");
            dec = IF_decode_unsigned_float(message[1],message[2]);
            sprintf(subcommand,"FOR %1.5fS",dec);
            if(allow_commands) {
                command_status = 1;
                blink_leds(dec);
            } else command_status = 2;
            break;
        case 18:
            strcpy(command,"SET BASE LED STATE");
            switch(message[1]) {
                case 1:
                    strcpy(subcommand,"ON");
                    break;
                case 0:
                    strcpy(subcommand,"OFF");
                    break;
            }
            //Function not yet implemented
            break;
        case 19:
            strcpy(command,"SET CENTER LED ");
            switch(message[1]) {
                case 1:
                    strcpy(colour_string,"RED");
                    break;
                case 2:
                    strcpy(colour_string,"GREEN");
                    break;
                case 3:
                    strcpy(colour_string,"BOTH");
                    break;
                case 0:
                    strcpy(colour_string,"OFF");
                    break;
            }
            dec = IF_decode_unsigned_float(message[2]);
            sprintf(subcommand,"%s @ %1.5f brightness",colour_string,dec);
            if(allow_commands) {
                command_status = 1;
                set_center_led(message[1],dec);
            } else command_status = 2;
            break;

            // DISPLAY COMMANDS

        case 20:
            strcpy(command,"SET DISPLAY ");
            switch(message[1]) {
                case 0:
                    strcpy(subcommand,"CLEAR");
                    if(allow_commands) {
                        command_status = 1;
                        display.clear_display();
                    } else command_status = 2;
                    break;
                case 1:
                    strcpy(subcommand,"MESSAGE 1");
                    if(allow_commands) {
                        command_status = 1;
                        display.clear_display();
                        display.home();
                        display.write_string("PC CONNECTION");
                        display.set_position(1,0);
                        display.write_string("STARTED");
                    } else command_status = 2;
                    break;
                case 2:
                    strcpy(subcommand,"MESSAGE 2");
                    if(allow_commands) {
                        command_status = 1;
                        display.clear_display();
                        display.home();
                        display.write_string("PC CONNECTION");
                        display.set_position(1,0);
                        display.write_string("TERMINATED");
                    } else command_status = 2;
                    break;
                case 3:
                    strcpy(subcommand,"MESSAGE 3");
                    if(allow_commands) {
                        command_status = 1;
                        display.clear_display();
                        display.home();
                        display.write_string("ANDROID DEVICE");
                        display.set_position(1,0);
                        display.write_string("CONNECTED");
                    } else command_status = 2;
                    break;
                case 4:
                    strcpy(subcommand,"MESSAGE 4");
                    if(allow_commands) {
                        command_status = 1;
                        display.clear_display();
                        display.home();
                        display.write_string("ANDROID DEVICE");
                        display.set_position(1,0);
                        display.write_string("DISCONNECTED");
                    } else command_status = 2;
                    break;
            }
            break;
        case 21:
            strcpy(command,"SET CURSOR ");
            if(message[1] < 2 && message[2] < 16) {
                sprintf(subcommand,"[%d,%d]",message[1],message[2]);
                if(allow_commands) {
                    display.set_position(message[1],message[2]);
                } else command_status = 2;
            } else {
                sprintf(subcommand,"[INVALID]");
                command_status = 3;
            }
            break;
        case 22:
            strcpy(command,"PRINT CHARACTERS ");
            char print_message[2];
            print_message[0]=message[1];
            print_message[1]=message[2];
            sprintf(subcommand,"[%c,%c]",message[1],message[2]);
            if(allow_commands) {
                display.write_string(print_message,2);
            } else command_status = 2;
            break;
        case 23:
            strcpy(command,"SET DISPLAY B.NESS");
            dec = IF_decode_unsigned_float(message[1],message[2]);
            sprintf(subcommand,"%1.5f",dec);
            if(allow_commands) {
                command_status = 1;
                display.set_backlight_brightness(dec);
            } else command_status = 2;
            break;

        case 30:
            strcpy(command,"SET DEBUG MODE");
            switch(message[1]) {
                case 1:
                    strcpy(subcommand,"ON");
                    break;
                case 0:
                    strcpy(subcommand,"OFF");
                    break;
            }
            if(message[2] & 1) strcat (subcommand,"-PC");
            if(message[2] & 2) strcat (subcommand,"-BT");
            if(message[2] & 4) strcat (subcommand,"-DISP");
            if(allow_commands) {
                command_status = 1;
                debug_mode = message[1];
                debug_output = message[2];
            } else command_status = 2;
            break;
        case 31:
            strcpy(command,"SET DEMO MODE");
            switch(message[1] % 2) {
                case 1:
                    strcpy(subcommand,"ON");
                    break;
                case 0:
                    strcpy(subcommand,"OFF");
                    break;
            }
            if(allow_commands) {
                command_status = 1;
                demo_on = message[1] % 2;
                if(demo_on == 1) {
                    user_code_restore_mode = user_code_running;
                    user_code_running = 0;
                } else {
                    user_code_running = user_code_restore_mode;
                }
            } else command_status = 2;
            break;
        case 32:
            strcpy(command,"SET USER CODE");
            switch(message[1] % 2) {
                case 1:
                    strcpy(subcommand,"ON");
                    break;
                case 0:
                    strcpy(subcommand,"OFF");
                    break;
            }
            if(allow_commands) {
                command_status = 1;
                user_code_running = message[1] % 2;
            } else command_status = 2;
            break;
        case 33:
            strcpy(command,"PAUSE USER CODE");
            dec = IF_decode_unsigned_float(message[1],message[2]) * 10;
            sprintf(subcommand,"FOR %2.3fS",dec);
            if(allow_commands) {
                command_status = 1;
                pause_user_code(dec);
            } else command_status = 2;
            break;

        case 34:
            strcpy(command,"RESET ENCODERS");
            if(allow_commands) {
                command_status = 1;
                reset_encoders();
            } else command_status = 2;
            break;

        case 35:
            strcpy(command,"SET ALLOW COMMANDS");
            switch(message[1] % 2) {
                case 1:
                    strcpy(subcommand,"ON");
                    break;
                case 0:
                    strcpy(subcommand,"OFF");
                    break;
            }
            allow_commands = message[1] % 2;
            command_status = 1;
            break;

        case 36:
            irp_delay = (message[1] << 8) + message[2];
            sprintf(command,"SET IR PULSE DELAY %d MS",irp_delay);
            if(allow_commands) {
                command_status = 1;
                ir_pulse_delay = irp_delay;
            } else command_status = 2;
            break;
        case 37:
            irp_delay = (message[1] << 8) + message[2];
            sprintf(command,"SET BASE IR PULSE DELAY %d MS",irp_delay);
            if(allow_commands) {
                command_status = 1;
                base_ir_pulse_delay = irp_delay;
            } else command_status = 2;
            break;

            // MOTOR REQUESTS
        case 40:
            strcpy(command,"GET LEFT MOTOR SPEED");
            sprintf(ret_message,"%1.5f",motor_left_speed);
            send_message = 1;
            break;

        case 41:
            strcpy(command,"GET RIGHT MOTOR SPEED");
            sprintf(ret_message,"%1.5f",motor_right_speed);
            send_message = 1;
            break;
        case 42:
            strcpy(command,"GET BRAKE STATES");
            sprintf(ret_message,"%d,%d",motor_left_brake,motor_right_brake);
            send_message = 1;
            break;
        case 43:
            strcpy(command,"GET MOTOR STATES");
            //sprintf(ret_message,"%d,%d",motor_left_brake,motor_right_brake);
            send_message = 1;
            break;
        case 44:
            strcpy(command,"GET ENCODERS");
            sprintf(ret_message,"%d,%d",left_encoder,right_encoder);
            send_message = 1;
            break;

            // LED REQUESTS
        case 50:
            strcpy(command,"GET LED STATES");
            sprintf(ret_message,"%04x",get_led_states());
            send_message = 1;
            break;

            // GENERAL REQUESTS
        case 60:
            strcpy(command,"GET SOFTWARE VERSION");
            sprintf(ret_message,"%1.2f",SOFTWARE_VERSION_CODE);
            send_message = 1;
            break;

        case 61:
            strcpy(command,"GET UPTIME");
            sprintf(ret_message,"%6.2f",get_uptime());
            send_message = 1;
            break;

        case 62:
            strcpy(command,"GET ID");
            sprintf(ret_message,"%d",robot_id);
            send_message = 1;
            break;

        case 63:
            strcpy(command,"GET SWITCH BYTE");
            sprintf(ret_message,"%02x",switch_byte);
            send_message = 1;
            break;
        case 64:
            strcpy(command,"GET USER CODE");
            sprintf(ret_message,"%d",user_code_running);
            send_message = 1;
            break;
        case 65:
            strcpy(command,"GET RESPONSE STRING");
            sprintf(ret_message,"PSI");
            send_message = 1;
            break;
        case 66:
            strcpy(command,"GET PROGRAM NAME");
            sprintf(ret_message,"%s",program_name);
            send_message = 1;
            break;
        case 67:
            strcpy(command,"GET AUTHOR NAME");
            sprintf(ret_message,"%s",author_name);
            send_message = 1;
            break;
        case 68:
            strcpy(command,"GET DEBUG MODE");
            sprintf(ret_message,"%1d%1d",debug_mode,debug_output);
            send_message = 1;
            break;
         case 69:
            strcpy(command,"GET SYSTEM WARNINGS");
            sprintf(ret_message,"%d",system_warnings);
            send_message = 1;
            break;
    

        // Sensors
        case 80:
            strcpy(command,"STORE BG. IR VALUES");
            if(allow_commands) {
                command_status = 1;
                store_background_raw_ir_values();
            } else command_status = 2;
            break;
        case 81:
            strcpy(command,"STORE IL. IR VALUES");
            if(allow_commands) {
                command_status = 1;
                store_illuminated_raw_ir_values();
            } else command_status = 2;
            break;   
        case 82:
            strcpy(command,"STORE IR VALUES");
            if(allow_commands) {
                command_status = 1;
                store_ir_values();
            } else command_status = 2;
            break;  
        case 83:
            strcpy(command,"STORE BG BASE IR VALUES");
            if(allow_commands) {
                command_status = 1;
                store_background_base_ir_values();
            } else command_status = 2;
            break;  
        case 84:
            strcpy(command,"STORE IL. BASE IR VALUES");
            if(allow_commands) {
                command_status = 1;
                store_illuminated_base_ir_values();
            } else command_status = 2;
            break;  
        case 85:
            strcpy(command,"STORE BASE IR VALUES");
            if(allow_commands) {
                command_status = 1;
                store_base_ir_values();
            } else command_status = 2;
            break; 
        case 86:
            strcpy(command,"STORE ALL IR VALUES");
            if(allow_commands) {
                command_status = 1;
                store_ir_values();
                store_base_ir_values();
            } else command_status = 2;
            break; 
        case 90:
            sprintf(command,"%s %d","GET BG IR VALUE",message[1]);
            sprintf(ret_message,"%d",get_background_raw_ir_value(message[1]));
            send_message = 1;
            break;
        case 91:
            sprintf(command,"%s %d","GET IL IR VALUE",message[1]);
            sprintf(ret_message,"%d",get_illuminated_raw_ir_value(message[1]));
            send_message = 1;
            break;
        case 92:
            strcpy(command,"GET BG IR VALUES");
            sprintf(ret_message,"%03X%03X%03X%03X%03X%03X%03X%03X",get_background_raw_ir_value(0),get_background_raw_ir_value(1),get_background_raw_ir_value(2),get_background_raw_ir_value(3),get_background_raw_ir_value(4),get_background_raw_ir_value(5),get_background_raw_ir_value(6),get_background_raw_ir_value(7));
            send_message = 1;
            break;
        case 93:
            strcpy(command,"GET ILLUMINATED IR VALUES");
            sprintf(ret_message,"%03X%03X%03X%03X%03X%03X%03X%03X",get_illuminated_raw_ir_value(0),get_illuminated_raw_ir_value(1),get_illuminated_raw_ir_value(2),get_illuminated_raw_ir_value(3),get_illuminated_raw_ir_value(4),get_illuminated_raw_ir_value(5),get_illuminated_raw_ir_value(6),get_illuminated_raw_ir_value(7));
            send_message = 1;
            break;
        case 94:
            sprintf(command,"%s %d","GET BG BASE IR VALUE",message[1]);
            sprintf(ret_message,"%d",get_background_base_ir_value(message[1]));
            send_message = 1;
            break;
        case 95:
            sprintf(command,"%s %d","GET IL BASE IR VALUE",message[1]);
            sprintf(ret_message,"%d",get_illuminated_base_ir_value(message[1]));
            send_message = 1;
            break;
        case 96:
            strcpy(command,"GET BG BASE IR VALUES");
            sprintf(ret_message,"%03X%03X%03X%03X%03X",get_background_base_ir_value(0),get_background_base_ir_value(1),get_background_base_ir_value(2),get_background_base_ir_value(3),get_background_base_ir_value(4));
            send_message = 1;
            break;
        case 97:
            strcpy(command,"GET IL BASE IR VALUES");
            sprintf(ret_message,"%03X%03X%03X%03X%03X",get_illuminated_base_ir_value(0),get_illuminated_base_ir_value(1),get_illuminated_base_ir_value(2),get_illuminated_base_ir_value(3),get_illuminated_base_ir_value(4));
            send_message = 1;
            break;        
    }


    if(send_message) {
        char message_length = strlen(ret_message);
        switch(interface) {
            case 0:
                pc.printf("%c%c%s",RESPONSE_MESSAGE_BYTE,message_length,ret_message);
                break;
            case 1:
                bt.printf("%c%c%s",RESPONSE_MESSAGE_BYTE,message_length,ret_message);
                break;
        }
        debug("Received %s request message: %s %s [%02x%02x%02x]\nReply: %s [%d ch]\n",iface, command, subcommand,message[0],message[1],message[2],ret_message,message_length);
    } else {
        switch(interface) {
            case 0:
                pc.printf("%c%c",ACKNOWLEDGE_MESSAGE_BYTE,command_status);
                break;
            case 1:
                bt.printf("%c%c",ACKNOWLEDGE_MESSAGE_BYTE,command_status);
                break;
        }
        switch(command_status) {
            case 0:
                debug("Unrecognised %s command message [%02x%02x%02x]\n",iface,message[0],message[1],message[2]);
                break;
            case 1:
                debug("Actioned %s command message:%s %s [%02x%02x%02x]\n",iface, command, subcommand,message[0],message[1],message[2]);
                break;
            case 2:
                debug("Blocked %s command message:%s %s [%02x%02x%02x]\n",iface, command, subcommand,message[0],message[1],message[2]);
                break;
            case 3:
                debug("Invalid %s command message:%s %s [%02x%02x%02x]\n",iface, command, subcommand,message[0],message[1],message[2]);
                break;
        }
    }
}

char * IF_nibble_to_binary_char(char in)
{
    char * ret = (char*)malloc(sizeof(char)*5);
    for(int i=0; i<4; i++) {
        if(in & (128 >> i)) ret[i]='1';
        else ret[i]='0';
    }
    ret[4]=0;
    return ret;
}

char * IF_char_to_binary_char(char in)
{
    char * ret = (char*)malloc(sizeof(char)*9);
    for(int i=0; i<8; i++) {
        if(in & (128 >> i)) ret[i]='1';
        else ret[i]='0';
    }
    ret[8]=0;
    return ret;
}

float IF_decode_unsigned_float(char byte0, char byte1)
{
    unsigned short sval = (byte0) << 8;
    sval += byte1;
    float scaled = sval / 65535.0f;
    return scaled;
}

float IF_decode_float(char byte0, char byte1)
{
    // MSB is byte 0 is sign, rest is linear spread between 0 and 1
    char sign = byte0 / 128;
    short sval = (byte0 % 128) << 8;
    sval += byte1;
    float scaled = sval / 32767.0f;
    if(sign == 0) scaled = 0-scaled;
    return scaled;
}

float IF_decode_unsigned_float(char byte0)
{
    unsigned short sval = (byte0);
    float scaled = sval / 255.0f;
    return scaled;
}

float IF_decode_float(char byte0)
{
    // MSB is byte 0 is sign, rest is linear spread between 0 and 1
    char sign = byte0 / 128;
    short sval = (byte0 % 128);
    float scaled = sval / 127.0f;
    if(sign == 0) scaled = 0-scaled;
    return scaled;
}

void IF_setup_serial_interfaces()
{
    if(ENABLE_PC_SERIAL) {
        pc.baud(PC_BAUD);
        pc.attach(&IF_pc_rx_callback, Serial::RxIrq);
    }
    if(ENABLE_BLUETOOTH) {
        bt.baud(BLUETOOTH_BAUD);
        bt.attach(&IF_bt_rx_callback, Serial::RxIrq);
    }
}

void IF_pc_rx_command_timeout()
{
    char message_array[6];
    char length = 1 + pc_command_message_byte;
    pc_command_message_started = 0;
    message_array[0] = COMMAND_MESSAGE_BYTE;
    for(int k=0; k<pc_command_message_byte; k++) {
        message_array[k+1] = pc_command_message[k];
    }
    IF_handle_user_serial_message(message_array, length, 0);
}

void IF_bt_rx_command_timeout()
{
    char message_array[6];
    char length = 1 + bt_command_message_byte;
    bt_command_message_started = 0;
    message_array[0] = COMMAND_MESSAGE_BYTE;
    for(int k=0; k<bt_command_message_byte; k++) {
        message_array[k+1] = bt_command_message[k];
    }
    IF_handle_user_serial_message(message_array, length, 1);
}

void IF_pc_rx_callback()
{
    int count = 0;
    char message_array[255];

    while(pc.readable()) {
        char tc = pc.getc();
        message_array[count] = tc;
        count ++;
        if(pc_command_message_started == 1) {
            if(pc_command_message_byte == 3) {
                pc_command_timeout.detach();
                if(tc == COMMAND_MESSAGE_BYTE) {
                    // A complete command message succesfully received, call handler
                    pc_command_message_started = 0;
                    count = 0;
                    IF_handle_command_serial_message(pc_command_message , 0);
                } else {
                    // Message is not a valid command message as 5th byte is not correct; treat whole message as a user message
                    pc_command_message_started = 0;
                    message_array[0] = COMMAND_MESSAGE_BYTE;
                    message_array[1] = pc_command_message[0];
                    message_array[2] = pc_command_message[1];
                    message_array[3] = pc_command_message[2];
                    message_array[4] = tc;
                    count = 5;
                }
            } else {
                pc_command_message[pc_command_message_byte] = tc;
                pc_command_message_byte ++;
            }
        } else {
            if(count == 1) {
                if(tc == COMMAND_MESSAGE_BYTE) {
                    pc_command_timeout.attach(&IF_pc_rx_command_timeout,command_timeout_period);
                    pc_command_message_started = 1;
                    pc_command_message_byte = 0;

                }
            }
        }
    }
    if(!pc_command_message_started && count>0) IF_handle_user_serial_message(message_array, count, 0);
}

void IF_bt_rx_callback()
{
    int count = 0;
    char message_array[255];

    while(bt.readable()) {
        char tc = bt.getc();
        message_array[count] = tc;
        count ++;
        if(bt_command_message_started == 1) {
            if(bt_command_message_byte == 3) {
                bt_command_timeout.detach();
                if(tc == COMMAND_MESSAGE_BYTE) {
                    // A complete command message succesfully received, call handler
                    bt_command_message_started = 0;
                    count = 0;
                    IF_handle_command_serial_message(bt_command_message , 1);
                } else {
                    // Message is not a valid command message as 5th byte is not correct; treat whole message as a user message
                    bt_command_message_started = 0;
                    message_array[0] = COMMAND_MESSAGE_BYTE;
                    message_array[1] = bt_command_message[0];
                    message_array[2] = bt_command_message[1];
                    message_array[3] = bt_command_message[2];
                    message_array[4] = tc;
                    count = 5;
                }
            } else {
                bt_command_timeout.attach(&IF_bt_rx_command_timeout,command_timeout_period);
                bt_command_message[bt_command_message_byte] = tc;
                bt_command_message_byte ++;
            }
        } else {
            if(count == 1) {
                if(tc == COMMAND_MESSAGE_BYTE) {
                    bt_command_message_started = 1;
                    bt_command_message_byte = 0;

                }
            }
        }
    }
    if(!bt_command_message_started && count>0) IF_handle_user_serial_message(message_array, count, 1);
}