serial.cpp

00001 /* University of York Robotics Laboratory PsiSwarm Library: Serial Control Source File
00002  *
00003  * File: serial.cpp
00004  *
00005  * (C) Dept. Electronics & Computer Science, University of York
00006  * James Hilder, Alan Millard, Homero Elizondo, Jon Timmis
00007  *
00008  * PsiSwarm Library Version: 0.3
00009  *
00010  * October 2015
00011  *
00012  */
00013 
00014 #include "psiswarm.h"
00015 
00016 static float command_timeout_period = 0.1f;     //If a complete command message is not received in 0.1s then consider it a user message
00017 char pc_command_message_started = 0;
00018 char pc_command_message_byte = 0;
00019 char pc_command_message[3];
00020 char bt_command_message_started = 0;
00021 char bt_command_message_byte = 0;
00022 char bt_command_message[3];
00023 
00024 
00025 
00026 char allow_commands = 1;
00027 char allow_requests = 1;
00028 
00029 Timeout pc_command_timeout;
00030 Timeout bt_command_timeout;
00031 
00032 
00033 
00034 // A predefined message structure for command messages is as follows:
00035 // [Byte 0][Byte 1][Byte 2][Byte 3][Byte 4]
00036 // Byte 0 and Byte 4 must be equal to COMMAND_MESSAGE_BYTE [in psiswarm.h] or message is treated as a user message
00037 
00038 
00039 //void handle_user_serial_message(char * message, char length, char interface)
00040 //{
00041     //This is where user code for handling a (non-system) serial message should go
00042     //By default, nothing is done
00043     //
00044     //message = pointer to message char array
00045     //length = length of message
00046     //interface = 0 for PC serial connection, 1 for Bluetooth
00047 //}
00048 
00049 void IF_handle_user_serial_message(char * message, char length, char interface)
00050 {
00051     char buffer[255];
00052     sprintf(buffer,message,length);
00053     for(int i=0; i<length; i++) {
00054         buffer[i]=message[i];
00055     }
00056     buffer[length]=0;
00057     //if(interface) debug("Received BT message:%s [%d chars]\n",buffer,length);
00058     //else debug("Received USB message:%s [%d chars]\n",buffer,length);
00059     handle_user_serial_message(message,length,interface);
00060 }
00061 
00062 
00063 void IF_handle_command_serial_message(char message[3], char interface)
00064 {
00065     char iface [4];
00066     if(interface) strcpy(iface,"BT");
00067     else strcpy(iface,"USB");
00068     char command [26];
00069     char subcommand[30];
00070     float dec;
00071     float l_dec;
00072     float r_dec;
00073     int irp_delay;
00074     char colour_string[7];
00075     char ret_message[50];
00076     char send_message = 0;
00077     char command_status = 0;
00078     // command_status values:
00079     // 0 - unrecognised command
00080     // 1 - command actioned
00081     // 2 - command blocked
00082     // 3 - invalid parameters
00083 
00084     subcommand[0]=0;
00085     command[0]=0;
00086     switch(message[0]) {
00087 
00088             // MOTOR COMMANDS
00089 
00090         case 1:
00091             strcpy(command,"SET LEFT MOTOR");
00092             dec = IF_decode_float(message[1],message[2]);
00093             sprintf(subcommand,"%1.5f",dec);
00094             if(allow_commands) {
00095                 command_status = 1;
00096                 set_left_motor_speed(dec);
00097             } else command_status = 2;
00098             break;
00099         case 2:
00100             strcpy(command,"SET RIGHT MOTOR");
00101             dec = IF_decode_float(message[1],message[2]);
00102             sprintf(subcommand,"%1.5f",dec);
00103             if(allow_commands) {
00104                 set_right_motor_speed(dec);
00105                 command_status = 1;
00106             } else command_status = 2;
00107             break;
00108         case 3:
00109             strcpy(command,"SET BOTH MOTORS");
00110             dec = IF_decode_float(message[1],message[2]);
00111             sprintf(subcommand,"%1.5f",dec);
00112             if(allow_commands) {
00113                 command_status = 1;
00114                 forward(dec);
00115             } else command_status = 2;
00116             break;
00117         case 4:
00118             strcpy(command,"BRAKE LEFT MOTOR");
00119             sprintf(subcommand,"");
00120             if(allow_commands) {
00121                 command_status = 1;
00122                 brake_left_motor();
00123             } else command_status = 2;
00124             break;
00125         case 5:
00126             strcpy(command,"BRAKE RIGHT MOTOR");
00127             sprintf(subcommand,"");
00128             if(allow_commands) {
00129                 command_status = 1;
00130                 brake_right_motor();
00131             } else command_status = 2;
00132             break;
00133         case 6:
00134             strcpy(command,"BRAKE BOTH MOTORS");
00135             sprintf(subcommand,"");
00136             if(allow_commands) {
00137                 command_status = 1;
00138                 brake();
00139             } else command_status = 2;
00140             break;
00141         case 7:
00142             strcpy(command,"STOP BOTH MOTORS");
00143             sprintf(subcommand,"");
00144             if(allow_commands) {
00145                 command_status = 1;
00146                 stop();
00147             } else command_status = 2;
00148             break;
00149         case 8:
00150             strcpy(command,"TURN ON SPOT");
00151             dec = IF_decode_float(message[1],message[2]);
00152             sprintf(subcommand,"%1.5f",dec);
00153             if(allow_commands) {
00154                 command_status = 1;
00155                 turn(dec);
00156             } else command_status = 2;
00157             break;
00158         case 9:
00159             strcpy(command,"SET EACH MOTOR");
00160             l_dec = IF_decode_float(message[1]);
00161             r_dec = IF_decode_float(message[2]);
00162             sprintf(subcommand,"L=%1.3f R=%1.3f",l_dec,r_dec);
00163             if(allow_commands) {
00164                 command_status = 1;
00165                 
00166                 set_left_motor_speed(l_dec);
00167                 set_right_motor_speed(r_dec);
00168             } else command_status = 2;
00169             break;
00170             // LED COMMANDS
00171 
00172         case 10:
00173             strcpy(command,"SET LED STATES");
00174             sprintf(subcommand,"G:%s R:%s",IF_char_to_binary_char(message[1]), IF_char_to_binary_char(message[2]));
00175             if(allow_commands) {
00176                 command_status = 1;
00177                 set_leds(message[1],message[2]);
00178             } else command_status = 2;
00179             break;
00180         case 11:
00181             strcpy(command,"SET RED LED STATES");
00182             sprintf(subcommand,"%s",IF_char_to_binary_char(message[1]));
00183             if(allow_commands) {
00184                 command_status = 1;
00185                 set_red_leds(message[1]);
00186             } else command_status = 2;
00187             break;
00188         case 12:
00189             strcpy(command,"SET GREEN LED STATES");
00190             sprintf(subcommand,"%s",IF_char_to_binary_char(message[1]));
00191             if(allow_commands) {
00192                 command_status = 1;
00193                 set_green_leds(message[1]);
00194             } else command_status = 2;
00195             break;
00196         case 13:
00197             strcpy(command,"SET LED");
00198             switch(message[2]) {
00199                 case 1:
00200                     strcpy(colour_string,"RED");
00201                     break;
00202                 case 2:
00203                     strcpy(colour_string,"GREEN");
00204                     break;
00205                 case 3:
00206                     strcpy(colour_string,"BOTH");
00207                     break;
00208                 case 0:
00209                     strcpy(colour_string,"OFF");
00210                     break;
00211             }
00212             if(message[1] < 8 && message[2] < 4) {
00213                 sprintf(subcommand,"%d %s",message[1],colour_string);
00214                 if(allow_commands) {
00215                     command_status = 1;
00216                     set_led(message[1],message[2]);
00217                 } else command_status = 2;
00218             } else {
00219                 sprintf(subcommand,"[INVALID CODE]");
00220                 command_status = 3;
00221             }
00222             break;
00223         case 14:
00224             strcpy(command,"SET CENTER LED STATE");
00225             switch(message[1]) {
00226                 case 1:
00227                     strcpy(colour_string,"RED");
00228                     break;
00229                 case 2:
00230                     strcpy(colour_string,"GREEN");
00231                     break;
00232                 case 3:
00233                     strcpy(colour_string,"BOTH");
00234                     break;
00235                 case 0:
00236                     strcpy(colour_string,"OFF");
00237                     break;
00238             }
00239             if(message[1] < 4) {
00240                 sprintf(subcommand,"%s",colour_string);
00241                 if(allow_commands) {
00242                     command_status = 1;
00243                     set_center_led(message[1]);
00244                 } else command_status = 2;
00245             } else {
00246                 sprintf(subcommand,"[INVALID CODE]");
00247                 command_status = 3;
00248             }
00249             break;
00250         case 15:
00251             strcpy(command,"SET C.LED BRIGHTNESS");
00252             dec = IF_decode_unsigned_float(message[1],message[2]);
00253             sprintf(subcommand,"%1.5f",dec);
00254             if(allow_commands) {
00255                 command_status = 1;
00256                 set_center_led_brightness(dec);
00257             } else command_status = 2;
00258             break;
00259         case 16:
00260             strcpy(command,"SET MBED LEDS");
00261             sprintf(subcommand,"%s",IF_nibble_to_binary_char(message[1]));
00262             if(allow_commands) {
00263                 command_status = 1;
00264                 mbed_led1 = (message[1] & 128) >> 7;
00265                 mbed_led2 = (message[1] & 64) >> 6;
00266                 mbed_led3 = (message[1] & 32) >> 5;
00267                 mbed_led4 = (message[1] & 16) >> 4;
00268             } else command_status = 2;
00269             break;
00270         case 17:
00271             strcpy(command,"BLINK OUTER LEDS");
00272             dec = IF_decode_unsigned_float(message[1],message[2]);
00273             sprintf(subcommand,"FOR %1.5fS",dec);
00274             if(allow_commands) {
00275                 command_status = 1;
00276                 blink_leds(dec);
00277             } else command_status = 2;
00278             break;
00279         case 18:
00280             strcpy(command,"SET BASE LED STATE");
00281             switch(message[1]) {
00282                 case 1:
00283                     strcpy(subcommand,"ON");
00284                     break;
00285                 case 0:
00286                     strcpy(subcommand,"OFF");
00287                     break;
00288             }
00289             //Function not yet implemented
00290             break;
00291         case 19:
00292             strcpy(command,"SET CENTER LED ");
00293             switch(message[1]) {
00294                 case 1:
00295                     strcpy(colour_string,"RED");
00296                     break;
00297                 case 2:
00298                     strcpy(colour_string,"GREEN");
00299                     break;
00300                 case 3:
00301                     strcpy(colour_string,"BOTH");
00302                     break;
00303                 case 0:
00304                     strcpy(colour_string,"OFF");
00305                     break;
00306             }
00307             dec = IF_decode_unsigned_float(message[2]);
00308             sprintf(subcommand,"%s @ %1.5f brightness",colour_string,dec);
00309             if(allow_commands) {
00310                 command_status = 1;
00311                 set_center_led(message[1],dec);
00312             } else command_status = 2;
00313             break;
00314 
00315             // DISPLAY COMMANDS
00316 
00317         case 20:
00318             strcpy(command,"SET DISPLAY ");
00319             switch(message[1]) {
00320                 case 0:
00321                     strcpy(subcommand,"CLEAR");
00322                     if(allow_commands) {
00323                         command_status = 1;
00324                         display.clear_display();
00325                     } else command_status = 2;
00326                     break;
00327                 case 1:
00328                     strcpy(subcommand,"MESSAGE 1");
00329                     if(allow_commands) {
00330                         command_status = 1;
00331                         display.clear_display();
00332                         display.home();
00333                         display.write_string("PC CONNECTION");
00334                         display.set_position(1,0);
00335                         display.write_string("STARTED");
00336                     } else command_status = 2;
00337                     break;
00338                 case 2:
00339                     strcpy(subcommand,"MESSAGE 2");
00340                     if(allow_commands) {
00341                         command_status = 1;
00342                         display.clear_display();
00343                         display.home();
00344                         display.write_string("PC CONNECTION");
00345                         display.set_position(1,0);
00346                         display.write_string("TERMINATED");
00347                     } else command_status = 2;
00348                     break;
00349                 case 3:
00350                     strcpy(subcommand,"MESSAGE 3");
00351                     if(allow_commands) {
00352                         command_status = 1;
00353                         display.clear_display();
00354                         display.home();
00355                         display.write_string("ANDROID DEVICE");
00356                         display.set_position(1,0);
00357                         display.write_string("CONNECTED");
00358                     } else command_status = 2;
00359                     break;
00360                 case 4:
00361                     strcpy(subcommand,"MESSAGE 4");
00362                     if(allow_commands) {
00363                         command_status = 1;
00364                         display.clear_display();
00365                         display.home();
00366                         display.write_string("ANDROID DEVICE");
00367                         display.set_position(1,0);
00368                         display.write_string("DISCONNECTED");
00369                     } else command_status = 2;
00370                     break;
00371             }
00372             break;
00373         case 21:
00374             strcpy(command,"SET CURSOR ");
00375             if(message[1] < 2 && message[2] < 16) {
00376                 sprintf(subcommand,"[%d,%d]",message[1],message[2]);
00377                 if(allow_commands) {
00378                     display.set_position(message[1],message[2]);
00379                 } else command_status = 2;
00380             } else {
00381                 sprintf(subcommand,"[INVALID]");
00382                 command_status = 3;
00383             }
00384             break;
00385         case 22:
00386             strcpy(command,"PRINT CHARACTERS ");
00387             char print_message[2];
00388             print_message[0]=message[1];
00389             print_message[1]=message[2];
00390             sprintf(subcommand,"[%c,%c]",message[1],message[2]);
00391             if(allow_commands) {
00392                 display.write_string(print_message,2);
00393             } else command_status = 2;
00394             break;
00395         case 23:
00396             strcpy(command,"SET DISPLAY B.NESS");
00397             dec = IF_decode_unsigned_float(message[1],message[2]);
00398             sprintf(subcommand,"%1.5f",dec);
00399             if(allow_commands) {
00400                 command_status = 1;
00401                 display.set_backlight_brightness(dec);
00402             } else command_status = 2;
00403             break;
00404 
00405         case 30:
00406             strcpy(command,"SET DEBUG MODE");
00407             switch(message[1]) {
00408                 case 1:
00409                     strcpy(subcommand,"ON");
00410                     break;
00411                 case 0:
00412                     strcpy(subcommand,"OFF");
00413                     break;
00414             }
00415             if(message[2] & 1) strcat (subcommand,"-PC");
00416             if(message[2] & 2) strcat (subcommand,"-BT");
00417             if(message[2] & 4) strcat (subcommand,"-DISP");
00418             if(allow_commands) {
00419                 command_status = 1;
00420                 debug_mode = message[1];
00421                 debug_output = message[2];
00422             } else command_status = 2;
00423             break;
00424         case 31:
00425             strcpy(command,"SET DEMO MODE");
00426             switch(message[1] % 2) {
00427                 case 1:
00428                     strcpy(subcommand,"ON");
00429                     break;
00430                 case 0:
00431                     strcpy(subcommand,"OFF");
00432                     break;
00433             }
00434             if(allow_commands) {
00435                 command_status = 1;
00436                 demo_on = message[1] % 2;
00437                 if(demo_on == 1) {
00438                     user_code_restore_mode = user_code_running;
00439                     user_code_running = 0;
00440                 } else {
00441                     user_code_running = user_code_restore_mode;
00442                 }
00443             } else command_status = 2;
00444             break;
00445         case 32:
00446             strcpy(command,"SET USER CODE");
00447             switch(message[1] % 2) {
00448                 case 1:
00449                     strcpy(subcommand,"ON");
00450                     break;
00451                 case 0:
00452                     strcpy(subcommand,"OFF");
00453                     break;
00454             }
00455             if(allow_commands) {
00456                 command_status = 1;
00457                 user_code_running = message[1] % 2;
00458             } else command_status = 2;
00459             break;
00460         case 33:
00461             strcpy(command,"PAUSE USER CODE");
00462             dec = IF_decode_unsigned_float(message[1],message[2]) * 10;
00463             sprintf(subcommand,"FOR %2.3fS",dec);
00464             if(allow_commands) {
00465                 command_status = 1;
00466                 pause_user_code(dec);
00467             } else command_status = 2;
00468             break;
00469 
00470         case 34:
00471             strcpy(command,"RESET ENCODERS");
00472             if(allow_commands) {
00473                 command_status = 1;
00474                 reset_encoders();
00475             } else command_status = 2;
00476             break;
00477 
00478         case 35:
00479             strcpy(command,"SET ALLOW COMMANDS");
00480             switch(message[1] % 2) {
00481                 case 1:
00482                     strcpy(subcommand,"ON");
00483                     break;
00484                 case 0:
00485                     strcpy(subcommand,"OFF");
00486                     break;
00487             }
00488             allow_commands = message[1] % 2;
00489             command_status = 1;
00490             break;
00491 
00492         case 36:
00493             irp_delay = (message[1] << 8) + message[2];
00494             sprintf(command,"SET IR PULSE DELAY %d MS",irp_delay);
00495             if(allow_commands) {
00496                 command_status = 1;
00497                 ir_pulse_delay = irp_delay;
00498             } else command_status = 2;
00499             break;
00500         case 37:
00501             irp_delay = (message[1] << 8) + message[2];
00502             sprintf(command,"SET BASE IR PULSE DELAY %d MS",irp_delay);
00503             if(allow_commands) {
00504                 command_status = 1;
00505                 base_ir_pulse_delay = irp_delay;
00506             } else command_status = 2;
00507             break;
00508 
00509             // MOTOR REQUESTS
00510         case 40:
00511             strcpy(command,"GET LEFT MOTOR SPEED");
00512             sprintf(ret_message,"%1.5f",motor_left_speed);
00513             send_message = 1;
00514             break;
00515 
00516         case 41:
00517             strcpy(command,"GET RIGHT MOTOR SPEED");
00518             sprintf(ret_message,"%1.5f",motor_right_speed);
00519             send_message = 1;
00520             break;
00521         case 42:
00522             strcpy(command,"GET BRAKE STATES");
00523             sprintf(ret_message,"%d,%d",motor_left_brake,motor_right_brake);
00524             send_message = 1;
00525             break;
00526         case 43:
00527             strcpy(command,"GET MOTOR STATES");
00528             //sprintf(ret_message,"%d,%d",motor_left_brake,motor_right_brake);
00529             send_message = 1;
00530             break;
00531         case 44:
00532             strcpy(command,"GET ENCODERS");
00533             sprintf(ret_message,"%d,%d",left_encoder,right_encoder);
00534             send_message = 1;
00535             break;
00536 
00537             // LED REQUESTS
00538         case 50:
00539             strcpy(command,"GET LED STATES");
00540             sprintf(ret_message,"%04x",get_led_states());
00541             send_message = 1;
00542             break;
00543 
00544             // GENERAL REQUESTS
00545         case 60:
00546             strcpy(command,"GET SOFTWARE VERSION");
00547             sprintf(ret_message,"%1.2f",SOFTWARE_VERSION_CODE);
00548             send_message = 1;
00549             break;
00550 
00551         case 61:
00552             strcpy(command,"GET UPTIME");
00553             sprintf(ret_message,"%6.2f",get_uptime());
00554             send_message = 1;
00555             break;
00556 
00557         case 62:
00558             strcpy(command,"GET ID");
00559             sprintf(ret_message,"%d",robot_id);
00560             send_message = 1;
00561             break;
00562 
00563         case 63:
00564             strcpy(command,"GET SWITCH BYTE");
00565             sprintf(ret_message,"%02x",switch_byte);
00566             send_message = 1;
00567             break;
00568         case 64:
00569             strcpy(command,"GET USER CODE");
00570             sprintf(ret_message,"%d",user_code_running);
00571             send_message = 1;
00572             break;
00573         case 65:
00574             strcpy(command,"GET RESPONSE STRING");
00575             sprintf(ret_message,"PSI");
00576             send_message = 1;
00577             break;
00578         case 66:
00579             strcpy(command,"GET PROGRAM NAME");
00580             sprintf(ret_message,"%s",program_name);
00581             send_message = 1;
00582             break;
00583         case 67:
00584             strcpy(command,"GET AUTHOR NAME");
00585             sprintf(ret_message,"%s",author_name);
00586             send_message = 1;
00587             break;
00588         case 68:
00589             strcpy(command,"GET DEBUG MODE");
00590             sprintf(ret_message,"%1d%1d",debug_mode,debug_output);
00591             send_message = 1;
00592             break;
00593          case 69:
00594             strcpy(command,"GET SYSTEM WARNINGS");
00595             sprintf(ret_message,"%d",system_warnings);
00596             send_message = 1;
00597             break;
00598     
00599 
00600         // Sensors
00601         case 80:
00602             strcpy(command,"STORE BG. IR VALUES");
00603             if(allow_commands) {
00604                 command_status = 1;
00605                 store_background_raw_ir_values();
00606             } else command_status = 2;
00607             break;
00608         case 81:
00609             strcpy(command,"STORE IL. IR VALUES");
00610             if(allow_commands) {
00611                 command_status = 1;
00612                 store_illuminated_raw_ir_values();
00613             } else command_status = 2;
00614             break;   
00615         case 82:
00616             strcpy(command,"STORE IR VALUES");
00617             if(allow_commands) {
00618                 command_status = 1;
00619                 store_ir_values();
00620             } else command_status = 2;
00621             break;  
00622         case 83:
00623             strcpy(command,"STORE BG BASE IR VALUES");
00624             if(allow_commands) {
00625                 command_status = 1;
00626                 store_background_base_ir_values();
00627             } else command_status = 2;
00628             break;  
00629         case 84:
00630             strcpy(command,"STORE IL. BASE IR VALUES");
00631             if(allow_commands) {
00632                 command_status = 1;
00633                 store_illuminated_base_ir_values();
00634             } else command_status = 2;
00635             break;  
00636         case 85:
00637             strcpy(command,"STORE BASE IR VALUES");
00638             if(allow_commands) {
00639                 command_status = 1;
00640                 store_base_ir_values();
00641             } else command_status = 2;
00642             break; 
00643         case 86:
00644             strcpy(command,"STORE ALL IR VALUES");
00645             if(allow_commands) {
00646                 command_status = 1;
00647                 store_ir_values();
00648                 store_base_ir_values();
00649             } else command_status = 2;
00650             break; 
00651         case 90:
00652             sprintf(command,"%s %d","GET BG IR VALUE",message[1]);
00653             sprintf(ret_message,"%d",get_background_raw_ir_value(message[1]));
00654             send_message = 1;
00655             break;
00656         case 91:
00657             sprintf(command,"%s %d","GET IL IR VALUE",message[1]);
00658             sprintf(ret_message,"%d",get_illuminated_raw_ir_value(message[1]));
00659             send_message = 1;
00660             break;
00661         case 92:
00662             strcpy(command,"GET BG IR VALUES");
00663             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));
00664             send_message = 1;
00665             break;
00666         case 93:
00667             strcpy(command,"GET ILLUMINATED IR VALUES");
00668             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));
00669             send_message = 1;
00670             break;
00671         case 94:
00672             sprintf(command,"%s %d","GET BG BASE IR VALUE",message[1]);
00673             sprintf(ret_message,"%d",get_background_base_ir_value(message[1]));
00674             send_message = 1;
00675             break;
00676         case 95:
00677             sprintf(command,"%s %d","GET IL BASE IR VALUE",message[1]);
00678             sprintf(ret_message,"%d",get_illuminated_base_ir_value(message[1]));
00679             send_message = 1;
00680             break;
00681         case 96:
00682             strcpy(command,"GET BG BASE IR VALUES");
00683             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));
00684             send_message = 1;
00685             break;
00686         case 97:
00687             strcpy(command,"GET IL BASE IR VALUES");
00688             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));
00689             send_message = 1;
00690             break;        
00691     }
00692 
00693 
00694     if(send_message) {
00695         char message_length = strlen(ret_message);
00696         switch(interface) {
00697             case 0:
00698                 pc.printf("%c%c%s",RESPONSE_MESSAGE_BYTE,message_length,ret_message);
00699                 break;
00700             case 1:
00701                 bt.printf("%c%c%s",RESPONSE_MESSAGE_BYTE,message_length,ret_message);
00702                 break;
00703         }
00704         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);
00705     } else {
00706         switch(interface) {
00707             case 0:
00708                 pc.printf("%c%c",ACKNOWLEDGE_MESSAGE_BYTE,command_status);
00709                 break;
00710             case 1:
00711                 bt.printf("%c%c",ACKNOWLEDGE_MESSAGE_BYTE,command_status);
00712                 break;
00713         }
00714         switch(command_status) {
00715             case 0:
00716                 debug("Unrecognised %s command message [%02x%02x%02x]\n",iface,message[0],message[1],message[2]);
00717                 break;
00718             case 1:
00719                 debug("Actioned %s command message:%s %s [%02x%02x%02x]\n",iface, command, subcommand,message[0],message[1],message[2]);
00720                 break;
00721             case 2:
00722                 debug("Blocked %s command message:%s %s [%02x%02x%02x]\n",iface, command, subcommand,message[0],message[1],message[2]);
00723                 break;
00724             case 3:
00725                 debug("Invalid %s command message:%s %s [%02x%02x%02x]\n",iface, command, subcommand,message[0],message[1],message[2]);
00726                 break;
00727         }
00728     }
00729 }
00730 
00731 char * IF_nibble_to_binary_char(char in)
00732 {
00733     char * ret = (char*)malloc(sizeof(char)*5);
00734     for(int i=0; i<4; i++) {
00735         if(in & (128 >> i)) ret[i]='1';
00736         else ret[i]='0';
00737     }
00738     ret[4]=0;
00739     return ret;
00740 }
00741 
00742 char * IF_char_to_binary_char(char in)
00743 {
00744     char * ret = (char*)malloc(sizeof(char)*9);
00745     for(int i=0; i<8; i++) {
00746         if(in & (128 >> i)) ret[i]='1';
00747         else ret[i]='0';
00748     }
00749     ret[8]=0;
00750     return ret;
00751 }
00752 
00753 float IF_decode_unsigned_float(char byte0, char byte1)
00754 {
00755     unsigned short sval = (byte0) << 8;
00756     sval += byte1;
00757     float scaled = sval / 65535.0f;
00758     return scaled;
00759 }
00760 
00761 float IF_decode_float(char byte0, char byte1)
00762 {
00763     // MSB is byte 0 is sign, rest is linear spread between 0 and 1
00764     char sign = byte0 / 128;
00765     short sval = (byte0 % 128) << 8;
00766     sval += byte1;
00767     float scaled = sval / 32767.0f;
00768     if(sign == 0) scaled = 0-scaled;
00769     return scaled;
00770 }
00771 
00772 float IF_decode_unsigned_float(char byte0)
00773 {
00774     unsigned short sval = (byte0);
00775     float scaled = sval / 255.0f;
00776     return scaled;
00777 }
00778 
00779 float IF_decode_float(char byte0)
00780 {
00781     // MSB is byte 0 is sign, rest is linear spread between 0 and 1
00782     char sign = byte0 / 128;
00783     short sval = (byte0 % 128);
00784     float scaled = sval / 127.0f;
00785     if(sign == 0) scaled = 0-scaled;
00786     return scaled;
00787 }
00788 
00789 void IF_setup_serial_interfaces()
00790 {
00791     if(ENABLE_PC_SERIAL) {
00792         pc.baud(PC_BAUD);
00793         pc.attach(&IF_pc_rx_callback, Serial::RxIrq);
00794     }
00795     if(ENABLE_BLUETOOTH) {
00796         bt.baud(BLUETOOTH_BAUD);
00797         bt.attach(&IF_bt_rx_callback, Serial::RxIrq);
00798     }
00799 }
00800 
00801 void IF_pc_rx_command_timeout()
00802 {
00803     char message_array[6];
00804     char length = 1 + pc_command_message_byte;
00805     pc_command_message_started = 0;
00806     message_array[0] = COMMAND_MESSAGE_BYTE;
00807     for(int k=0; k<pc_command_message_byte; k++) {
00808         message_array[k+1] = pc_command_message[k];
00809     }
00810     IF_handle_user_serial_message(message_array, length, 0);
00811 }
00812 
00813 void IF_bt_rx_command_timeout()
00814 {
00815     char message_array[6];
00816     char length = 1 + bt_command_message_byte;
00817     bt_command_message_started = 0;
00818     message_array[0] = COMMAND_MESSAGE_BYTE;
00819     for(int k=0; k<bt_command_message_byte; k++) {
00820         message_array[k+1] = bt_command_message[k];
00821     }
00822     IF_handle_user_serial_message(message_array, length, 1);
00823 }
00824 
00825 void IF_pc_rx_callback()
00826 {
00827     int count = 0;
00828     char message_array[255];
00829 
00830     while(pc.readable()) {
00831         char tc = pc.getc();
00832         message_array[count] = tc;
00833         count ++;
00834         if(pc_command_message_started == 1) {
00835             if(pc_command_message_byte == 3) {
00836                 pc_command_timeout.detach();
00837                 if(tc == COMMAND_MESSAGE_BYTE) {
00838                     // A complete command message succesfully received, call handler
00839                     pc_command_message_started = 0;
00840                     count = 0;
00841                     IF_handle_command_serial_message(pc_command_message , 0);
00842                 } else {
00843                     // Message is not a valid command message as 5th byte is not correct; treat whole message as a user message
00844                     pc_command_message_started = 0;
00845                     message_array[0] = COMMAND_MESSAGE_BYTE;
00846                     message_array[1] = pc_command_message[0];
00847                     message_array[2] = pc_command_message[1];
00848                     message_array[3] = pc_command_message[2];
00849                     message_array[4] = tc;
00850                     count = 5;
00851                 }
00852             } else {
00853                 pc_command_message[pc_command_message_byte] = tc;
00854                 pc_command_message_byte ++;
00855             }
00856         } else {
00857             if(count == 1) {
00858                 if(tc == COMMAND_MESSAGE_BYTE) {
00859                     pc_command_timeout.attach(&IF_pc_rx_command_timeout,command_timeout_period);
00860                     pc_command_message_started = 1;
00861                     pc_command_message_byte = 0;
00862 
00863                 }
00864             }
00865         }
00866     }
00867     if(!pc_command_message_started && count>0) IF_handle_user_serial_message(message_array, count, 0);
00868 }
00869 
00870 void IF_bt_rx_callback()
00871 {
00872     int count = 0;
00873     char message_array[255];
00874 
00875     while(bt.readable()) {
00876         char tc = bt.getc();
00877         message_array[count] = tc;
00878         count ++;
00879         if(bt_command_message_started == 1) {
00880             if(bt_command_message_byte == 3) {
00881                 bt_command_timeout.detach();
00882                 if(tc == COMMAND_MESSAGE_BYTE) {
00883                     // A complete command message succesfully received, call handler
00884                     bt_command_message_started = 0;
00885                     count = 0;
00886                     IF_handle_command_serial_message(bt_command_message , 1);
00887                 } else {
00888                     // Message is not a valid command message as 5th byte is not correct; treat whole message as a user message
00889                     bt_command_message_started = 0;
00890                     message_array[0] = COMMAND_MESSAGE_BYTE;
00891                     message_array[1] = bt_command_message[0];
00892                     message_array[2] = bt_command_message[1];
00893                     message_array[3] = bt_command_message[2];
00894                     message_array[4] = tc;
00895                     count = 5;
00896                 }
00897             } else {
00898                 bt_command_timeout.attach(&IF_bt_rx_command_timeout,command_timeout_period);
00899                 bt_command_message[bt_command_message_byte] = tc;
00900                 bt_command_message_byte ++;
00901             }
00902         } else {
00903             if(count == 1) {
00904                 if(tc == COMMAND_MESSAGE_BYTE) {
00905                     bt_command_message_started = 1;
00906                     bt_command_message_byte = 0;
00907 
00908                 }
00909             }
00910         }
00911     }
00912     if(!bt_command_message_started && count>0) IF_handle_user_serial_message(message_array, count, 1);
00913 }