UKESF Headstart Summer School
Library for the PsiSwarm Robot for Headstart Lab - Version 0.5
Fork of
PsiSwarmLibrary
by 
James Hilder
serial.cpp
- Committer:
- jah128
- Date:
- 2016-03-03
- Revision:
- 1:060690a934a9
- Parent:
- 0:d6269d17c8cf
- Child:
- 2:c6986ee3c7c5
File content as of revision 1:060690a934a9:
/* 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;
char file_transfer_state = 0;
int file_length;
char filename [13];
Timeout ft_timeout;
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_start_file_transfer_mode()
{
display.clear_display();
display.set_position(0,0);
display.write_string("FILE TRANSFER");
display.set_position(1,0);
display.write_string("MODE...");
file_transfer_mode = 1;
file_transfer_state = 0;
file_length = 0;
user_code_restore_mode = user_code_running;
user_code_running = 0;
ft_timeout.attach(IF_file_transfer_timeout,2.0);
}
void IF_invalid_transfer(void)
{
debug("File transfer failed\n");
display.clear_display();
display.set_position(0,0);
display.write_string("TRANSFER FAILED");
wait(0.5);
IF_end_file_transfer_mode();
}
void IF_file_transfer_timeout(void)
{
debug("File transfer failed: timeout\n");
display.clear_display();
display.set_position(0,0);
display.write_string("TRANSFER TIMEOUT");
wait(0.5);
IF_end_file_transfer_mode();
}
void IF_end_file_transfer_mode(void)
{
display.clear_display();
file_transfer_mode = 0;
user_code_running = user_code_restore_mode;
}
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",length,message,length);
char * file_data;
int block_index;
int final_block;
int expected_size;
int block_size = 100;
// The first byte in EVERY message received should be 33; if it isn't, abort the transfer
if(message[0] != 33) {
IF_invalid_transfer();
} else {
switch(file_transfer_state) {
case 0: //First message received is the target filename
//The filenames cannot be more that 8.3 characters long (FAT12 format)
if(length == 1 || length > 13) IF_invalid_transfer();
else {
strncpy(filename, message + 1, length - 1);
//Send acknowledge ("FN")
ft_timeout.detach();
ft_timeout.attach(IF_file_transfer_timeout,2.0);
bt.printf("%c%c%s",RESPONSE_MESSAGE_BYTE,2,"FN");
file_transfer_state = 1;
}
break;
case 1: //Second message is the length of the file in bytes
//Length is encoded as a 3-byte value
if(length != 4) IF_invalid_transfer();
else {
file_length = (message[1]) * 256;
file_length += (message[2]);
file_length *= 256;
file_length += message[3];
file_transfer_state = 2;
display.clear_display();
char display_message[17];
sprintf(display_message,"F:%s",filename);
display.set_position(0,0);
display.write_string(display_message);
display.set_position(1,0);
sprintf(display_message,"S:%d b",file_length);
display.write_string(display_message);
block_index = 0;
//Work out how many blocks the file will be sent in (size = block_size, tested at 100 bytes)
//Allocate memory for the file up to a limit of 16 blocks; larger files will be split across
//multiple blocks....
final_block = file_length / block_size;
if(file_length % block_size != 0) final_block ++;
int target_size = file_length;
if(file_length > (block_size * 16)) target_size = block_size * 16;
file_data = (char *) malloc(target_size);
ft_timeout.detach();
ft_timeout.attach(IF_file_transfer_timeout,1.0);
//Send acknowledge (size of file)
bt.printf("%c%c%c%c%c",RESPONSE_MESSAGE_BYTE,3,message[1],message[2],message[3]);
}
break;
case 2:
block_index ++;
if(block_index == final_block) expected_size = file_length % block_size;
if(expected_size == 0) expected_size = block_size;
if(length!=expected_size + 1){
// Unexpected length
}else{
ft_timeout.detach();
ft_timeout.attach(IF_file_transfer_timeout,1.0);
}
}
}
}
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;
IF_start_file_transfer_mode();
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);
//}