Science Memeseum
/
BeaconDemo_RobotCodeA
Important changes to repositories hosted on mbed.com
Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.
To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.
It is also possible to export all your personal repositories from the account settings page.
Dependencies: mbed
Fork of
BeaconDemo_RobotCode
by 
Science Memeseum
PsiSwarm/serial.cpp
- Committer:
- jah128
- Date:
- 2015-10-03
- Revision:
- 0:8a5497a2e366
- Child:
- 6:ff3c66f7372b
File content as of revision 0:8a5497a2e366:
/* 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);
}