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, Alexander Horsfield, Homero Elizondo, Jon Timmis
00007  *
00008  * PsiSwarm Library Version: 0.41
00009  *
00010  * March 2016
00011  *
00012  *
00013  */
00014 
00015 #include "psiswarm.h"
00016 
00017 static float command_timeout_period = 0.1f;     //If a complete command message is not received in 0.1s then consider it a user message
00018 char pc_command_message_started = 0;
00019 char pc_command_message_byte = 0;
00020 char pc_command_message[3];
00021 char bt_command_message_started = 0;
00022 char bt_command_message_byte = 0;
00023 char bt_command_message[3];
00024 
00025 char allow_commands = 1;
00026 char allow_requests = 1;
00027 char file_transfer_state = 0;
00028 
00029 int block_size = 88;   // The data block size for file transfer
00030 char data_block[89];   // Stores the data block to write for Bluetooth file transfer
00031 int data_written;       // Stores if partial data has been written to a file
00032 int file_length;        // Stores the file length for a Bluetooth file transfer
00033 int final_block;        // Stores the index of the final data block for a Bluetooth file transfer
00034 int block_index;        // Stores the current block index for a Bluetooth file transfer
00035 char filename [21];     // Stores the filename for a Bluetooth file transfer
00036 
00037 Timeout ft_timeout;
00038 Timeout pc_command_timeout;
00039 Timeout bt_command_timeout;
00040 
00041 // A predefined message structure for command messages is as follows:
00042 // [Byte 0][Byte 1][Byte 2][Byte 3][Byte 4]
00043 // Byte 0 and Byte 4 must be equal to COMMAND_MESSAGE_BYTE [in psiswarm.h] or message is treated as a user message
00044 
00045 
00046 
00047 
00048 void IF_start_file_transfer_mode()
00049 {
00050     display.clear_display();
00051     display.set_position(0,0);
00052     display.write_string("FILE TRANSFER");
00053     display.set_position(1,0);
00054     display.write_string("MODE...");
00055     data_written = 0;
00056     file_transfer_mode = 1;
00057     file_transfer_state = 0;
00058     file_length = 0;
00059     user_code_restore_mode = user_code_running;
00060     user_code_running = 0;
00061     ft_timeout.attach(IF_file_transfer_timeout,2.0);
00062 }
00063 
00064 
00065 void IF_invalid_transfer(void)
00066 {
00067     debug("File transfer failed\n");
00068     if(data_written == 1){
00069         debug("Deleting corrupted file\n");
00070         remove(filename);
00071     }
00072     display.clear_display();
00073     display.set_position(0,0);
00074     display.write_string("TRANSFER FAILED");
00075     wait(0.5);
00076     IF_end_file_transfer_mode();
00077 }
00078 
00079 void IF_file_transfer_timeout(void)
00080 {
00081     debug("File transfer failed: timeout\n");
00082     display.clear_display();
00083     display.set_position(0,0);
00084     display.write_string("TRANSFER TIMEOUT");
00085     wait(0.5);
00086     IF_end_file_transfer_mode();
00087 }
00088 
00089 void IF_end_file_transfer_mode(void)
00090 {
00091     display.clear_display();
00092     file_transfer_mode = 0;
00093     user_code_running = user_code_restore_mode;
00094 }
00095 
00096 
00097 void IF_handle_file_transfer_serial_message(char * message, char length, char interface)
00098 {
00099     // Code for handling a serial (Bluetooth) message when in file-transfer mode
00100     //
00101     // message = pointer to message char array
00102     // length = length of message
00103     // interface = 0 for PC serial connection, 1 for Bluetooth [NB only Bluetooth used for file transfer in this version]
00104 
00105     if(file_transfer_state < 2)debug("FTM Message:%.*s [%d]\n",length,message,length);
00106     else debug("FTM data block received (%d bytes)\n",length);
00107     int expected_size;
00108     // The first byte in EVERY message received should be 33; if it isn't, abort the transfer
00109     if(message[0] != 33) {
00110         IF_invalid_transfer();
00111     } else {
00112         switch(file_transfer_state) {
00113             case 0: //First message received is the target filename
00114                 //The filenames cannot be more that 8.3 characters long (FAT12 format)
00115                 if(length == 1 || length > 13) IF_invalid_transfer();
00116                 else {
00117                     strcpy(filename, "/local/");
00118                     strncat(filename, message + 1, length - 1);
00119                     debug("Target filename:%s\n",filename);
00120                     //Send acknowledge ("FN")
00121                     ft_timeout.detach();
00122                     ft_timeout.attach(IF_file_transfer_timeout,2.0);
00123                     bt.printf("%c%c%s",RESPONSE_MESSAGE_BYTE,2,"FN");
00124                     file_transfer_state = 1;
00125                 }
00126                 break;
00127             case 1: //Second message is the length of the file in bytes
00128                 //Length is encoded as a 3-byte value
00129                 if(length != 4) IF_invalid_transfer();
00130                 else {
00131                     file_length = (message[1]) * 256;
00132                     file_length += (message[2]);
00133                     file_length *= 256;
00134                     file_length += message[3];
00135                     file_transfer_state = 2;
00136                     display.clear_display();
00137                     char display_message[17];
00138                     sprintf(display_message,"F:%s",filename);
00139                     display.set_position(0,0);
00140                     display.write_string(display_message);
00141                     display.set_position(1,0);
00142                     sprintf(display_message,"S:%d b",file_length);
00143                     display.write_string(display_message);
00144                     block_index = 0;
00145                     //Work out how many blocks the file will be sent in (size = block_size, tested at 100 bytes)
00146                     //Allocate memory for the file up to a limit of 16 blocks; larger files will be split across
00147                     //multiple blocks....
00148                     final_block = file_length / block_size;
00149                     if(file_length % block_size != 0) final_block ++;
00150                     //int target_size = file_length;
00151                     //if(file_length > (block_size * 16)) target_size = block_size * 16;
00152                     //file_data = (char *) malloc(target_size);
00153                     debug("File size %d bytes (%d blocks of %d bytes)\n",file_length,final_block,block_size);
00154                     ft_timeout.detach();
00155                     ft_timeout.attach(IF_file_transfer_timeout,1.0);
00156                     //Send acknowledge (size of file)
00157                     bt.printf("%c%c%c%c%c",RESPONSE_MESSAGE_BYTE,3,message[1],message[2],message[3]);
00158                 }
00159                 break;
00160             case 2:
00161                 block_index ++;
00162                 display.clear_display();
00163                 display.set_position(0,0);
00164                 display.write_string("FILE TRANSFER");
00165                 display.set_position(1,0);
00166                 char details_string[17];
00167                 sprintf(details_string,"BLOCK %d OF %d",block_index,final_block);
00168                 display.write_string(details_string);
00169                 expected_size = block_size;
00170                 if(block_index == final_block) expected_size = file_length % block_size;
00171                 if(expected_size == 0) expected_size = block_size;
00172                 if(length!=expected_size + 1){
00173                     // Unexpected length   
00174                     debug("File data unexpected length in packet %d (%d bytes received, %d bytes expected)\n",block_index,length-1,expected_size);
00175                 }else{
00176                     char transfer_mode[2]={'a'};
00177                     if(block_index == 1){
00178                         transfer_mode[0]='w';
00179                     }
00180                     FILE *fp = fopen(filename,transfer_mode);
00181                     //strncpy(data_block,message+1,length);
00182                     //data_block[length]=0;
00183                     //fprintf(fp,data_block);
00184                     int bytes_written;
00185                     bytes_written = fwrite(message+1,expected_size,1,fp);
00186                     fclose(fp);
00187                     if(data_written == false && bytes_written > 0) data_written = true;
00188                     debug("Bytes written: %d\n",expected_size * bytes_written);
00189                     if(block_index < final_block){
00190                     debug("Message packet %d received and written\n",block_index);
00191                     //Send acknowledge ("D")
00192                     ft_timeout.detach();
00193                     ft_timeout.attach(IF_file_transfer_timeout,1.0);   
00194                     bt.printf("%c%c%s",RESPONSE_MESSAGE_BYTE,1,"D");
00195                     }
00196                     else{
00197                         //Last data block written
00198                         //[Put file checking code here]
00199                         //Send acknowledge ("P");
00200                         bt.printf("%c%c%s",RESPONSE_MESSAGE_BYTE,1,"F");
00201                         ft_timeout.detach();
00202                         debug("File transfer completed successfully\n");
00203                         wait(0.25);
00204                         //Calculate CRC16 value for file
00205                         IF_calculateCRC16(file_length);
00206                         
00207                         display.clear_display();
00208                         display.write_string("FILE TRANSFER");
00209                         display.set_position(1,0);
00210                         display.write_string("COMPLETE");   
00211                         wait(1);
00212                         debug("File transfer mode ended\n");
00213                         IF_end_file_transfer_mode();
00214                     }
00215                 }
00216                 break;
00217         }
00218     }
00219 }
00220 
00221 
00222 void IF_handle_user_serial_message(char * message, char length, char interface)
00223 {
00224     char buffer[255];
00225     sprintf(buffer,message,length);
00226     for(int i=0; i<length; i++) {
00227         buffer[i]=message[i];
00228     }
00229     buffer[length]=0;
00230 //    if(interface) debug("Received BT message:%s [%d chars]\n",buffer,length);
00231 //    else debug("Received USB message:%s [%d chars]\n",buffer,length);
00232     handle_user_serial_message(message,length,interface);
00233 }
00234 
00235 void IF_handle_command_serial_message(char message[3], char interface)
00236 {
00237     char iface [4];
00238     if(interface) strcpy(iface,"BT");
00239     else strcpy(iface,"USB");
00240     char command [26];
00241     char subcommand[30];
00242     float dec;
00243     float l_dec;
00244     float r_dec;
00245     int irp_delay;
00246     char colour_string[7];
00247     char ret_message[50];
00248     char send_message = 0;
00249     char command_status = 0;
00250     // command_status values:
00251     // 0 - unrecognised command
00252     // 1 - command actioned
00253     // 2 - command blocked
00254     // 3 - invalid parameters
00255 
00256     subcommand[0]=0;
00257     command[0]=0;
00258     switch(message[0]) {
00259 
00260             // MOTOR COMMANDS
00261 
00262         case 1:
00263             strcpy(command,"SET LEFT MOTOR");
00264             dec = IF_decode_float(message[1],message[2]);
00265             sprintf(subcommand,"%1.5f",dec);
00266             if(allow_commands) {
00267                 command_status = 1;
00268                 set_left_motor_speed(dec);
00269             } else command_status = 2;
00270             break;
00271         case 2:
00272             strcpy(command,"SET RIGHT MOTOR");
00273             dec = IF_decode_float(message[1],message[2]);
00274             sprintf(subcommand,"%1.5f",dec);
00275             if(allow_commands) {
00276                 set_right_motor_speed(dec);
00277                 command_status = 1;
00278             } else command_status = 2;
00279             break;
00280         case 3:
00281             strcpy(command,"SET BOTH MOTORS");
00282             dec = IF_decode_float(message[1],message[2]);
00283             sprintf(subcommand,"%1.5f",dec);
00284             if(allow_commands) {
00285                 command_status = 1;
00286                 forward(dec);
00287             } else command_status = 2;
00288             break;
00289         case 4:
00290             strcpy(command,"BRAKE LEFT MOTOR");
00291             sprintf(subcommand,"");
00292             if(allow_commands) {
00293                 command_status = 1;
00294                 brake_left_motor();
00295             } else command_status = 2;
00296             break;
00297         case 5:
00298             strcpy(command,"BRAKE RIGHT MOTOR");
00299             sprintf(subcommand,"");
00300             if(allow_commands) {
00301                 command_status = 1;
00302                 brake_right_motor();
00303             } else command_status = 2;
00304             break;
00305         case 6:
00306             strcpy(command,"BRAKE BOTH MOTORS");
00307             sprintf(subcommand,"");
00308             if(allow_commands) {
00309                 command_status = 1;
00310                 brake();
00311             } else command_status = 2;
00312             break;
00313         case 7:
00314             strcpy(command,"STOP BOTH MOTORS");
00315             sprintf(subcommand,"");
00316             if(allow_commands) {
00317                 command_status = 1;
00318                 stop();
00319             } else command_status = 2;
00320             break;
00321         case 8:
00322             strcpy(command,"TURN ON SPOT");
00323             dec = IF_decode_float(message[1],message[2]);
00324             sprintf(subcommand,"%1.5f",dec);
00325             if(allow_commands) {
00326                 command_status = 1;
00327                 turn(dec);
00328             } else command_status = 2;
00329             break;
00330         case 9:
00331             strcpy(command,"SET EACH MOTOR");
00332             l_dec = IF_decode_float(message[1]);
00333             r_dec = IF_decode_float(message[2]);
00334             sprintf(subcommand,"L=%1.3f R=%1.3f",l_dec,r_dec);
00335             if(allow_commands) {
00336                 command_status = 1;
00337                 set_left_motor_speed(l_dec);
00338                 set_right_motor_speed(r_dec);
00339             } else command_status = 2;
00340             break;
00341             // LED COMMANDS
00342 
00343         case 10:
00344             strcpy(command,"SET LED STATES");
00345             sprintf(subcommand,"G:%s R:%s",IF_char_to_binary_char(message[1]), IF_char_to_binary_char(message[2]));
00346             if(allow_commands) {
00347                 command_status = 1;
00348                 set_leds(message[1],message[2]);
00349             } else command_status = 2;
00350             break;
00351         case 11:
00352             strcpy(command,"SET RED LED STATES");
00353             sprintf(subcommand,"%s",IF_char_to_binary_char(message[1]));
00354             if(allow_commands) {
00355                 command_status = 1;
00356                 set_red_leds(message[1]);
00357             } else command_status = 2;
00358             break;
00359         case 12:
00360             strcpy(command,"SET GREEN LED STATES");
00361             sprintf(subcommand,"%s",IF_char_to_binary_char(message[1]));
00362             if(allow_commands) {
00363                 command_status = 1;
00364                 set_green_leds(message[1]);
00365             } else command_status = 2;
00366             break;
00367         case 13:
00368             strcpy(command,"SET LED");
00369             switch(message[2]) {
00370                 case 1:
00371                     strcpy(colour_string,"RED");
00372                     break;
00373                 case 2:
00374                     strcpy(colour_string,"GREEN");
00375                     break;
00376                 case 3:
00377                     strcpy(colour_string,"BOTH");
00378                     break;
00379                 case 0:
00380                     strcpy(colour_string,"OFF");
00381                     break;
00382             }
00383             if(message[1] < 8 && message[2] < 4) {
00384                 sprintf(subcommand,"%d %s",message[1],colour_string);
00385                 if(allow_commands) {
00386                     command_status = 1;
00387                     set_led(message[1],message[2]);
00388                 } else command_status = 2;
00389             } else {
00390                 sprintf(subcommand,"[INVALID CODE]");
00391                 command_status = 3;
00392             }
00393             break;
00394         case 14:
00395             strcpy(command,"SET CENTER LED STATE");
00396             switch(message[1]) {
00397                 case 1:
00398                     strcpy(colour_string,"RED");
00399                     break;
00400                 case 2:
00401                     strcpy(colour_string,"GREEN");
00402                     break;
00403                 case 3:
00404                     strcpy(colour_string,"BOTH");
00405                     break;
00406                 case 0:
00407                     strcpy(colour_string,"OFF");
00408                     break;
00409             }
00410             if(message[1] < 4) {
00411                 sprintf(subcommand,"%s",colour_string);
00412                 if(allow_commands) {
00413                     command_status = 1;
00414                     set_center_led(message[1]);
00415                 } else command_status = 2;
00416             } else {
00417                 sprintf(subcommand,"[INVALID CODE]");
00418                 command_status = 3;
00419             }
00420             break;
00421         case 15:
00422             strcpy(command,"SET C.LED BRIGHTNESS");
00423             dec = IF_decode_unsigned_float(message[1],message[2]);
00424             sprintf(subcommand,"%1.5f",dec);
00425             if(allow_commands) {
00426                 command_status = 1;
00427                 set_center_led_brightness(dec);
00428             } else command_status = 2;
00429             break;
00430         case 16:
00431             strcpy(command,"SET MBED LEDS");
00432             sprintf(subcommand,"%s",IF_nibble_to_binary_char(message[1]));
00433             if(allow_commands) {
00434                 command_status = 1;
00435                 mbed_led1 = (message[1] & 128) >> 7;
00436                 mbed_led2 = (message[1] & 64) >> 6;
00437                 mbed_led3 = (message[1] & 32) >> 5;
00438                 mbed_led4 = (message[1] & 16) >> 4;
00439             } else command_status = 2;
00440             break;
00441         case 17:
00442             strcpy(command,"BLINK OUTER LEDS");
00443             dec = IF_decode_unsigned_float(message[1],message[2]);
00444             sprintf(subcommand,"FOR %1.5fS",dec);
00445             if(allow_commands) {
00446                 command_status = 1;
00447                 blink_leds(dec);
00448             } else command_status = 2;
00449             break;
00450         case 18:
00451             strcpy(command,"SET BASE LED STATE");
00452             switch(message[1]) {
00453                 case 1:
00454                     strcpy(subcommand,"ON");
00455                     break;
00456                 case 0:
00457                     strcpy(subcommand,"OFF");
00458                     break;
00459             }
00460             //Function not yet implemented
00461             break;
00462         case 19:
00463             strcpy(command,"SET CENTER LED ");
00464             switch(message[1]) {
00465                 case 1:
00466                     strcpy(colour_string,"RED");
00467                     break;
00468                 case 2:
00469                     strcpy(colour_string,"GREEN");
00470                     break;
00471                 case 3:
00472                     strcpy(colour_string,"BOTH");
00473                     break;
00474                 case 0:
00475                     strcpy(colour_string,"OFF");
00476                     break;
00477             }
00478             dec = IF_decode_unsigned_float(message[2]);
00479             sprintf(subcommand,"%s @ %1.5f brightness",colour_string,dec);
00480             if(allow_commands) {
00481                 command_status = 1;
00482                 set_center_led(message[1],dec);
00483             } else command_status = 2;
00484             break;
00485 
00486             // DISPLAY COMMANDS
00487 
00488         case 20:
00489             strcpy(command,"SET DISPLAY ");
00490             switch(message[1]) {
00491                 case 0:
00492                     strcpy(subcommand,"CLEAR");
00493                     if(allow_commands) {
00494                         command_status = 1;
00495                         display.clear_display();
00496                     } else command_status = 2;
00497                     break;
00498                 case 1:
00499                     strcpy(subcommand,"MESSAGE 1");
00500                     if(allow_commands) {
00501                         command_status = 1;
00502                         display.clear_display();
00503                         display.home();
00504                         display.write_string("PC CONNECTION");
00505                         display.set_position(1,0);
00506                         display.write_string("STARTED");
00507                     } else command_status = 2;
00508                     break;
00509                 case 2:
00510                     strcpy(subcommand,"MESSAGE 2");
00511                     if(allow_commands) {
00512                         command_status = 1;
00513                         display.clear_display();
00514                         display.home();
00515                         display.write_string("PC CONNECTION");
00516                         display.set_position(1,0);
00517                         display.write_string("TERMINATED");
00518                     } else command_status = 2;
00519                     break;
00520                 case 3:
00521                     strcpy(subcommand,"MESSAGE 3");
00522                     if(allow_commands) {
00523                         command_status = 1;
00524                         display.clear_display();
00525                         display.home();
00526                         display.write_string("ANDROID DEVICE");
00527                         display.set_position(1,0);
00528                         display.write_string("CONNECTED");
00529                     } else command_status = 2;
00530                     break;
00531                 case 4:
00532                     strcpy(subcommand,"MESSAGE 4");
00533                     if(allow_commands) {
00534                         command_status = 1;
00535                         display.clear_display();
00536                         display.home();
00537                         display.write_string("ANDROID DEVICE");
00538                         display.set_position(1,0);
00539                         display.write_string("DISCONNECTED");
00540                     } else command_status = 2;
00541                     break;
00542                 case 5:
00543                     strcpy(subcommand,"MESSAGE 5");
00544                     if(allow_commands) {
00545                         command_status = 1;
00546                         display.clear_display();
00547                         display.home();
00548                         display.write_string("PSI CONSOLE");
00549                         display.set_position(1,0);
00550                         display.write_string("CONNECTED");
00551                     } else command_status = 2;
00552                     break;
00553                  case 6:
00554                     strcpy(subcommand,"MESSAGE 6");
00555                     if(allow_commands) {
00556                         command_status = 1;
00557                         display.clear_display();
00558                         display.home();
00559                         display.write_string("PSI CONSOLE");
00560                         display.set_position(1,0);
00561                         display.write_string("DISCONNECTED");
00562                     } else command_status = 2;
00563                     break;    
00564             }
00565             break;
00566         case 21:
00567             strcpy(command,"SET CURSOR ");
00568             if(message[1] < 2 && message[2] < 16) {
00569                 sprintf(subcommand,"[%d,%d]",message[1],message[2]);
00570                 if(allow_commands) {
00571                     display.set_position(message[1],message[2]);
00572                 } else command_status = 2;
00573             } else {
00574                 sprintf(subcommand,"[INVALID]");
00575                 command_status = 3;
00576             }
00577             break;
00578         case 22:
00579             strcpy(command,"PRINT CHARACTERS ");
00580             char print_message[2];
00581             print_message[0]=message[1];
00582             print_message[1]=message[2];
00583             sprintf(subcommand,"[%c,%c]",message[1],message[2]);
00584             if(allow_commands) {
00585                 display.write_string(print_message,2);
00586             } else command_status = 2;
00587             break;
00588         case 23:
00589             strcpy(command,"SET DISPLAY B.NESS");
00590             dec = IF_decode_unsigned_float(message[1],message[2]);
00591             sprintf(subcommand,"%1.5f",dec);
00592             if(allow_commands) {
00593                 command_status = 1;
00594                 display.set_backlight_brightness(dec);
00595             } else command_status = 2;
00596             break;
00597 
00598         case 30:
00599             strcpy(command,"SET DEBUG MODE");
00600             switch(message[1]) {
00601                 case 1:
00602                     strcpy(subcommand,"ON");
00603                     break;
00604                 case 0:
00605                     strcpy(subcommand,"OFF");
00606                     break;
00607             }
00608             if(message[2] & 1) strcat (subcommand,"-PC");
00609             if(message[2] & 2) strcat (subcommand,"-BT");
00610             if(message[2] & 4) strcat (subcommand,"-DISP");
00611             if(allow_commands) {
00612                 command_status = 1;
00613                 debug_mode = message[1];
00614                 debug_output = message[2];
00615             } else command_status = 2;
00616             break;
00617         case 31:
00618             strcpy(command,"SET DEMO MODE");
00619             switch(message[1] % 2) {
00620                 case 1:
00621                     strcpy(subcommand,"ON");
00622                     break;
00623                 case 0:
00624                     strcpy(subcommand,"OFF");
00625                     break;
00626             }
00627             if(allow_commands) {
00628                 command_status = 1;
00629                 demo_on = message[1] % 2;
00630                 if(demo_on == 1) {
00631                     user_code_restore_mode = user_code_running;
00632                     user_code_running = 0;
00633                 } else {
00634                     user_code_running = user_code_restore_mode;
00635                 }
00636             } else command_status = 2;
00637             break;
00638         case 32:
00639             strcpy(command,"SET USER CODE");
00640             switch(message[1] % 2) {
00641                 case 1:
00642                     strcpy(subcommand,"ON");
00643                     break;
00644                 case 0:
00645                     strcpy(subcommand,"OFF");
00646                     break;
00647             }
00648             if(allow_commands) {
00649                 command_status = 1;
00650                 user_code_running = message[1] % 2;
00651             } else command_status = 2;
00652             break;
00653         case 33:
00654             strcpy(command,"PAUSE USER CODE");
00655             dec = IF_decode_unsigned_float(message[1],message[2]) * 10;
00656             sprintf(subcommand,"FOR %2.3fS",dec);
00657             if(allow_commands) {
00658                 command_status = 1;
00659                 pause_user_code(dec);
00660             } else command_status = 2;
00661             break;
00662 
00663         case 34:
00664             strcpy(command,"RESET ENCODERS");
00665             if(allow_commands) {
00666                 command_status = 1;
00667                 reset_encoders();
00668             } else command_status = 2;
00669             break;
00670 
00671         case 35:
00672             strcpy(command,"SET ALLOW COMMANDS");
00673             switch(message[1] % 2) {
00674                 case 1:
00675                     strcpy(subcommand,"ON");
00676                     break;
00677                 case 0:
00678                     strcpy(subcommand,"OFF");
00679                     break;
00680             }
00681             allow_commands = message[1] % 2;
00682             command_status = 1;
00683             break;
00684 
00685         case 36:
00686             irp_delay = (message[1] << 8) + message[2];
00687             sprintf(command,"SET IR PULSE DELAY %d MS",irp_delay);
00688             if(allow_commands) {
00689                 command_status = 1;
00690                 ir_pulse_delay = irp_delay;
00691             } else command_status = 2;
00692             break;
00693         case 37:
00694             irp_delay = (message[1] << 8) + message[2];
00695             sprintf(command,"SET BASE IR PULSE DELAY %d MS",irp_delay);
00696             if(allow_commands) {
00697                 command_status = 1;
00698                 base_ir_pulse_delay = irp_delay;
00699             } else command_status = 2;
00700             break;
00701 
00702             // MOTOR REQUESTS
00703         case 40:
00704             strcpy(command,"GET LEFT MOTOR SPEED");
00705             sprintf(ret_message,"%1.5f",motor_left_speed);
00706             send_message = 1;
00707             break;
00708 
00709         case 41:
00710             strcpy(command,"GET RIGHT MOTOR SPEED");
00711             sprintf(ret_message,"%1.5f",motor_right_speed);
00712             send_message = 1;
00713             break;
00714         case 42:
00715             strcpy(command,"GET BRAKE STATES");
00716             sprintf(ret_message,"%d,%d",motor_left_brake,motor_right_brake);
00717             send_message = 1;
00718             break;
00719         case 43:
00720             strcpy(command,"GET MOTOR STATES");
00721             //sprintf(ret_message,"%d,%d",motor_left_brake,motor_right_brake);
00722             send_message = 1;
00723             break;
00724         case 44:
00725             strcpy(command,"GET ENCODERS");
00726             sprintf(ret_message,"%d,%d",left_encoder,right_encoder);
00727             send_message = 1;
00728             break;
00729 
00730             // LED REQUESTS
00731         case 50:
00732             strcpy(command,"GET LED STATES");
00733             sprintf(ret_message,"%04x",get_led_states());
00734             send_message = 1;
00735             break;
00736 
00737             // GENERAL REQUESTS
00738         case 60:
00739             strcpy(command,"GET SOFTWARE VERSION");
00740             sprintf(ret_message,"%1.2f",SOFTWARE_VERSION_CODE);
00741             send_message = 1;
00742             break;
00743 
00744         case 61:
00745             strcpy(command,"GET UPTIME");
00746             sprintf(ret_message,"%6.2f",get_uptime());
00747             send_message = 1;
00748             break;
00749 
00750         case 62:
00751             strcpy(command,"GET ID");
00752             sprintf(ret_message,"%d",robot_id);
00753             send_message = 1;
00754             break;
00755 
00756         case 63:
00757             strcpy(command,"GET SWITCH BYTE");
00758             sprintf(ret_message,"%02x",switch_byte);
00759             send_message = 1;
00760             break;
00761         case 64:
00762             strcpy(command,"GET USER CODE");
00763             sprintf(ret_message,"%d",user_code_running);
00764             send_message = 1;
00765             break;
00766         case 65:
00767             strcpy(command,"GET RESPONSE STRING");
00768             sprintf(ret_message,"PSI");
00769             send_message = 1;
00770             break;
00771         case 66:
00772             strcpy(command,"GET PROGRAM NAME");
00773             sprintf(ret_message,"%s",program_name);
00774             send_message = 1;
00775             break;
00776         case 67:
00777             strcpy(command,"GET AUTHOR NAME");
00778             sprintf(ret_message,"%s",author_name);
00779             send_message = 1;
00780             break;
00781         case 68:
00782             strcpy(command,"GET DEBUG MODE");
00783             sprintf(ret_message,"%1d%1d",debug_mode,debug_output);
00784             send_message = 1;
00785             break;
00786         case 69:
00787             strcpy(command,"GET SYSTEM WARNINGS");
00788             sprintf(ret_message,"%d",system_warnings);
00789             send_message = 1;
00790             break;
00791 
00792 
00793             // Sensors
00794         case 80:
00795             strcpy(command,"STORE BG. IR VALUES");
00796             if(allow_commands) {
00797                 command_status = 1;
00798                 store_background_raw_ir_values();
00799             } else command_status = 2;
00800             break;
00801         case 81:
00802             strcpy(command,"STORE IL. IR VALUES");
00803             if(allow_commands) {
00804                 command_status = 1;
00805                 store_illuminated_raw_ir_values();
00806             } else command_status = 2;
00807             break;
00808         case 82:
00809             strcpy(command,"STORE IR VALUES");
00810             if(allow_commands) {
00811                 command_status = 1;
00812                 store_ir_values();
00813             } else command_status = 2;
00814             break;
00815         case 83:
00816             strcpy(command,"STORE BG BASE IR VALUES");
00817             if(allow_commands) {
00818                 command_status = 1;
00819                 store_background_base_ir_values();
00820             } else command_status = 2;
00821             break;
00822         case 84:
00823             strcpy(command,"STORE IL. BASE IR VALUES");
00824             if(allow_commands) {
00825                 command_status = 1;
00826                 store_illuminated_base_ir_values();
00827             } else command_status = 2;
00828             break;
00829         case 85:
00830             strcpy(command,"STORE BASE IR VALUES");
00831             if(allow_commands) {
00832                 command_status = 1;
00833                 store_base_ir_values();
00834             } else command_status = 2;
00835             break;
00836         case 86:
00837             strcpy(command,"STORE ALL IR VALUES");
00838             if(allow_commands) {
00839                 command_status = 1;
00840                 store_ir_values();
00841                 store_base_ir_values();
00842             } else command_status = 2;
00843             break;
00844         case 90:
00845             sprintf(command,"%s %d","GET BG IR VALUE",message[1]);
00846             sprintf(ret_message,"%d",get_background_raw_ir_value(message[1]));
00847             send_message = 1;
00848             break;
00849         case 91:
00850             sprintf(command,"%s %d","GET IL IR VALUE",message[1]);
00851             sprintf(ret_message,"%d",get_illuminated_raw_ir_value(message[1]));
00852             send_message = 1;
00853             break;
00854         case 92:
00855             strcpy(command,"GET BG IR VALUES");
00856             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));
00857             send_message = 1;
00858             break;
00859         case 93:
00860             strcpy(command,"GET ILLUMINATED IR VALUES");
00861             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));
00862             send_message = 1;
00863             break;
00864         case 94:
00865             sprintf(command,"%s %d","GET BG BASE IR VALUE",message[1]);
00866             sprintf(ret_message,"%d",get_background_base_ir_value(message[1]));
00867             send_message = 1;
00868             break;
00869         case 95:
00870             sprintf(command,"%s %d","GET IL BASE IR VALUE",message[1]);
00871             sprintf(ret_message,"%d",get_illuminated_base_ir_value(message[1]));
00872             send_message = 1;
00873             break;
00874         case 96:
00875             strcpy(command,"GET BG BASE IR VALUES");
00876             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));
00877             send_message = 1;
00878             break;
00879         case 97:
00880             strcpy(command,"GET IL BASE IR VALUES");
00881             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));
00882             send_message = 1;
00883             break;
00884         case 98:
00885             strcpy(command,"CALCULATE BASE IR VALUES");
00886             sprintf(ret_message,"%03X%03X%03X%03X%03X",calculate_base_ir_value(0),calculate_base_ir_value(1),calculate_base_ir_value(2),calculate_base_ir_value(3),calculate_base_ir_value(4));
00887             send_message = 1;
00888             break;
00889         case 99:
00890             strcpy(command,"CALCULATE SIDE IR VALUES");
00891             sprintf(ret_message,"%03X%03X%03X%03X%03X%03X%03X%03X",calculate_side_ir_value(0),calculate_side_ir_value(1),calculate_side_ir_value(2),calculate_side_ir_value(3),calculate_side_ir_value(4),calculate_side_ir_value(5),calculate_side_ir_value(6),calculate_side_ir_value(7));
00892             send_message = 1;
00893             break;
00894         case 100:
00895             strcpy(command,"START FILE TRANSFER MODE");
00896             if(allow_commands) {
00897                 command_status = 1;
00898                 IF_start_file_transfer_mode();
00899                 sprintf(ret_message,"OK");
00900                 send_message = 1;
00901             } else command_status = 2;
00902             break;
00903         case 110:
00904             strcpy(command,"GET FIRMWARE VERSION");
00905             sprintf(ret_message,"%1.2f",firmware_version);
00906             send_message = 1;
00907             break;
00908         case 111:
00909             strcpy(command,"GET SERIAL NUMBER");
00910             sprintf(ret_message,"%2.3f",serial_number);
00911             send_message = 1;
00912             break;
00913         case 112:
00914             strcpy(command,"GET HAS SIDE IR");
00915             sprintf(ret_message,"%d",has_side_ir);
00916             send_message = 1;
00917             break;
00918         case 113:
00919             strcpy(command,"GET HAS BASE IR");
00920             sprintf(ret_message,"%d",has_base_ir);
00921             send_message = 1;
00922             break;
00923         case 114:
00924             strcpy(command,"GET HAS ENCODERS");
00925             sprintf(ret_message,"%d",has_wheel_encoders);
00926             send_message = 1;
00927             break;
00928         case 115:
00929             strcpy(command,"GET HAS AUDIO");
00930             sprintf(ret_message,"%d",has_audio_pic);
00931             send_message = 1;
00932             break;
00933         case 116:
00934             strcpy(command,"GET HAS RECHARGING");
00935             sprintf(ret_message,"%d",has_recharging_circuit);
00936             send_message = 1;
00937             break;
00938         case 117:
00939             strcpy(command,"GET HAS COMPASS");
00940             sprintf(ret_message,"%d",has_compass);
00941             send_message = 1;
00942             break;
00943         case 118:
00944             strcpy(command,"GET HAS ULTRASONIC");
00945             sprintf(ret_message,"%d",has_ultrasonic_sensor);
00946             send_message = 1;
00947             break;
00948         case 119:
00949             strcpy(command,"GET HAS TEMPERATURE");
00950             sprintf(ret_message,"%d",has_temperature_sensor);
00951             send_message = 1;
00952             break;
00953         case 120:
00954             strcpy(command,"GET HAS BASE COLOUR");
00955             sprintf(ret_message,"%d",has_base_colour_sensor);
00956             send_message = 1;
00957             break;
00958         case 121:
00959             strcpy(command,"GET HAS TOP COLOUR");
00960             sprintf(ret_message,"%d",has_top_colour_sensor);
00961             send_message = 1;
00962             break;
00963         case 122:
00964             strcpy(command,"GET HAS RADIO");
00965             sprintf(ret_message,"%d",has_433_radio);
00966             send_message = 1;
00967             break;
00968         case 123:
00969             strcpy(command,"GET FIRMWARE H-DESC");
00970             char byte0 = 0;
00971             char byte1 = 1;
00972             if(has_side_ir)byte0+=128;
00973             if(has_base_ir)byte0+=64;
00974             if(has_wheel_encoders)byte0+=32;
00975             if(has_audio_pic)byte0+=16;
00976             if(has_recharging_circuit)byte0+=8;
00977             if(has_compass)byte0+=4;
00978             if(has_ultrasonic_sensor)byte0+=2;
00979             if(has_temperature_sensor)byte0+=1;
00980             if(has_base_colour_sensor)byte1+=128;
00981             if(has_top_colour_sensor)byte1+=64;
00982             if(has_433_radio)byte1+=32;
00983             sprintf(ret_message,"%c%c",byte0,byte1);
00984             send_message = 1;
00985             break;
00986         case 124:
00987             strcpy(command,"GET PCB VERSION");
00988             sprintf(ret_message,"%1.2f",pcb_version);
00989             send_message = 1;
00990             break;
00991     }
00992 
00993 
00994     if(send_message) {
00995         char message_length = strlen(ret_message);
00996         switch(interface) {
00997             case 0:
00998                 pc.printf("%c%c%s",RESPONSE_MESSAGE_BYTE,message_length,ret_message);
00999                 break;
01000             case 1:
01001                 bt.printf("%c%c%s",RESPONSE_MESSAGE_BYTE,message_length,ret_message);
01002                 break;
01003         }
01004         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);
01005     } else {
01006         switch(interface) {
01007             case 0:
01008                 pc.printf("%c%c",ACKNOWLEDGE_MESSAGE_BYTE,command_status);
01009                 break;
01010             case 1:
01011                 bt.printf("%c%c",ACKNOWLEDGE_MESSAGE_BYTE,command_status);
01012                 break;
01013         }
01014         switch(command_status) {
01015             case 0:
01016                 debug("Unrecognised %s command message [%02x%02x%02x]\n",iface,message[0],message[1],message[2]);
01017                 break;
01018             case 1:
01019                 debug("Actioned %s command message:%s %s [%02x%02x%02x]\n",iface, command, subcommand,message[0],message[1],message[2]);
01020                 break;
01021             case 2:
01022                 debug("Blocked %s command message:%s %s [%02x%02x%02x]\n",iface, command, subcommand,message[0],message[1],message[2]);
01023                 break;
01024             case 3:
01025                 debug("Invalid %s command message:%s %s [%02x%02x%02x]\n",iface, command, subcommand,message[0],message[1],message[2]);
01026                 break;
01027         }
01028     }
01029 }
01030 
01031 char * IF_nibble_to_binary_char(char in)
01032 {
01033     char * ret = (char*)malloc(sizeof(char)*5);
01034     for(int i=0; i<4; i++) {
01035         if(in & (128 >> i)) ret[i]='1';
01036         else ret[i]='0';
01037     }
01038     ret[4]=0;
01039     return ret;
01040 }
01041 
01042 char * IF_char_to_binary_char(char in)
01043 {
01044     char * ret = (char*)malloc(sizeof(char)*9);
01045     for(int i=0; i<8; i++) {
01046         if(in & (128 >> i)) ret[i]='1';
01047         else ret[i]='0';
01048     }
01049     ret[8]=0;
01050     return ret;
01051 }
01052 
01053 float IF_decode_unsigned_float(char byte0, char byte1)
01054 {
01055     unsigned short sval = (byte0) << 8;
01056     sval += byte1;
01057     float scaled = sval / 65535.0f;
01058     return scaled;
01059 }
01060 
01061 float IF_decode_float(char byte0, char byte1)
01062 {
01063     // MSB is byte 0 is sign, rest is linear spread between 0 and 1
01064     char sign = byte0 / 128;
01065     short sval = (byte0 % 128) << 8;
01066     sval += byte1;
01067     float scaled = sval / 32767.0f;
01068     if(sign == 0) scaled = 0-scaled;
01069     return scaled;
01070 }
01071 
01072 float IF_decode_unsigned_float(char byte0)
01073 {
01074     unsigned short sval = (byte0);
01075     float scaled = sval / 255.0f;
01076     return scaled;
01077 }
01078 
01079 float IF_decode_float(char byte0)
01080 {
01081     // MSB is byte 0 is sign, rest is linear spread between 0 and 1
01082     char sign = byte0 / 128;
01083     short sval = (byte0 % 128);
01084     float scaled = sval / 127.0f;
01085     if(sign == 0) scaled = 0-scaled;
01086     return scaled;
01087 }
01088 
01089 void IF_setup_serial_interfaces()
01090 {
01091     if(ENABLE_PC_SERIAL) {
01092         pc.baud(PC_BAUD);
01093         pc.attach(&IF_pc_rx_callback, Serial::RxIrq);
01094     }
01095     if(ENABLE_BLUETOOTH) {
01096         bt.baud(BLUETOOTH_BAUD);
01097         bt.attach(&IF_bt_rx_callback, Serial::RxIrq);
01098     }
01099 }
01100 
01101 void IF_pc_rx_command_timeout()
01102 {
01103     char message_array[6];
01104     char length = 1 + pc_command_message_byte;
01105     pc_command_message_started = 0;
01106     message_array[0] = COMMAND_MESSAGE_BYTE;
01107     for(int k=0; k<pc_command_message_byte; k++) {
01108         message_array[k+1] = pc_command_message[k];
01109     }
01110     IF_handle_user_serial_message(message_array, length, 0);
01111 }
01112 
01113 void IF_bt_rx_command_timeout()
01114 {
01115     char message_array[6];
01116     char length = 1 + bt_command_message_byte;
01117     bt_command_message_started = 0;
01118     message_array[0] = COMMAND_MESSAGE_BYTE;
01119     for(int k=0; k<bt_command_message_byte; k++) {
01120         message_array[k+1] = bt_command_message[k];
01121     }
01122     IF_handle_user_serial_message(message_array, length, 1);
01123 }
01124 
01125 void IF_pc_rx_callback()
01126 {
01127     int count = 0;
01128     char message_array[255];
01129 
01130     while(pc.readable()) {
01131         char tc = pc.getc();
01132         message_array[count] = tc;
01133         count ++;
01134         if(pc_command_message_started == 1) {
01135             if(pc_command_message_byte == 3) {
01136                 pc_command_timeout.detach();
01137                 if(tc == COMMAND_MESSAGE_BYTE) {
01138                     // A complete command message succesfully received, call handler
01139                     pc_command_message_started = 0;
01140                     count = 0;
01141                     IF_handle_command_serial_message(pc_command_message , 0);
01142                 } else {
01143                     // Message is not a valid command message as 5th byte is not correct; treat whole message as a user message
01144                     pc_command_message_started = 0;
01145                     message_array[0] = COMMAND_MESSAGE_BYTE;
01146                     message_array[1] = pc_command_message[0];
01147                     message_array[2] = pc_command_message[1];
01148                     message_array[3] = pc_command_message[2];
01149                     message_array[4] = tc;
01150                     count = 5;
01151                 }
01152             } else {
01153                 pc_command_message[pc_command_message_byte] = tc;
01154                 pc_command_message_byte ++;
01155             }
01156         } else {
01157             if(count == 1) {
01158                 if(tc == COMMAND_MESSAGE_BYTE) {
01159                     pc_command_timeout.attach(&IF_pc_rx_command_timeout,command_timeout_period);
01160                     pc_command_message_started = 1;
01161                     pc_command_message_byte = 0;
01162 
01163                 }
01164             }
01165         }
01166     }
01167     if(!pc_command_message_started && count>0) IF_handle_user_serial_message(message_array, count, 0);
01168 }
01169 
01170 Timeout bt_message_timeout;
01171 static float bt_message_timeout_period = 0.001; // 1 millisecond
01172 char bt_buffer[255];
01173 int bt_buffer_index = 0;
01174 
01175 void IF_bt_message_timeout()
01176 {
01177     char buffer[255];
01178 
01179     sprintf(buffer, bt_buffer, bt_buffer_index);
01180     buffer[bt_buffer_index] = 0;
01181     if(file_transfer_mode == 1) {
01182         IF_handle_file_transfer_serial_message(bt_buffer, bt_buffer_index, 1);
01183     } else {
01184 //    debug("BT message timeout: %s [%d chars]\n", buffer, bt_buffer_index);
01185         if(bt_buffer_index == 5 && buffer[0] == COMMAND_MESSAGE_BYTE && buffer[4] == COMMAND_MESSAGE_BYTE) {
01186             bt_command_message[0] = buffer[1];
01187             bt_command_message[1] = buffer[2];
01188             bt_command_message[2] = buffer[3];
01189             IF_handle_command_serial_message(bt_command_message , 1);
01190         } else IF_handle_user_serial_message(bt_buffer, bt_buffer_index, 1);
01191     }
01192     bt_buffer_index = 0;
01193 }
01194 
01195 
01196 //void IF_bt_rx_callback()
01197 //{
01198 //    while(bt.readable()) {
01199 //        char byte = bt.getc();
01200 //
01201 //        bt_buffer[bt_buffer_index] = byte;
01202 //        bt_buffer_index++;
01203 //    }
01204 //
01205 //    bt_message_timeout.attach(&IF_bt_message_timeout, bt_message_timeout_period);
01206 //}
01207 
01208 
01209 void IF_set_filename(char * filename_in){
01210     strcpy(filename,filename_in);
01211 }
01212  
01213 unsigned short IF_calculateCRC16(int file_length){
01214     unsigned short crc16table[256] = { 
01215         0x0000, 0x8005, 0x800F, 0x000A, 0x801B, 0x001E, 0x0014, 0x8011,
01216         0x8033, 0x0036, 0x003C, 0x8039, 0x0028, 0x802D, 0x8027, 0x0022,
01217         0x8063, 0x0066, 0x006C, 0x8069, 0x0078, 0x807D, 0x8077, 0x0072,
01218         0x0050, 0x8055, 0x805F, 0x005A, 0x804B, 0x004E, 0x0044, 0x8041,
01219         0x80C3, 0x00C6, 0x00CC, 0x80C9, 0x00D8, 0x80DD, 0x80D7, 0x00D2,
01220         0x00F0, 0x80F5, 0x80FF, 0x00FA, 0x80EB, 0x00EE, 0x00E4, 0x80E1,
01221         0x00A0, 0x80A5, 0x80AF, 0x00AA, 0x80BB, 0x00BE, 0x00B4, 0x80B1,
01222         0x8093, 0x0096, 0x009C, 0x8099, 0x0088, 0x808D, 0x8087, 0x0082,
01223         0x8183, 0x0186, 0x018C, 0x8189, 0x0198, 0x819D, 0x8197, 0x0192,
01224         0x01B0, 0x81B5, 0x81BF, 0x01BA, 0x81AB, 0x01AE, 0x01A4, 0x81A1,
01225         0x01E0, 0x81E5, 0x81EF, 0x01EA, 0x81FB, 0x01FE, 0x01F4, 0x81F1,
01226         0x81D3, 0x01D6, 0x01DC, 0x81D9, 0x01C8, 0x81CD, 0x81C7, 0x01C2,
01227         0x0140, 0x8145, 0x814F, 0x014A, 0x815B, 0x015E, 0x0154, 0x8151,
01228         0x8173, 0x0176, 0x017C, 0x8179, 0x0168, 0x816D, 0x8167, 0x0162,
01229         0x8123, 0x0126, 0x012C, 0x8129, 0x0138, 0x813D, 0x8137, 0x0132,
01230         0x0110, 0x8115, 0x811F, 0x011A, 0x810B, 0x010E, 0x0104, 0x8101, 
01231         0x8303, 0x0306, 0x030C, 0x8309, 0x0318, 0x831D, 0x8317, 0x0312,
01232         0x0330, 0x8335, 0x833F, 0x033A, 0x832B, 0x032E, 0x0324, 0x8321,
01233         0x0360, 0x8365, 0x836F, 0x036A, 0x837B, 0x037E, 0x0374, 0x8371,
01234         0x8353, 0x0356, 0x035C, 0x8359, 0x0348, 0x834D, 0x8347, 0x0342,
01235         0x03C0, 0x83C5, 0x83CF, 0x03CA, 0x83DB, 0x03DE, 0x03D4, 0x83D1,
01236         0x83F3, 0x03F6, 0x03FC, 0x83F9, 0x03E8, 0x83ED, 0x83E7, 0x03E2,
01237         0x83A3, 0x03A6, 0x03AC, 0x83A9, 0x03B8, 0x83BD, 0x83B7, 0x03B2,
01238         0x0390, 0x8395, 0x839F, 0x039A, 0x838B, 0x038E, 0x0384, 0x8381,
01239         0x0280, 0x8285, 0x828F, 0x028A, 0x829B, 0x029E, 0x0294, 0x8291,
01240         0x82B3, 0x02B6, 0x02BC, 0x82B9, 0x02A8, 0x82AD, 0x82A7, 0x02A2,
01241         0x82E3, 0x02E6, 0x02EC, 0x82E9, 0x02F8, 0x82FD, 0x82F7, 0x02F2,
01242         0x02D0, 0x82D5, 0x82DF, 0x02DA, 0x82CB, 0x02CE, 0x02C4, 0x82C1,
01243         0x8243, 0x0246, 0x024C, 0x8249, 0x0258, 0x825D, 0x8257, 0x0252,
01244         0x0270, 0x8275, 0x827F, 0x027A, 0x826B, 0x026E, 0x0264, 0x8261,
01245         0x0220, 0x8225, 0x822F, 0x022A, 0x823B, 0x023E, 0x0234, 0x8231,
01246         0x8213, 0x0216, 0x021C, 0x8219, 0x0208, 0x820D, 0x8207, 0x0202};
01247         
01248     //Opens, reads and calculates the CRC16 value for file pointed to by filename
01249     unsigned short crc_value = 0;
01250     FILE *fp = fopen(filename,"r");
01251     char * buffer;
01252     int limit = 512;
01253     if(file_length < 512) limit = file_length;
01254     buffer = (char*) malloc (sizeof(char)*limit);
01255     int blocks = 1;
01256     if(file_length > limit) blocks += file_length / limit;
01257     for(int i=0;i<blocks;i++){
01258         //Determine size of this block
01259         int blocksize = limit;
01260         if(i == blocks-1){
01261             if((file_length % limit) != 0) blocksize = file_length % limit;   
01262         }               
01263         debug("Calculating %d bytes of CRC data...\n",blocksize);
01264         int result;
01265         result = fread(buffer,1,blocksize,fp);
01266         debug("Data read: %d\n",result);
01267         for(int j=0;j<blocksize;j++){
01268             int subindex = ((crc_value>>8)^*(char *)(buffer[j]))&0x00FF;
01269             //debug("J:%d Subindex:%d\n",j,subindex);
01270             unsigned short table_value = crc16table[subindex];
01271             crc_value=(crc_value<<8)^table_value;   
01272         }
01273     }
01274     fclose(fp);
01275     debug("CRC Calculated: %x\n",crc_value);
01276     return crc_value;               
01277 }
01278 
01279 void IF_bt_rx_callback()
01280 {
01281     int count = 0;
01282     char message_array[255];
01283 
01284     wait_ms(500); // Wait 0.5ms to allow a complete message to arrive before atttempting to process it
01285 
01286     while(bt.readable()) {
01287         char tc = bt.getc();
01288         message_array[count] = tc;
01289         count ++;
01290         if(bt_command_message_started == 1) {
01291             if(bt_command_message_byte == 3) {
01292                 bt_command_timeout.detach();
01293                 if(tc == COMMAND_MESSAGE_BYTE) {
01294                     // A complete command message succesfully received, call handler
01295                     bt_command_message_started = 0;
01296                     count = 0;
01297                     IF_handle_command_serial_message(bt_command_message , 1);
01298                 } else {
01299                     // Message is not a valid command message as 5th byte is not correct; treat whole message as a user message
01300                     bt_command_message_started = 0;
01301                     message_array[0] = COMMAND_MESSAGE_BYTE;
01302                     message_array[1] = bt_command_message[0];
01303                     message_array[2] = bt_command_message[1];
01304                     message_array[3] = bt_command_message[2];
01305                     message_array[4] = tc;
01306                     count = 5;
01307                 }
01308             } else {
01309                 bt_command_timeout.attach(&IF_bt_rx_command_timeout,command_timeout_period);
01310                 bt_command_message[bt_command_message_byte] = tc;
01311                 bt_command_message_byte ++;
01312             }
01313         } else {
01314             if(count == 1) {
01315                 if(tc == COMMAND_MESSAGE_BYTE) {
01316                     bt_command_message_started = 1;
01317                     bt_command_message_byte = 0;
01318 
01319                 }
01320             }
01321         }
01322     }
01323     if(!bt_command_message_started && count>0) IF_handle_user_serial_message(message_array, count, 1);
01324 }