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