Bluetooth communication for flocking.
Fork of BeautifulMemeProject by
Diff: PsiSwarm/serial.cpp
- Revision:
- 0:8a5497a2e366
- Child:
- 6:ff3c66f7372b
diff -r 000000000000 -r 8a5497a2e366 PsiSwarm/serial.cpp --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/PsiSwarm/serial.cpp Sat Oct 03 22:48:50 2015 +0000 @@ -0,0 +1,912 @@ +/* University of York Robotics Laboratory PsiSwarm Library: Serial Control Source File + * + * File: serial.cpp + * + * (C) Dr James Hilder, Dept. Electronics & Computer Science, University of York + * + * PsiSwarm Library Version: 0.2 + * + * September 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); +} \ No newline at end of file