Diff: serial.cpp

Revision:

0:d6269d17c8cf

Child:

1:060690a934a9

diff -r 000000000000 -r d6269d17c8cf serial.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/serial.cpp	Thu Feb 04 21:48:54 2016 +0000
@@ -0,0 +1,995 @@
+/* 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, Alexander Horsfield, Homero Elizondo, Jon Timmis
+ *
+ * PsiSwarm Library Version: 0.4
+ *
+ * February 2016
+ *
+ *
+ */
+
+#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
+    //
+    // message = pointer to message char array
+    // length = length of message
+    // interface = 0 for PC serial connection, 1 for Bluetooth
+
+    if(interface) {
+        if(length == 8) {
+            for(int i = 0; i < length; i++) {
+                // Convert single byte value into a beacon heading in the range +/-180 degrees
+                float beacon_heading = message[i];
+                float degrees_per_value = 256.0f / 360.0f;
+
+                if(beacon_heading != 0)
+                    beacon_heading /= degrees_per_value;
+
+                beacon_heading -= 180;
+
+                flocking_headings[i] = beacon_heading;
+
+                debug("%d, ", flocking_headings[i]);
+                //debug("%f, ", beacon_heading);
+            }
+
+            debug("\n");
+        }
+    }
+}
+
+
+void IF_handle_file_transfer_serial_message(char * message, char length, char interface)
+{
+    // Code for handling a serial (Bluetooth) message when in file-transfer mode
+    //
+    // message = pointer to message char array
+    // length = length of message
+    // interface = 0 for PC serial connection, 1 for Bluetooth [NB only Bluetooth used for file transfer in this version]
+
+    debug("FTM Message:%s [%d]\n",message,length);
+    
+}
+
+
+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;
+        case 100:
+            strcpy(command,"START FILE TRANSFER MODE");
+            if(allow_commands) {
+                command_status = 1;
+                file_transfer_mode = 1;
+                user_code_restore_mode = user_code_running;
+                user_code_running = 0;
+                sprintf(ret_message,"OK");
+                send_message = 1;
+            } else command_status = 2;
+            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);
+}
+
+Timeout bt_message_timeout;
+static float bt_message_timeout_period = 0.001; // 1 millisecond
+char bt_buffer[255];
+int bt_buffer_index = 0;
+
+void IF_bt_message_timeout()
+{
+    char buffer[255];
+
+    sprintf(buffer, bt_buffer, bt_buffer_index);
+    buffer[bt_buffer_index] = 0;
+    if(file_transfer_mode == 1) {
+        IF_handle_file_transfer_serial_message(bt_buffer, bt_buffer_index, 1);
+    } else {
+//    debug("BT message timeout: %s [%d chars]\n", buffer, bt_buffer_index);
+        if(bt_buffer_index == 5 && buffer[0] == COMMAND_MESSAGE_BYTE && buffer[4] == COMMAND_MESSAGE_BYTE) {
+            bt_command_message[0] = buffer[1];
+            bt_command_message[1] = buffer[2];
+            bt_command_message[2] = buffer[3];
+            IF_handle_command_serial_message(bt_command_message , 1);
+        } else IF_handle_user_serial_message(bt_buffer, bt_buffer_index, 1);
+    }
+    bt_buffer_index = 0;
+}
+
+void IF_bt_rx_callback()
+{
+    while(bt.readable()) {
+        char byte = bt.getc();
+
+        bt_buffer[bt_buffer_index] = byte;
+        bt_buffer_index++;
+    }
+
+    bt_message_timeout.attach(&IF_bt_message_timeout, bt_message_timeout_period);
+}
+
+//void IF_bt_rx_callback()
+//{
+//    int count = 0;
+//    char message_array[255];
+//
+//    wait_ms(500); // Wait 0.5ms to allow a complete message to arrive before atttempting to process it
+//
+//    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);
+//}
\ No newline at end of file