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