Psi Swarm Robot / PsiSwarmV8_CPP

C++ Library for the PsiSwarm Robot - Version 0.8

Dependents:   PsiSwarm_V8_Blank_CPP Autonomia_RndmWlk

Fork of PsiSwarmV7_CPP by Psi Swarm Robot

serial.cpp@1:060690a934a9, 2016-03-03 (annotated)

Committer:
jah128
Date:
Thu Mar 03 23:21:47 2016 +0000
Revision:
1:060690a934a9
Parent:
0:d6269d17c8cf
Child:
2:c6986ee3c7c5
Updated for web page;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
jah128 0:d6269d17c8cf 1 /* University of York Robotics Laboratory PsiSwarm Library: Serial Control Source File
jah128 0:d6269d17c8cf 2 *
jah128 0:d6269d17c8cf 3 * File: serial.cpp
jah128 0:d6269d17c8cf 4 *
jah128 0:d6269d17c8cf 5 * (C) Dept. Electronics & Computer Science, University of York
jah128 0:d6269d17c8cf 6 * James Hilder, Alan Millard, Alexander Horsfield, Homero Elizondo, Jon Timmis
jah128 0:d6269d17c8cf 7 *
jah128 0:d6269d17c8cf 8 * PsiSwarm Library Version: 0.4
jah128 0:d6269d17c8cf 9 *
jah128 0:d6269d17c8cf 10 * February 2016
jah128 0:d6269d17c8cf 11 *
jah128 0:d6269d17c8cf 12 *
jah128 0:d6269d17c8cf 13 */
jah128 0:d6269d17c8cf 14
jah128 0:d6269d17c8cf 15 #include "psiswarm.h"
jah128 0:d6269d17c8cf 16
jah128 0:d6269d17c8cf 17 static float command_timeout_period = 0.1f; //If a complete command message is not received in 0.1s then consider it a user message
jah128 0:d6269d17c8cf 18 char pc_command_message_started = 0;
jah128 0:d6269d17c8cf 19 char pc_command_message_byte = 0;
jah128 0:d6269d17c8cf 20 char pc_command_message[3];
jah128 0:d6269d17c8cf 21 char bt_command_message_started = 0;
jah128 0:d6269d17c8cf 22 char bt_command_message_byte = 0;
jah128 0:d6269d17c8cf 23 char bt_command_message[3];
jah128 0:d6269d17c8cf 24
jah128 0:d6269d17c8cf 25 char allow_commands = 1;
jah128 0:d6269d17c8cf 26 char allow_requests = 1;
jah128 0:d6269d17c8cf 27
jah128 1:060690a934a9 28 char file_transfer_state = 0;
jah128 1:060690a934a9 29 int file_length;
jah128 1:060690a934a9 30 char filename [13];
jah128 1:060690a934a9 31
jah128 1:060690a934a9 32
jah128 1:060690a934a9 33 Timeout ft_timeout;
jah128 0:d6269d17c8cf 34 Timeout pc_command_timeout;
jah128 0:d6269d17c8cf 35 Timeout bt_command_timeout;
jah128 0:d6269d17c8cf 36
jah128 0:d6269d17c8cf 37 // A predefined message structure for command messages is as follows:
jah128 0:d6269d17c8cf 38 // [Byte 0][Byte 1][Byte 2][Byte 3][Byte 4]
jah128 0:d6269d17c8cf 39 // Byte 0 and Byte 4 must be equal to COMMAND_MESSAGE_BYTE [in psiswarm.h] or message is treated as a user message
jah128 0:d6269d17c8cf 40
jah128 0:d6269d17c8cf 41
jah128 0:d6269d17c8cf 42
jah128 0:d6269d17c8cf 43 void handle_user_serial_message(char * message, char length, char interface)
jah128 0:d6269d17c8cf 44 {
jah128 0:d6269d17c8cf 45 // This is where user code for handling a (non-system) serial message should go
jah128 0:d6269d17c8cf 46 //
jah128 0:d6269d17c8cf 47 // message = pointer to message char array
jah128 0:d6269d17c8cf 48 // length = length of message
jah128 0:d6269d17c8cf 49 // interface = 0 for PC serial connection, 1 for Bluetooth
jah128 0:d6269d17c8cf 50
jah128 0:d6269d17c8cf 51 if(interface) {
jah128 0:d6269d17c8cf 52 if(length == 8) {
jah128 0:d6269d17c8cf 53 for(int i = 0; i < length; i++) {
jah128 0:d6269d17c8cf 54 // Convert single byte value into a beacon heading in the range +/-180 degrees
jah128 0:d6269d17c8cf 55 float beacon_heading = message[i];
jah128 0:d6269d17c8cf 56 float degrees_per_value = 256.0f / 360.0f;
jah128 0:d6269d17c8cf 57
jah128 0:d6269d17c8cf 58 if(beacon_heading != 0)
jah128 0:d6269d17c8cf 59 beacon_heading /= degrees_per_value;
jah128 0:d6269d17c8cf 60
jah128 0:d6269d17c8cf 61 beacon_heading -= 180;
jah128 0:d6269d17c8cf 62
jah128 0:d6269d17c8cf 63 flocking_headings[i] = beacon_heading;
jah128 0:d6269d17c8cf 64
jah128 0:d6269d17c8cf 65 debug("%d, ", flocking_headings[i]);
jah128 0:d6269d17c8cf 66 //debug("%f, ", beacon_heading);
jah128 0:d6269d17c8cf 67 }
jah128 0:d6269d17c8cf 68
jah128 0:d6269d17c8cf 69 debug("\n");
jah128 0:d6269d17c8cf 70 }
jah128 0:d6269d17c8cf 71 }
jah128 0:d6269d17c8cf 72 }
jah128 0:d6269d17c8cf 73
jah128 1:060690a934a9 74 void IF_start_file_transfer_mode()
jah128 1:060690a934a9 75 {
jah128 1:060690a934a9 76 display.clear_display();
jah128 1:060690a934a9 77 display.set_position(0,0);
jah128 1:060690a934a9 78 display.write_string("FILE TRANSFER");
jah128 1:060690a934a9 79 display.set_position(1,0);
jah128 1:060690a934a9 80 display.write_string("MODE...");
jah128 1:060690a934a9 81 file_transfer_mode = 1;
jah128 1:060690a934a9 82 file_transfer_state = 0;
jah128 1:060690a934a9 83 file_length = 0;
jah128 1:060690a934a9 84 user_code_restore_mode = user_code_running;
jah128 1:060690a934a9 85 user_code_running = 0;
jah128 1:060690a934a9 86 ft_timeout.attach(IF_file_transfer_timeout,2.0);
jah128 1:060690a934a9 87 }
jah128 1:060690a934a9 88
jah128 1:060690a934a9 89
jah128 1:060690a934a9 90 void IF_invalid_transfer(void)
jah128 1:060690a934a9 91 {
jah128 1:060690a934a9 92 debug("File transfer failed\n");
jah128 1:060690a934a9 93 display.clear_display();
jah128 1:060690a934a9 94 display.set_position(0,0);
jah128 1:060690a934a9 95 display.write_string("TRANSFER FAILED");
jah128 1:060690a934a9 96 wait(0.5);
jah128 1:060690a934a9 97 IF_end_file_transfer_mode();
jah128 1:060690a934a9 98 }
jah128 1:060690a934a9 99
jah128 1:060690a934a9 100 void IF_file_transfer_timeout(void)
jah128 1:060690a934a9 101 {
jah128 1:060690a934a9 102 debug("File transfer failed: timeout\n");
jah128 1:060690a934a9 103 display.clear_display();
jah128 1:060690a934a9 104 display.set_position(0,0);
jah128 1:060690a934a9 105 display.write_string("TRANSFER TIMEOUT");
jah128 1:060690a934a9 106 wait(0.5);
jah128 1:060690a934a9 107 IF_end_file_transfer_mode();
jah128 1:060690a934a9 108 }
jah128 1:060690a934a9 109
jah128 1:060690a934a9 110 void IF_end_file_transfer_mode(void)
jah128 1:060690a934a9 111 {
jah128 1:060690a934a9 112 display.clear_display();
jah128 1:060690a934a9 113 file_transfer_mode = 0;
jah128 1:060690a934a9 114 user_code_running = user_code_restore_mode;
jah128 1:060690a934a9 115 }
jah128 1:060690a934a9 116
jah128 0:d6269d17c8cf 117
jah128 0:d6269d17c8cf 118 void IF_handle_file_transfer_serial_message(char * message, char length, char interface)
jah128 0:d6269d17c8cf 119 {
jah128 0:d6269d17c8cf 120 // Code for handling a serial (Bluetooth) message when in file-transfer mode
jah128 0:d6269d17c8cf 121 //
jah128 0:d6269d17c8cf 122 // message = pointer to message char array
jah128 0:d6269d17c8cf 123 // length = length of message
jah128 0:d6269d17c8cf 124 // interface = 0 for PC serial connection, 1 for Bluetooth [NB only Bluetooth used for file transfer in this version]
jah128 0:d6269d17c8cf 125
jah128 1:060690a934a9 126 debug("FTM Message:%.*s [%d]\n",length,message,length);
jah128 1:060690a934a9 127 char * file_data;
jah128 1:060690a934a9 128 int block_index;
jah128 1:060690a934a9 129 int final_block;
jah128 1:060690a934a9 130 int expected_size;
jah128 1:060690a934a9 131 int block_size = 100;
jah128 1:060690a934a9 132 // The first byte in EVERY message received should be 33; if it isn't, abort the transfer
jah128 1:060690a934a9 133 if(message[0] != 33) {
jah128 1:060690a934a9 134 IF_invalid_transfer();
jah128 1:060690a934a9 135 } else {
jah128 1:060690a934a9 136 switch(file_transfer_state) {
jah128 1:060690a934a9 137 case 0: //First message received is the target filename
jah128 1:060690a934a9 138 //The filenames cannot be more that 8.3 characters long (FAT12 format)
jah128 1:060690a934a9 139 if(length == 1 || length > 13) IF_invalid_transfer();
jah128 1:060690a934a9 140 else {
jah128 1:060690a934a9 141 strncpy(filename, message + 1, length - 1);
jah128 1:060690a934a9 142 //Send acknowledge ("FN")
jah128 1:060690a934a9 143 ft_timeout.detach();
jah128 1:060690a934a9 144 ft_timeout.attach(IF_file_transfer_timeout,2.0);
jah128 1:060690a934a9 145 bt.printf("%c%c%s",RESPONSE_MESSAGE_BYTE,2,"FN");
jah128 1:060690a934a9 146 file_transfer_state = 1;
jah128 1:060690a934a9 147 }
jah128 1:060690a934a9 148 break;
jah128 1:060690a934a9 149 case 1: //Second message is the length of the file in bytes
jah128 1:060690a934a9 150 //Length is encoded as a 3-byte value
jah128 1:060690a934a9 151 if(length != 4) IF_invalid_transfer();
jah128 1:060690a934a9 152 else {
jah128 1:060690a934a9 153 file_length = (message[1]) * 256;
jah128 1:060690a934a9 154 file_length += (message[2]);
jah128 1:060690a934a9 155 file_length *= 256;
jah128 1:060690a934a9 156 file_length += message[3];
jah128 1:060690a934a9 157 file_transfer_state = 2;
jah128 1:060690a934a9 158 display.clear_display();
jah128 1:060690a934a9 159 char display_message[17];
jah128 1:060690a934a9 160 sprintf(display_message,"F:%s",filename);
jah128 1:060690a934a9 161 display.set_position(0,0);
jah128 1:060690a934a9 162 display.write_string(display_message);
jah128 1:060690a934a9 163 display.set_position(1,0);
jah128 1:060690a934a9 164 sprintf(display_message,"S:%d b",file_length);
jah128 1:060690a934a9 165 display.write_string(display_message);
jah128 1:060690a934a9 166 block_index = 0;
jah128 1:060690a934a9 167 //Work out how many blocks the file will be sent in (size = block_size, tested at 100 bytes)
jah128 1:060690a934a9 168 //Allocate memory for the file up to a limit of 16 blocks; larger files will be split across
jah128 1:060690a934a9 169 //multiple blocks....
jah128 1:060690a934a9 170 final_block = file_length / block_size;
jah128 1:060690a934a9 171 if(file_length % block_size != 0) final_block ++;
jah128 1:060690a934a9 172 int target_size = file_length;
jah128 1:060690a934a9 173 if(file_length > (block_size * 16)) target_size = block_size * 16;
jah128 1:060690a934a9 174 file_data = (char *) malloc(target_size);
jah128 1:060690a934a9 175 ft_timeout.detach();
jah128 1:060690a934a9 176 ft_timeout.attach(IF_file_transfer_timeout,1.0);
jah128 1:060690a934a9 177 //Send acknowledge (size of file)
jah128 1:060690a934a9 178 bt.printf("%c%c%c%c%c",RESPONSE_MESSAGE_BYTE,3,message[1],message[2],message[3]);
jah128 1:060690a934a9 179 }
jah128 1:060690a934a9 180 break;
jah128 1:060690a934a9 181 case 2:
jah128 1:060690a934a9 182 block_index ++;
jah128 1:060690a934a9 183 if(block_index == final_block) expected_size = file_length % block_size;
jah128 1:060690a934a9 184 if(expected_size == 0) expected_size = block_size;
jah128 1:060690a934a9 185 if(length!=expected_size + 1){
jah128 1:060690a934a9 186 // Unexpected length
jah128 1:060690a934a9 187
jah128 1:060690a934a9 188 }else{
jah128 1:060690a934a9 189
jah128 1:060690a934a9 190 ft_timeout.detach();
jah128 1:060690a934a9 191 ft_timeout.attach(IF_file_transfer_timeout,1.0);
jah128 1:060690a934a9 192 }
jah128 1:060690a934a9 193 }
jah128 1:060690a934a9 194 }
jah128 0:d6269d17c8cf 195 }
jah128 0:d6269d17c8cf 196
jah128 0:d6269d17c8cf 197
jah128 0:d6269d17c8cf 198 void IF_handle_user_serial_message(char * message, char length, char interface)
jah128 0:d6269d17c8cf 199 {
jah128 0:d6269d17c8cf 200 char buffer[255];
jah128 0:d6269d17c8cf 201 sprintf(buffer,message,length);
jah128 0:d6269d17c8cf 202 for(int i=0; i<length; i++) {
jah128 0:d6269d17c8cf 203 buffer[i]=message[i];
jah128 0:d6269d17c8cf 204 }
jah128 0:d6269d17c8cf 205 buffer[length]=0;
jah128 0:d6269d17c8cf 206 // if(interface) debug("Received BT message:%s [%d chars]\n",buffer,length);
jah128 0:d6269d17c8cf 207 // else debug("Received USB message:%s [%d chars]\n",buffer,length);
jah128 0:d6269d17c8cf 208 handle_user_serial_message(message,length,interface);
jah128 0:d6269d17c8cf 209 }
jah128 0:d6269d17c8cf 210
jah128 0:d6269d17c8cf 211 void IF_handle_command_serial_message(char message[3], char interface)
jah128 0:d6269d17c8cf 212 {
jah128 0:d6269d17c8cf 213 char iface [4];
jah128 0:d6269d17c8cf 214 if(interface) strcpy(iface,"BT");
jah128 0:d6269d17c8cf 215 else strcpy(iface,"USB");
jah128 0:d6269d17c8cf 216 char command [26];
jah128 0:d6269d17c8cf 217 char subcommand[30];
jah128 0:d6269d17c8cf 218 float dec;
jah128 0:d6269d17c8cf 219 float l_dec;
jah128 0:d6269d17c8cf 220 float r_dec;
jah128 0:d6269d17c8cf 221 int irp_delay;
jah128 0:d6269d17c8cf 222 char colour_string[7];
jah128 0:d6269d17c8cf 223 char ret_message[50];
jah128 0:d6269d17c8cf 224 char send_message = 0;
jah128 0:d6269d17c8cf 225 char command_status = 0;
jah128 0:d6269d17c8cf 226 // command_status values:
jah128 0:d6269d17c8cf 227 // 0 - unrecognised command
jah128 0:d6269d17c8cf 228 // 1 - command actioned
jah128 0:d6269d17c8cf 229 // 2 - command blocked
jah128 0:d6269d17c8cf 230 // 3 - invalid parameters
jah128 0:d6269d17c8cf 231
jah128 0:d6269d17c8cf 232 subcommand[0]=0;
jah128 0:d6269d17c8cf 233 command[0]=0;
jah128 0:d6269d17c8cf 234 switch(message[0]) {
jah128 0:d6269d17c8cf 235
jah128 0:d6269d17c8cf 236 // MOTOR COMMANDS
jah128 0:d6269d17c8cf 237
jah128 0:d6269d17c8cf 238 case 1:
jah128 0:d6269d17c8cf 239 strcpy(command,"SET LEFT MOTOR");
jah128 0:d6269d17c8cf 240 dec = IF_decode_float(message[1],message[2]);
jah128 0:d6269d17c8cf 241 sprintf(subcommand,"%1.5f",dec);
jah128 0:d6269d17c8cf 242 if(allow_commands) {
jah128 0:d6269d17c8cf 243 command_status = 1;
jah128 0:d6269d17c8cf 244 set_left_motor_speed(dec);
jah128 0:d6269d17c8cf 245 } else command_status = 2;
jah128 0:d6269d17c8cf 246 break;
jah128 0:d6269d17c8cf 247 case 2:
jah128 0:d6269d17c8cf 248 strcpy(command,"SET RIGHT MOTOR");
jah128 0:d6269d17c8cf 249 dec = IF_decode_float(message[1],message[2]);
jah128 0:d6269d17c8cf 250 sprintf(subcommand,"%1.5f",dec);
jah128 0:d6269d17c8cf 251 if(allow_commands) {
jah128 0:d6269d17c8cf 252 set_right_motor_speed(dec);
jah128 0:d6269d17c8cf 253 command_status = 1;
jah128 0:d6269d17c8cf 254 } else command_status = 2;
jah128 0:d6269d17c8cf 255 break;
jah128 0:d6269d17c8cf 256 case 3:
jah128 0:d6269d17c8cf 257 strcpy(command,"SET BOTH MOTORS");
jah128 0:d6269d17c8cf 258 dec = IF_decode_float(message[1],message[2]);
jah128 0:d6269d17c8cf 259 sprintf(subcommand,"%1.5f",dec);
jah128 0:d6269d17c8cf 260 if(allow_commands) {
jah128 0:d6269d17c8cf 261 command_status = 1;
jah128 0:d6269d17c8cf 262 forward(dec);
jah128 0:d6269d17c8cf 263 } else command_status = 2;
jah128 0:d6269d17c8cf 264 break;
jah128 0:d6269d17c8cf 265 case 4:
jah128 0:d6269d17c8cf 266 strcpy(command,"BRAKE LEFT MOTOR");
jah128 0:d6269d17c8cf 267 sprintf(subcommand,"");
jah128 0:d6269d17c8cf 268 if(allow_commands) {
jah128 0:d6269d17c8cf 269 command_status = 1;
jah128 0:d6269d17c8cf 270 brake_left_motor();
jah128 0:d6269d17c8cf 271 } else command_status = 2;
jah128 0:d6269d17c8cf 272 break;
jah128 0:d6269d17c8cf 273 case 5:
jah128 0:d6269d17c8cf 274 strcpy(command,"BRAKE RIGHT MOTOR");
jah128 0:d6269d17c8cf 275 sprintf(subcommand,"");
jah128 0:d6269d17c8cf 276 if(allow_commands) {
jah128 0:d6269d17c8cf 277 command_status = 1;
jah128 0:d6269d17c8cf 278 brake_right_motor();
jah128 0:d6269d17c8cf 279 } else command_status = 2;
jah128 0:d6269d17c8cf 280 break;
jah128 0:d6269d17c8cf 281 case 6:
jah128 0:d6269d17c8cf 282 strcpy(command,"BRAKE BOTH MOTORS");
jah128 0:d6269d17c8cf 283 sprintf(subcommand,"");
jah128 0:d6269d17c8cf 284 if(allow_commands) {
jah128 0:d6269d17c8cf 285 command_status = 1;
jah128 0:d6269d17c8cf 286 brake();
jah128 0:d6269d17c8cf 287 } else command_status = 2;
jah128 0:d6269d17c8cf 288 break;
jah128 0:d6269d17c8cf 289 case 7:
jah128 0:d6269d17c8cf 290 strcpy(command,"STOP BOTH MOTORS");
jah128 0:d6269d17c8cf 291 sprintf(subcommand,"");
jah128 0:d6269d17c8cf 292 if(allow_commands) {
jah128 0:d6269d17c8cf 293 command_status = 1;
jah128 0:d6269d17c8cf 294 stop();
jah128 0:d6269d17c8cf 295 } else command_status = 2;
jah128 0:d6269d17c8cf 296 break;
jah128 0:d6269d17c8cf 297 case 8:
jah128 0:d6269d17c8cf 298 strcpy(command,"TURN ON SPOT");
jah128 0:d6269d17c8cf 299 dec = IF_decode_float(message[1],message[2]);
jah128 0:d6269d17c8cf 300 sprintf(subcommand,"%1.5f",dec);
jah128 0:d6269d17c8cf 301 if(allow_commands) {
jah128 0:d6269d17c8cf 302 command_status = 1;
jah128 0:d6269d17c8cf 303 turn(dec);
jah128 0:d6269d17c8cf 304 } else command_status = 2;
jah128 0:d6269d17c8cf 305 break;
jah128 0:d6269d17c8cf 306 case 9:
jah128 0:d6269d17c8cf 307 strcpy(command,"SET EACH MOTOR");
jah128 0:d6269d17c8cf 308 l_dec = IF_decode_float(message[1]);
jah128 0:d6269d17c8cf 309 r_dec = IF_decode_float(message[2]);
jah128 0:d6269d17c8cf 310 sprintf(subcommand,"L=%1.3f R=%1.3f",l_dec,r_dec);
jah128 0:d6269d17c8cf 311 if(allow_commands) {
jah128 0:d6269d17c8cf 312 command_status = 1;
jah128 0:d6269d17c8cf 313
jah128 0:d6269d17c8cf 314 set_left_motor_speed(l_dec);
jah128 0:d6269d17c8cf 315 set_right_motor_speed(r_dec);
jah128 0:d6269d17c8cf 316 } else command_status = 2;
jah128 0:d6269d17c8cf 317 break;
jah128 0:d6269d17c8cf 318 // LED COMMANDS
jah128 0:d6269d17c8cf 319
jah128 0:d6269d17c8cf 320 case 10:
jah128 0:d6269d17c8cf 321 strcpy(command,"SET LED STATES");
jah128 0:d6269d17c8cf 322 sprintf(subcommand,"G:%s R:%s",IF_char_to_binary_char(message[1]), IF_char_to_binary_char(message[2]));
jah128 0:d6269d17c8cf 323 if(allow_commands) {
jah128 0:d6269d17c8cf 324 command_status = 1;
jah128 0:d6269d17c8cf 325 set_leds(message[1],message[2]);
jah128 0:d6269d17c8cf 326 } else command_status = 2;
jah128 0:d6269d17c8cf 327 break;
jah128 0:d6269d17c8cf 328 case 11:
jah128 0:d6269d17c8cf 329 strcpy(command,"SET RED LED STATES");
jah128 0:d6269d17c8cf 330 sprintf(subcommand,"%s",IF_char_to_binary_char(message[1]));
jah128 0:d6269d17c8cf 331 if(allow_commands) {
jah128 0:d6269d17c8cf 332 command_status = 1;
jah128 0:d6269d17c8cf 333 set_red_leds(message[1]);
jah128 0:d6269d17c8cf 334 } else command_status = 2;
jah128 0:d6269d17c8cf 335 break;
jah128 0:d6269d17c8cf 336 case 12:
jah128 0:d6269d17c8cf 337 strcpy(command,"SET GREEN LED STATES");
jah128 0:d6269d17c8cf 338 sprintf(subcommand,"%s",IF_char_to_binary_char(message[1]));
jah128 0:d6269d17c8cf 339 if(allow_commands) {
jah128 0:d6269d17c8cf 340 command_status = 1;
jah128 0:d6269d17c8cf 341 set_green_leds(message[1]);
jah128 0:d6269d17c8cf 342 } else command_status = 2;
jah128 0:d6269d17c8cf 343 break;
jah128 0:d6269d17c8cf 344 case 13:
jah128 0:d6269d17c8cf 345 strcpy(command,"SET LED");
jah128 0:d6269d17c8cf 346 switch(message[2]) {
jah128 0:d6269d17c8cf 347 case 1:
jah128 0:d6269d17c8cf 348 strcpy(colour_string,"RED");
jah128 0:d6269d17c8cf 349 break;
jah128 0:d6269d17c8cf 350 case 2:
jah128 0:d6269d17c8cf 351 strcpy(colour_string,"GREEN");
jah128 0:d6269d17c8cf 352 break;
jah128 0:d6269d17c8cf 353 case 3:
jah128 0:d6269d17c8cf 354 strcpy(colour_string,"BOTH");
jah128 0:d6269d17c8cf 355 break;
jah128 0:d6269d17c8cf 356 case 0:
jah128 0:d6269d17c8cf 357 strcpy(colour_string,"OFF");
jah128 0:d6269d17c8cf 358 break;
jah128 0:d6269d17c8cf 359 }
jah128 0:d6269d17c8cf 360 if(message[1] < 8 && message[2] < 4) {
jah128 0:d6269d17c8cf 361 sprintf(subcommand,"%d %s",message[1],colour_string);
jah128 0:d6269d17c8cf 362 if(allow_commands) {
jah128 0:d6269d17c8cf 363 command_status = 1;
jah128 0:d6269d17c8cf 364 set_led(message[1],message[2]);
jah128 0:d6269d17c8cf 365 } else command_status = 2;
jah128 0:d6269d17c8cf 366 } else {
jah128 0:d6269d17c8cf 367 sprintf(subcommand,"[INVALID CODE]");
jah128 0:d6269d17c8cf 368 command_status = 3;
jah128 0:d6269d17c8cf 369 }
jah128 0:d6269d17c8cf 370 break;
jah128 0:d6269d17c8cf 371 case 14:
jah128 0:d6269d17c8cf 372 strcpy(command,"SET CENTER LED STATE");
jah128 0:d6269d17c8cf 373 switch(message[1]) {
jah128 0:d6269d17c8cf 374 case 1:
jah128 0:d6269d17c8cf 375 strcpy(colour_string,"RED");
jah128 0:d6269d17c8cf 376 break;
jah128 0:d6269d17c8cf 377 case 2:
jah128 0:d6269d17c8cf 378 strcpy(colour_string,"GREEN");
jah128 0:d6269d17c8cf 379 break;
jah128 0:d6269d17c8cf 380 case 3:
jah128 0:d6269d17c8cf 381 strcpy(colour_string,"BOTH");
jah128 0:d6269d17c8cf 382 break;
jah128 0:d6269d17c8cf 383 case 0:
jah128 0:d6269d17c8cf 384 strcpy(colour_string,"OFF");
jah128 0:d6269d17c8cf 385 break;
jah128 0:d6269d17c8cf 386 }
jah128 0:d6269d17c8cf 387 if(message[1] < 4) {
jah128 0:d6269d17c8cf 388 sprintf(subcommand,"%s",colour_string);
jah128 0:d6269d17c8cf 389 if(allow_commands) {
jah128 0:d6269d17c8cf 390 command_status = 1;
jah128 0:d6269d17c8cf 391 set_center_led(message[1]);
jah128 0:d6269d17c8cf 392 } else command_status = 2;
jah128 0:d6269d17c8cf 393 } else {
jah128 0:d6269d17c8cf 394 sprintf(subcommand,"[INVALID CODE]");
jah128 0:d6269d17c8cf 395 command_status = 3;
jah128 0:d6269d17c8cf 396 }
jah128 0:d6269d17c8cf 397 break;
jah128 0:d6269d17c8cf 398 case 15:
jah128 0:d6269d17c8cf 399 strcpy(command,"SET C.LED BRIGHTNESS");
jah128 0:d6269d17c8cf 400 dec = IF_decode_unsigned_float(message[1],message[2]);
jah128 0:d6269d17c8cf 401 sprintf(subcommand,"%1.5f",dec);
jah128 0:d6269d17c8cf 402 if(allow_commands) {
jah128 0:d6269d17c8cf 403 command_status = 1;
jah128 0:d6269d17c8cf 404 set_center_led_brightness(dec);
jah128 0:d6269d17c8cf 405 } else command_status = 2;
jah128 0:d6269d17c8cf 406 break;
jah128 0:d6269d17c8cf 407 case 16:
jah128 0:d6269d17c8cf 408 strcpy(command,"SET MBED LEDS");
jah128 0:d6269d17c8cf 409 sprintf(subcommand,"%s",IF_nibble_to_binary_char(message[1]));
jah128 0:d6269d17c8cf 410 if(allow_commands) {
jah128 0:d6269d17c8cf 411 command_status = 1;
jah128 0:d6269d17c8cf 412 mbed_led1 = (message[1] & 128) >> 7;
jah128 0:d6269d17c8cf 413 mbed_led2 = (message[1] & 64) >> 6;
jah128 0:d6269d17c8cf 414 mbed_led3 = (message[1] & 32) >> 5;
jah128 0:d6269d17c8cf 415 mbed_led4 = (message[1] & 16) >> 4;
jah128 0:d6269d17c8cf 416 } else command_status = 2;
jah128 0:d6269d17c8cf 417 break;
jah128 0:d6269d17c8cf 418 case 17:
jah128 0:d6269d17c8cf 419 strcpy(command,"BLINK OUTER LEDS");
jah128 0:d6269d17c8cf 420 dec = IF_decode_unsigned_float(message[1],message[2]);
jah128 0:d6269d17c8cf 421 sprintf(subcommand,"FOR %1.5fS",dec);
jah128 0:d6269d17c8cf 422 if(allow_commands) {
jah128 0:d6269d17c8cf 423 command_status = 1;
jah128 0:d6269d17c8cf 424 blink_leds(dec);
jah128 0:d6269d17c8cf 425 } else command_status = 2;
jah128 0:d6269d17c8cf 426 break;
jah128 0:d6269d17c8cf 427 case 18:
jah128 0:d6269d17c8cf 428 strcpy(command,"SET BASE LED STATE");
jah128 0:d6269d17c8cf 429 switch(message[1]) {
jah128 0:d6269d17c8cf 430 case 1:
jah128 0:d6269d17c8cf 431 strcpy(subcommand,"ON");
jah128 0:d6269d17c8cf 432 break;
jah128 0:d6269d17c8cf 433 case 0:
jah128 0:d6269d17c8cf 434 strcpy(subcommand,"OFF");
jah128 0:d6269d17c8cf 435 break;
jah128 0:d6269d17c8cf 436 }
jah128 0:d6269d17c8cf 437 //Function not yet implemented
jah128 0:d6269d17c8cf 438 break;
jah128 0:d6269d17c8cf 439 case 19:
jah128 0:d6269d17c8cf 440 strcpy(command,"SET CENTER LED ");
jah128 0:d6269d17c8cf 441 switch(message[1]) {
jah128 0:d6269d17c8cf 442 case 1:
jah128 0:d6269d17c8cf 443 strcpy(colour_string,"RED");
jah128 0:d6269d17c8cf 444 break;
jah128 0:d6269d17c8cf 445 case 2:
jah128 0:d6269d17c8cf 446 strcpy(colour_string,"GREEN");
jah128 0:d6269d17c8cf 447 break;
jah128 0:d6269d17c8cf 448 case 3:
jah128 0:d6269d17c8cf 449 strcpy(colour_string,"BOTH");
jah128 0:d6269d17c8cf 450 break;
jah128 0:d6269d17c8cf 451 case 0:
jah128 0:d6269d17c8cf 452 strcpy(colour_string,"OFF");
jah128 0:d6269d17c8cf 453 break;
jah128 0:d6269d17c8cf 454 }
jah128 0:d6269d17c8cf 455 dec = IF_decode_unsigned_float(message[2]);
jah128 0:d6269d17c8cf 456 sprintf(subcommand,"%s @ %1.5f brightness",colour_string,dec);
jah128 0:d6269d17c8cf 457 if(allow_commands) {
jah128 0:d6269d17c8cf 458 command_status = 1;
jah128 0:d6269d17c8cf 459 set_center_led(message[1],dec);
jah128 0:d6269d17c8cf 460 } else command_status = 2;
jah128 0:d6269d17c8cf 461 break;
jah128 0:d6269d17c8cf 462
jah128 0:d6269d17c8cf 463 // DISPLAY COMMANDS
jah128 0:d6269d17c8cf 464
jah128 0:d6269d17c8cf 465 case 20:
jah128 0:d6269d17c8cf 466 strcpy(command,"SET DISPLAY ");
jah128 0:d6269d17c8cf 467 switch(message[1]) {
jah128 0:d6269d17c8cf 468 case 0:
jah128 0:d6269d17c8cf 469 strcpy(subcommand,"CLEAR");
jah128 0:d6269d17c8cf 470 if(allow_commands) {
jah128 0:d6269d17c8cf 471 command_status = 1;
jah128 0:d6269d17c8cf 472 display.clear_display();
jah128 0:d6269d17c8cf 473 } else command_status = 2;
jah128 0:d6269d17c8cf 474 break;
jah128 0:d6269d17c8cf 475 case 1:
jah128 0:d6269d17c8cf 476 strcpy(subcommand,"MESSAGE 1");
jah128 0:d6269d17c8cf 477 if(allow_commands) {
jah128 0:d6269d17c8cf 478 command_status = 1;
jah128 0:d6269d17c8cf 479 display.clear_display();
jah128 0:d6269d17c8cf 480 display.home();
jah128 0:d6269d17c8cf 481 display.write_string("PC CONNECTION");
jah128 0:d6269d17c8cf 482 display.set_position(1,0);
jah128 0:d6269d17c8cf 483 display.write_string("STARTED");
jah128 0:d6269d17c8cf 484 } else command_status = 2;
jah128 0:d6269d17c8cf 485 break;
jah128 0:d6269d17c8cf 486 case 2:
jah128 0:d6269d17c8cf 487 strcpy(subcommand,"MESSAGE 2");
jah128 0:d6269d17c8cf 488 if(allow_commands) {
jah128 0:d6269d17c8cf 489 command_status = 1;
jah128 0:d6269d17c8cf 490 display.clear_display();
jah128 0:d6269d17c8cf 491 display.home();
jah128 0:d6269d17c8cf 492 display.write_string("PC CONNECTION");
jah128 0:d6269d17c8cf 493 display.set_position(1,0);
jah128 0:d6269d17c8cf 494 display.write_string("TERMINATED");
jah128 0:d6269d17c8cf 495 } else command_status = 2;
jah128 0:d6269d17c8cf 496 break;
jah128 0:d6269d17c8cf 497 case 3:
jah128 0:d6269d17c8cf 498 strcpy(subcommand,"MESSAGE 3");
jah128 0:d6269d17c8cf 499 if(allow_commands) {
jah128 0:d6269d17c8cf 500 command_status = 1;
jah128 0:d6269d17c8cf 501 display.clear_display();
jah128 0:d6269d17c8cf 502 display.home();
jah128 0:d6269d17c8cf 503 display.write_string("ANDROID DEVICE");
jah128 0:d6269d17c8cf 504 display.set_position(1,0);
jah128 0:d6269d17c8cf 505 display.write_string("CONNECTED");
jah128 0:d6269d17c8cf 506 } else command_status = 2;
jah128 0:d6269d17c8cf 507 break;
jah128 0:d6269d17c8cf 508 case 4:
jah128 0:d6269d17c8cf 509 strcpy(subcommand,"MESSAGE 4");
jah128 0:d6269d17c8cf 510 if(allow_commands) {
jah128 0:d6269d17c8cf 511 command_status = 1;
jah128 0:d6269d17c8cf 512 display.clear_display();
jah128 0:d6269d17c8cf 513 display.home();
jah128 0:d6269d17c8cf 514 display.write_string("ANDROID DEVICE");
jah128 0:d6269d17c8cf 515 display.set_position(1,0);
jah128 0:d6269d17c8cf 516 display.write_string("DISCONNECTED");
jah128 0:d6269d17c8cf 517 } else command_status = 2;
jah128 0:d6269d17c8cf 518 break;
jah128 0:d6269d17c8cf 519 }
jah128 0:d6269d17c8cf 520 break;
jah128 0:d6269d17c8cf 521 case 21:
jah128 0:d6269d17c8cf 522 strcpy(command,"SET CURSOR ");
jah128 0:d6269d17c8cf 523 if(message[1] < 2 && message[2] < 16) {
jah128 0:d6269d17c8cf 524 sprintf(subcommand,"[%d,%d]",message[1],message[2]);
jah128 0:d6269d17c8cf 525 if(allow_commands) {
jah128 0:d6269d17c8cf 526 display.set_position(message[1],message[2]);
jah128 0:d6269d17c8cf 527 } else command_status = 2;
jah128 0:d6269d17c8cf 528 } else {
jah128 0:d6269d17c8cf 529 sprintf(subcommand,"[INVALID]");
jah128 0:d6269d17c8cf 530 command_status = 3;
jah128 0:d6269d17c8cf 531 }
jah128 0:d6269d17c8cf 532 break;
jah128 0:d6269d17c8cf 533 case 22:
jah128 0:d6269d17c8cf 534 strcpy(command,"PRINT CHARACTERS ");
jah128 0:d6269d17c8cf 535 char print_message[2];
jah128 0:d6269d17c8cf 536 print_message[0]=message[1];
jah128 0:d6269d17c8cf 537 print_message[1]=message[2];
jah128 0:d6269d17c8cf 538 sprintf(subcommand,"[%c,%c]",message[1],message[2]);
jah128 0:d6269d17c8cf 539 if(allow_commands) {
jah128 0:d6269d17c8cf 540 display.write_string(print_message,2);
jah128 0:d6269d17c8cf 541 } else command_status = 2;
jah128 0:d6269d17c8cf 542 break;
jah128 0:d6269d17c8cf 543 case 23:
jah128 0:d6269d17c8cf 544 strcpy(command,"SET DISPLAY B.NESS");
jah128 0:d6269d17c8cf 545 dec = IF_decode_unsigned_float(message[1],message[2]);
jah128 0:d6269d17c8cf 546 sprintf(subcommand,"%1.5f",dec);
jah128 0:d6269d17c8cf 547 if(allow_commands) {
jah128 0:d6269d17c8cf 548 command_status = 1;
jah128 0:d6269d17c8cf 549 display.set_backlight_brightness(dec);
jah128 0:d6269d17c8cf 550 } else command_status = 2;
jah128 0:d6269d17c8cf 551 break;
jah128 0:d6269d17c8cf 552
jah128 0:d6269d17c8cf 553 case 30:
jah128 0:d6269d17c8cf 554 strcpy(command,"SET DEBUG MODE");
jah128 0:d6269d17c8cf 555 switch(message[1]) {
jah128 0:d6269d17c8cf 556 case 1:
jah128 0:d6269d17c8cf 557 strcpy(subcommand,"ON");
jah128 0:d6269d17c8cf 558 break;
jah128 0:d6269d17c8cf 559 case 0:
jah128 0:d6269d17c8cf 560 strcpy(subcommand,"OFF");
jah128 0:d6269d17c8cf 561 break;
jah128 0:d6269d17c8cf 562 }
jah128 0:d6269d17c8cf 563 if(message[2] & 1) strcat (subcommand,"-PC");
jah128 0:d6269d17c8cf 564 if(message[2] & 2) strcat (subcommand,"-BT");
jah128 0:d6269d17c8cf 565 if(message[2] & 4) strcat (subcommand,"-DISP");
jah128 0:d6269d17c8cf 566 if(allow_commands) {
jah128 0:d6269d17c8cf 567 command_status = 1;
jah128 0:d6269d17c8cf 568 debug_mode = message[1];
jah128 0:d6269d17c8cf 569 debug_output = message[2];
jah128 0:d6269d17c8cf 570 } else command_status = 2;
jah128 0:d6269d17c8cf 571 break;
jah128 0:d6269d17c8cf 572 case 31:
jah128 0:d6269d17c8cf 573 strcpy(command,"SET DEMO MODE");
jah128 0:d6269d17c8cf 574 switch(message[1] % 2) {
jah128 0:d6269d17c8cf 575 case 1:
jah128 0:d6269d17c8cf 576 strcpy(subcommand,"ON");
jah128 0:d6269d17c8cf 577 break;
jah128 0:d6269d17c8cf 578 case 0:
jah128 0:d6269d17c8cf 579 strcpy(subcommand,"OFF");
jah128 0:d6269d17c8cf 580 break;
jah128 0:d6269d17c8cf 581 }
jah128 0:d6269d17c8cf 582 if(allow_commands) {
jah128 0:d6269d17c8cf 583 command_status = 1;
jah128 0:d6269d17c8cf 584 demo_on = message[1] % 2;
jah128 0:d6269d17c8cf 585 if(demo_on == 1) {
jah128 0:d6269d17c8cf 586 user_code_restore_mode = user_code_running;
jah128 0:d6269d17c8cf 587 user_code_running = 0;
jah128 0:d6269d17c8cf 588 } else {
jah128 0:d6269d17c8cf 589 user_code_running = user_code_restore_mode;
jah128 0:d6269d17c8cf 590 }
jah128 0:d6269d17c8cf 591 } else command_status = 2;
jah128 0:d6269d17c8cf 592 break;
jah128 0:d6269d17c8cf 593 case 32:
jah128 0:d6269d17c8cf 594 strcpy(command,"SET USER CODE");
jah128 0:d6269d17c8cf 595 switch(message[1] % 2) {
jah128 0:d6269d17c8cf 596 case 1:
jah128 0:d6269d17c8cf 597 strcpy(subcommand,"ON");
jah128 0:d6269d17c8cf 598 break;
jah128 0:d6269d17c8cf 599 case 0:
jah128 0:d6269d17c8cf 600 strcpy(subcommand,"OFF");
jah128 0:d6269d17c8cf 601 break;
jah128 0:d6269d17c8cf 602 }
jah128 0:d6269d17c8cf 603 if(allow_commands) {
jah128 0:d6269d17c8cf 604 command_status = 1;
jah128 0:d6269d17c8cf 605 user_code_running = message[1] % 2;
jah128 0:d6269d17c8cf 606 } else command_status = 2;
jah128 0:d6269d17c8cf 607 break;
jah128 0:d6269d17c8cf 608 case 33:
jah128 0:d6269d17c8cf 609 strcpy(command,"PAUSE USER CODE");
jah128 0:d6269d17c8cf 610 dec = IF_decode_unsigned_float(message[1],message[2]) * 10;
jah128 0:d6269d17c8cf 611 sprintf(subcommand,"FOR %2.3fS",dec);
jah128 0:d6269d17c8cf 612 if(allow_commands) {
jah128 0:d6269d17c8cf 613 command_status = 1;
jah128 0:d6269d17c8cf 614 pause_user_code(dec);
jah128 0:d6269d17c8cf 615 } else command_status = 2;
jah128 0:d6269d17c8cf 616 break;
jah128 0:d6269d17c8cf 617
jah128 0:d6269d17c8cf 618 case 34:
jah128 0:d6269d17c8cf 619 strcpy(command,"RESET ENCODERS");
jah128 0:d6269d17c8cf 620 if(allow_commands) {
jah128 0:d6269d17c8cf 621 command_status = 1;
jah128 0:d6269d17c8cf 622 reset_encoders();
jah128 0:d6269d17c8cf 623 } else command_status = 2;
jah128 0:d6269d17c8cf 624 break;
jah128 0:d6269d17c8cf 625
jah128 0:d6269d17c8cf 626 case 35:
jah128 0:d6269d17c8cf 627 strcpy(command,"SET ALLOW COMMANDS");
jah128 0:d6269d17c8cf 628 switch(message[1] % 2) {
jah128 0:d6269d17c8cf 629 case 1:
jah128 0:d6269d17c8cf 630 strcpy(subcommand,"ON");
jah128 0:d6269d17c8cf 631 break;
jah128 0:d6269d17c8cf 632 case 0:
jah128 0:d6269d17c8cf 633 strcpy(subcommand,"OFF");
jah128 0:d6269d17c8cf 634 break;
jah128 0:d6269d17c8cf 635 }
jah128 0:d6269d17c8cf 636 allow_commands = message[1] % 2;
jah128 0:d6269d17c8cf 637 command_status = 1;
jah128 0:d6269d17c8cf 638 break;
jah128 0:d6269d17c8cf 639
jah128 0:d6269d17c8cf 640 case 36:
jah128 0:d6269d17c8cf 641 irp_delay = (message[1] << 8) + message[2];
jah128 0:d6269d17c8cf 642 sprintf(command,"SET IR PULSE DELAY %d MS",irp_delay);
jah128 0:d6269d17c8cf 643 if(allow_commands) {
jah128 0:d6269d17c8cf 644 command_status = 1;
jah128 0:d6269d17c8cf 645 ir_pulse_delay = irp_delay;
jah128 0:d6269d17c8cf 646 } else command_status = 2;
jah128 0:d6269d17c8cf 647 break;
jah128 0:d6269d17c8cf 648 case 37:
jah128 0:d6269d17c8cf 649 irp_delay = (message[1] << 8) + message[2];
jah128 0:d6269d17c8cf 650 sprintf(command,"SET BASE IR PULSE DELAY %d MS",irp_delay);
jah128 0:d6269d17c8cf 651 if(allow_commands) {
jah128 0:d6269d17c8cf 652 command_status = 1;
jah128 0:d6269d17c8cf 653 base_ir_pulse_delay = irp_delay;
jah128 0:d6269d17c8cf 654 } else command_status = 2;
jah128 0:d6269d17c8cf 655 break;
jah128 0:d6269d17c8cf 656
jah128 0:d6269d17c8cf 657 // MOTOR REQUESTS
jah128 0:d6269d17c8cf 658 case 40:
jah128 0:d6269d17c8cf 659 strcpy(command,"GET LEFT MOTOR SPEED");
jah128 0:d6269d17c8cf 660 sprintf(ret_message,"%1.5f",motor_left_speed);
jah128 0:d6269d17c8cf 661 send_message = 1;
jah128 0:d6269d17c8cf 662 break;
jah128 0:d6269d17c8cf 663
jah128 0:d6269d17c8cf 664 case 41:
jah128 0:d6269d17c8cf 665 strcpy(command,"GET RIGHT MOTOR SPEED");
jah128 0:d6269d17c8cf 666 sprintf(ret_message,"%1.5f",motor_right_speed);
jah128 0:d6269d17c8cf 667 send_message = 1;
jah128 0:d6269d17c8cf 668 break;
jah128 0:d6269d17c8cf 669 case 42:
jah128 0:d6269d17c8cf 670 strcpy(command,"GET BRAKE STATES");
jah128 0:d6269d17c8cf 671 sprintf(ret_message,"%d,%d",motor_left_brake,motor_right_brake);
jah128 0:d6269d17c8cf 672 send_message = 1;
jah128 0:d6269d17c8cf 673 break;
jah128 0:d6269d17c8cf 674 case 43:
jah128 0:d6269d17c8cf 675 strcpy(command,"GET MOTOR STATES");
jah128 0:d6269d17c8cf 676 //sprintf(ret_message,"%d,%d",motor_left_brake,motor_right_brake);
jah128 0:d6269d17c8cf 677 send_message = 1;
jah128 0:d6269d17c8cf 678 break;
jah128 0:d6269d17c8cf 679 case 44:
jah128 0:d6269d17c8cf 680 strcpy(command,"GET ENCODERS");
jah128 0:d6269d17c8cf 681 sprintf(ret_message,"%d,%d",left_encoder,right_encoder);
jah128 0:d6269d17c8cf 682 send_message = 1;
jah128 0:d6269d17c8cf 683 break;
jah128 0:d6269d17c8cf 684
jah128 0:d6269d17c8cf 685 // LED REQUESTS
jah128 0:d6269d17c8cf 686 case 50:
jah128 0:d6269d17c8cf 687 strcpy(command,"GET LED STATES");
jah128 0:d6269d17c8cf 688 sprintf(ret_message,"%04x",get_led_states());
jah128 0:d6269d17c8cf 689 send_message = 1;
jah128 0:d6269d17c8cf 690 break;
jah128 0:d6269d17c8cf 691
jah128 0:d6269d17c8cf 692 // GENERAL REQUESTS
jah128 0:d6269d17c8cf 693 case 60:
jah128 0:d6269d17c8cf 694 strcpy(command,"GET SOFTWARE VERSION");
jah128 0:d6269d17c8cf 695 sprintf(ret_message,"%1.2f",SOFTWARE_VERSION_CODE);
jah128 0:d6269d17c8cf 696 send_message = 1;
jah128 0:d6269d17c8cf 697 break;
jah128 0:d6269d17c8cf 698
jah128 0:d6269d17c8cf 699 case 61:
jah128 0:d6269d17c8cf 700 strcpy(command,"GET UPTIME");
jah128 0:d6269d17c8cf 701 sprintf(ret_message,"%6.2f",get_uptime());
jah128 0:d6269d17c8cf 702 send_message = 1;
jah128 0:d6269d17c8cf 703 break;
jah128 0:d6269d17c8cf 704
jah128 0:d6269d17c8cf 705 case 62:
jah128 0:d6269d17c8cf 706 strcpy(command,"GET ID");
jah128 0:d6269d17c8cf 707 sprintf(ret_message,"%d",robot_id);
jah128 0:d6269d17c8cf 708 send_message = 1;
jah128 0:d6269d17c8cf 709 break;
jah128 0:d6269d17c8cf 710
jah128 0:d6269d17c8cf 711 case 63:
jah128 0:d6269d17c8cf 712 strcpy(command,"GET SWITCH BYTE");
jah128 0:d6269d17c8cf 713 sprintf(ret_message,"%02x",switch_byte);
jah128 0:d6269d17c8cf 714 send_message = 1;
jah128 0:d6269d17c8cf 715 break;
jah128 0:d6269d17c8cf 716 case 64:
jah128 0:d6269d17c8cf 717 strcpy(command,"GET USER CODE");
jah128 0:d6269d17c8cf 718 sprintf(ret_message,"%d",user_code_running);
jah128 0:d6269d17c8cf 719 send_message = 1;
jah128 0:d6269d17c8cf 720 break;
jah128 0:d6269d17c8cf 721 case 65:
jah128 0:d6269d17c8cf 722 strcpy(command,"GET RESPONSE STRING");
jah128 0:d6269d17c8cf 723 sprintf(ret_message,"PSI");
jah128 0:d6269d17c8cf 724 send_message = 1;
jah128 0:d6269d17c8cf 725 break;
jah128 0:d6269d17c8cf 726 case 66:
jah128 0:d6269d17c8cf 727 strcpy(command,"GET PROGRAM NAME");
jah128 0:d6269d17c8cf 728 sprintf(ret_message,"%s",program_name);
jah128 0:d6269d17c8cf 729 send_message = 1;
jah128 0:d6269d17c8cf 730 break;
jah128 0:d6269d17c8cf 731 case 67:
jah128 0:d6269d17c8cf 732 strcpy(command,"GET AUTHOR NAME");
jah128 0:d6269d17c8cf 733 sprintf(ret_message,"%s",author_name);
jah128 0:d6269d17c8cf 734 send_message = 1;
jah128 0:d6269d17c8cf 735 break;
jah128 0:d6269d17c8cf 736 case 68:
jah128 0:d6269d17c8cf 737 strcpy(command,"GET DEBUG MODE");
jah128 0:d6269d17c8cf 738 sprintf(ret_message,"%1d%1d",debug_mode,debug_output);
jah128 0:d6269d17c8cf 739 send_message = 1;
jah128 0:d6269d17c8cf 740 break;
jah128 0:d6269d17c8cf 741 case 69:
jah128 0:d6269d17c8cf 742 strcpy(command,"GET SYSTEM WARNINGS");
jah128 0:d6269d17c8cf 743 sprintf(ret_message,"%d",system_warnings);
jah128 0:d6269d17c8cf 744 send_message = 1;
jah128 0:d6269d17c8cf 745 break;
jah128 0:d6269d17c8cf 746
jah128 0:d6269d17c8cf 747
jah128 0:d6269d17c8cf 748 // Sensors
jah128 0:d6269d17c8cf 749 case 80:
jah128 0:d6269d17c8cf 750 strcpy(command,"STORE BG. IR VALUES");
jah128 0:d6269d17c8cf 751 if(allow_commands) {
jah128 0:d6269d17c8cf 752 command_status = 1;
jah128 0:d6269d17c8cf 753 store_background_raw_ir_values();
jah128 0:d6269d17c8cf 754 } else command_status = 2;
jah128 0:d6269d17c8cf 755 break;
jah128 0:d6269d17c8cf 756 case 81:
jah128 0:d6269d17c8cf 757 strcpy(command,"STORE IL. IR VALUES");
jah128 0:d6269d17c8cf 758 if(allow_commands) {
jah128 0:d6269d17c8cf 759 command_status = 1;
jah128 0:d6269d17c8cf 760 store_illuminated_raw_ir_values();
jah128 0:d6269d17c8cf 761 } else command_status = 2;
jah128 0:d6269d17c8cf 762 break;
jah128 0:d6269d17c8cf 763 case 82:
jah128 0:d6269d17c8cf 764 strcpy(command,"STORE IR VALUES");
jah128 0:d6269d17c8cf 765 if(allow_commands) {
jah128 0:d6269d17c8cf 766 command_status = 1;
jah128 0:d6269d17c8cf 767 store_ir_values();
jah128 0:d6269d17c8cf 768 } else command_status = 2;
jah128 0:d6269d17c8cf 769 break;
jah128 0:d6269d17c8cf 770 case 83:
jah128 0:d6269d17c8cf 771 strcpy(command,"STORE BG BASE IR VALUES");
jah128 0:d6269d17c8cf 772 if(allow_commands) {
jah128 0:d6269d17c8cf 773 command_status = 1;
jah128 0:d6269d17c8cf 774 store_background_base_ir_values();
jah128 0:d6269d17c8cf 775 } else command_status = 2;
jah128 0:d6269d17c8cf 776 break;
jah128 0:d6269d17c8cf 777 case 84:
jah128 0:d6269d17c8cf 778 strcpy(command,"STORE IL. BASE IR VALUES");
jah128 0:d6269d17c8cf 779 if(allow_commands) {
jah128 0:d6269d17c8cf 780 command_status = 1;
jah128 0:d6269d17c8cf 781 store_illuminated_base_ir_values();
jah128 0:d6269d17c8cf 782 } else command_status = 2;
jah128 0:d6269d17c8cf 783 break;
jah128 0:d6269d17c8cf 784 case 85:
jah128 0:d6269d17c8cf 785 strcpy(command,"STORE BASE IR VALUES");
jah128 0:d6269d17c8cf 786 if(allow_commands) {
jah128 0:d6269d17c8cf 787 command_status = 1;
jah128 0:d6269d17c8cf 788 store_base_ir_values();
jah128 0:d6269d17c8cf 789 } else command_status = 2;
jah128 0:d6269d17c8cf 790 break;
jah128 0:d6269d17c8cf 791 case 86:
jah128 0:d6269d17c8cf 792 strcpy(command,"STORE ALL IR VALUES");
jah128 0:d6269d17c8cf 793 if(allow_commands) {
jah128 0:d6269d17c8cf 794 command_status = 1;
jah128 0:d6269d17c8cf 795 store_ir_values();
jah128 0:d6269d17c8cf 796 store_base_ir_values();
jah128 0:d6269d17c8cf 797 } else command_status = 2;
jah128 0:d6269d17c8cf 798 break;
jah128 0:d6269d17c8cf 799 case 90:
jah128 0:d6269d17c8cf 800 sprintf(command,"%s %d","GET BG IR VALUE",message[1]);
jah128 0:d6269d17c8cf 801 sprintf(ret_message,"%d",get_background_raw_ir_value(message[1]));
jah128 0:d6269d17c8cf 802 send_message = 1;
jah128 0:d6269d17c8cf 803 break;
jah128 0:d6269d17c8cf 804 case 91:
jah128 0:d6269d17c8cf 805 sprintf(command,"%s %d","GET IL IR VALUE",message[1]);
jah128 0:d6269d17c8cf 806 sprintf(ret_message,"%d",get_illuminated_raw_ir_value(message[1]));
jah128 0:d6269d17c8cf 807 send_message = 1;
jah128 0:d6269d17c8cf 808 break;
jah128 0:d6269d17c8cf 809 case 92:
jah128 0:d6269d17c8cf 810 strcpy(command,"GET BG IR VALUES");
jah128 0:d6269d17c8cf 811 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));
jah128 0:d6269d17c8cf 812 send_message = 1;
jah128 0:d6269d17c8cf 813 break;
jah128 0:d6269d17c8cf 814 case 93:
jah128 0:d6269d17c8cf 815 strcpy(command,"GET ILLUMINATED IR VALUES");
jah128 0:d6269d17c8cf 816 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));
jah128 0:d6269d17c8cf 817 send_message = 1;
jah128 0:d6269d17c8cf 818 break;
jah128 0:d6269d17c8cf 819 case 94:
jah128 0:d6269d17c8cf 820 sprintf(command,"%s %d","GET BG BASE IR VALUE",message[1]);
jah128 0:d6269d17c8cf 821 sprintf(ret_message,"%d",get_background_base_ir_value(message[1]));
jah128 0:d6269d17c8cf 822 send_message = 1;
jah128 0:d6269d17c8cf 823 break;
jah128 0:d6269d17c8cf 824 case 95:
jah128 0:d6269d17c8cf 825 sprintf(command,"%s %d","GET IL BASE IR VALUE",message[1]);
jah128 0:d6269d17c8cf 826 sprintf(ret_message,"%d",get_illuminated_base_ir_value(message[1]));
jah128 0:d6269d17c8cf 827 send_message = 1;
jah128 0:d6269d17c8cf 828 break;
jah128 0:d6269d17c8cf 829 case 96:
jah128 0:d6269d17c8cf 830 strcpy(command,"GET BG BASE IR VALUES");
jah128 0:d6269d17c8cf 831 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));
jah128 0:d6269d17c8cf 832 send_message = 1;
jah128 0:d6269d17c8cf 833 break;
jah128 0:d6269d17c8cf 834 case 97:
jah128 0:d6269d17c8cf 835 strcpy(command,"GET IL BASE IR VALUES");
jah128 0:d6269d17c8cf 836 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));
jah128 0:d6269d17c8cf 837 send_message = 1;
jah128 0:d6269d17c8cf 838 break;
jah128 0:d6269d17c8cf 839 case 100:
jah128 0:d6269d17c8cf 840 strcpy(command,"START FILE TRANSFER MODE");
jah128 0:d6269d17c8cf 841 if(allow_commands) {
jah128 0:d6269d17c8cf 842 command_status = 1;
jah128 1:060690a934a9 843 IF_start_file_transfer_mode();
jah128 0:d6269d17c8cf 844 sprintf(ret_message,"OK");
jah128 0:d6269d17c8cf 845 send_message = 1;
jah128 0:d6269d17c8cf 846 } else command_status = 2;
jah128 0:d6269d17c8cf 847 break;
jah128 0:d6269d17c8cf 848 }
jah128 0:d6269d17c8cf 849
jah128 0:d6269d17c8cf 850
jah128 0:d6269d17c8cf 851 if(send_message) {
jah128 0:d6269d17c8cf 852 char message_length = strlen(ret_message);
jah128 0:d6269d17c8cf 853 switch(interface) {
jah128 0:d6269d17c8cf 854 case 0:
jah128 0:d6269d17c8cf 855 pc.printf("%c%c%s",RESPONSE_MESSAGE_BYTE,message_length,ret_message);
jah128 0:d6269d17c8cf 856 break;
jah128 0:d6269d17c8cf 857 case 1:
jah128 0:d6269d17c8cf 858 bt.printf("%c%c%s",RESPONSE_MESSAGE_BYTE,message_length,ret_message);
jah128 0:d6269d17c8cf 859 break;
jah128 0:d6269d17c8cf 860 }
jah128 0:d6269d17c8cf 861 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);
jah128 0:d6269d17c8cf 862 } else {
jah128 0:d6269d17c8cf 863 switch(interface) {
jah128 0:d6269d17c8cf 864 case 0:
jah128 0:d6269d17c8cf 865 pc.printf("%c%c",ACKNOWLEDGE_MESSAGE_BYTE,command_status);
jah128 0:d6269d17c8cf 866 break;
jah128 0:d6269d17c8cf 867 case 1:
jah128 0:d6269d17c8cf 868 bt.printf("%c%c",ACKNOWLEDGE_MESSAGE_BYTE,command_status);
jah128 0:d6269d17c8cf 869 break;
jah128 0:d6269d17c8cf 870 }
jah128 0:d6269d17c8cf 871 switch(command_status) {
jah128 0:d6269d17c8cf 872 case 0:
jah128 0:d6269d17c8cf 873 debug("Unrecognised %s command message [%02x%02x%02x]\n",iface,message[0],message[1],message[2]);
jah128 0:d6269d17c8cf 874 break;
jah128 0:d6269d17c8cf 875 case 1:
jah128 0:d6269d17c8cf 876 debug("Actioned %s command message:%s %s [%02x%02x%02x]\n",iface, command, subcommand,message[0],message[1],message[2]);
jah128 0:d6269d17c8cf 877 break;
jah128 0:d6269d17c8cf 878 case 2:
jah128 0:d6269d17c8cf 879 debug("Blocked %s command message:%s %s [%02x%02x%02x]\n",iface, command, subcommand,message[0],message[1],message[2]);
jah128 0:d6269d17c8cf 880 break;
jah128 0:d6269d17c8cf 881 case 3:
jah128 0:d6269d17c8cf 882 debug("Invalid %s command message:%s %s [%02x%02x%02x]\n",iface, command, subcommand,message[0],message[1],message[2]);
jah128 0:d6269d17c8cf 883 break;
jah128 0:d6269d17c8cf 884 }
jah128 0:d6269d17c8cf 885 }
jah128 0:d6269d17c8cf 886 }
jah128 0:d6269d17c8cf 887
jah128 0:d6269d17c8cf 888 char * IF_nibble_to_binary_char(char in)
jah128 0:d6269d17c8cf 889 {
jah128 0:d6269d17c8cf 890 char * ret = (char*)malloc(sizeof(char)*5);
jah128 0:d6269d17c8cf 891 for(int i=0; i<4; i++) {
jah128 0:d6269d17c8cf 892 if(in & (128 >> i)) ret[i]='1';
jah128 0:d6269d17c8cf 893 else ret[i]='0';
jah128 0:d6269d17c8cf 894 }
jah128 0:d6269d17c8cf 895 ret[4]=0;
jah128 0:d6269d17c8cf 896 return ret;
jah128 0:d6269d17c8cf 897 }
jah128 0:d6269d17c8cf 898
jah128 0:d6269d17c8cf 899 char * IF_char_to_binary_char(char in)
jah128 0:d6269d17c8cf 900 {
jah128 0:d6269d17c8cf 901 char * ret = (char*)malloc(sizeof(char)*9);
jah128 0:d6269d17c8cf 902 for(int i=0; i<8; i++) {
jah128 0:d6269d17c8cf 903 if(in & (128 >> i)) ret[i]='1';
jah128 0:d6269d17c8cf 904 else ret[i]='0';
jah128 0:d6269d17c8cf 905 }
jah128 0:d6269d17c8cf 906 ret[8]=0;
jah128 0:d6269d17c8cf 907 return ret;
jah128 0:d6269d17c8cf 908 }
jah128 0:d6269d17c8cf 909
jah128 0:d6269d17c8cf 910 float IF_decode_unsigned_float(char byte0, char byte1)
jah128 0:d6269d17c8cf 911 {
jah128 0:d6269d17c8cf 912 unsigned short sval = (byte0) << 8;
jah128 0:d6269d17c8cf 913 sval += byte1;
jah128 0:d6269d17c8cf 914 float scaled = sval / 65535.0f;
jah128 0:d6269d17c8cf 915 return scaled;
jah128 0:d6269d17c8cf 916 }
jah128 0:d6269d17c8cf 917
jah128 0:d6269d17c8cf 918 float IF_decode_float(char byte0, char byte1)
jah128 0:d6269d17c8cf 919 {
jah128 0:d6269d17c8cf 920 // MSB is byte 0 is sign, rest is linear spread between 0 and 1
jah128 0:d6269d17c8cf 921 char sign = byte0 / 128;
jah128 0:d6269d17c8cf 922 short sval = (byte0 % 128) << 8;
jah128 0:d6269d17c8cf 923 sval += byte1;
jah128 0:d6269d17c8cf 924 float scaled = sval / 32767.0f;
jah128 0:d6269d17c8cf 925 if(sign == 0) scaled = 0-scaled;
jah128 0:d6269d17c8cf 926 return scaled;
jah128 0:d6269d17c8cf 927 }
jah128 0:d6269d17c8cf 928
jah128 0:d6269d17c8cf 929 float IF_decode_unsigned_float(char byte0)
jah128 0:d6269d17c8cf 930 {
jah128 0:d6269d17c8cf 931 unsigned short sval = (byte0);
jah128 0:d6269d17c8cf 932 float scaled = sval / 255.0f;
jah128 0:d6269d17c8cf 933 return scaled;
jah128 0:d6269d17c8cf 934 }
jah128 0:d6269d17c8cf 935
jah128 0:d6269d17c8cf 936 float IF_decode_float(char byte0)
jah128 0:d6269d17c8cf 937 {
jah128 0:d6269d17c8cf 938 // MSB is byte 0 is sign, rest is linear spread between 0 and 1
jah128 0:d6269d17c8cf 939 char sign = byte0 / 128;
jah128 0:d6269d17c8cf 940 short sval = (byte0 % 128);
jah128 0:d6269d17c8cf 941 float scaled = sval / 127.0f;
jah128 0:d6269d17c8cf 942 if(sign == 0) scaled = 0-scaled;
jah128 0:d6269d17c8cf 943 return scaled;
jah128 0:d6269d17c8cf 944 }
jah128 0:d6269d17c8cf 945
jah128 0:d6269d17c8cf 946 void IF_setup_serial_interfaces()
jah128 0:d6269d17c8cf 947 {
jah128 0:d6269d17c8cf 948 if(ENABLE_PC_SERIAL) {
jah128 0:d6269d17c8cf 949 pc.baud(PC_BAUD);
jah128 0:d6269d17c8cf 950 pc.attach(&IF_pc_rx_callback, Serial::RxIrq);
jah128 0:d6269d17c8cf 951 }
jah128 0:d6269d17c8cf 952 if(ENABLE_BLUETOOTH) {
jah128 0:d6269d17c8cf 953 bt.baud(BLUETOOTH_BAUD);
jah128 0:d6269d17c8cf 954 bt.attach(&IF_bt_rx_callback, Serial::RxIrq);
jah128 0:d6269d17c8cf 955 }
jah128 0:d6269d17c8cf 956 }
jah128 0:d6269d17c8cf 957
jah128 0:d6269d17c8cf 958 void IF_pc_rx_command_timeout()
jah128 0:d6269d17c8cf 959 {
jah128 0:d6269d17c8cf 960 char message_array[6];
jah128 0:d6269d17c8cf 961 char length = 1 + pc_command_message_byte;
jah128 0:d6269d17c8cf 962 pc_command_message_started = 0;
jah128 0:d6269d17c8cf 963 message_array[0] = COMMAND_MESSAGE_BYTE;
jah128 0:d6269d17c8cf 964 for(int k=0; k<pc_command_message_byte; k++) {
jah128 0:d6269d17c8cf 965 message_array[k+1] = pc_command_message[k];
jah128 0:d6269d17c8cf 966 }
jah128 0:d6269d17c8cf 967 IF_handle_user_serial_message(message_array, length, 0);
jah128 0:d6269d17c8cf 968 }
jah128 0:d6269d17c8cf 969
jah128 0:d6269d17c8cf 970 void IF_bt_rx_command_timeout()
jah128 0:d6269d17c8cf 971 {
jah128 0:d6269d17c8cf 972 char message_array[6];
jah128 0:d6269d17c8cf 973 char length = 1 + bt_command_message_byte;
jah128 0:d6269d17c8cf 974 bt_command_message_started = 0;
jah128 0:d6269d17c8cf 975 message_array[0] = COMMAND_MESSAGE_BYTE;
jah128 0:d6269d17c8cf 976 for(int k=0; k<bt_command_message_byte; k++) {
jah128 0:d6269d17c8cf 977 message_array[k+1] = bt_command_message[k];
jah128 0:d6269d17c8cf 978 }
jah128 0:d6269d17c8cf 979 IF_handle_user_serial_message(message_array, length, 1);
jah128 0:d6269d17c8cf 980 }
jah128 0:d6269d17c8cf 981
jah128 0:d6269d17c8cf 982 void IF_pc_rx_callback()
jah128 0:d6269d17c8cf 983 {
jah128 0:d6269d17c8cf 984 int count = 0;
jah128 0:d6269d17c8cf 985 char message_array[255];
jah128 0:d6269d17c8cf 986
jah128 0:d6269d17c8cf 987 while(pc.readable()) {
jah128 0:d6269d17c8cf 988 char tc = pc.getc();
jah128 0:d6269d17c8cf 989 message_array[count] = tc;
jah128 0:d6269d17c8cf 990 count ++;
jah128 0:d6269d17c8cf 991 if(pc_command_message_started == 1) {
jah128 0:d6269d17c8cf 992 if(pc_command_message_byte == 3) {
jah128 0:d6269d17c8cf 993 pc_command_timeout.detach();
jah128 0:d6269d17c8cf 994 if(tc == COMMAND_MESSAGE_BYTE) {
jah128 0:d6269d17c8cf 995 // A complete command message succesfully received, call handler
jah128 0:d6269d17c8cf 996 pc_command_message_started = 0;
jah128 0:d6269d17c8cf 997 count = 0;
jah128 0:d6269d17c8cf 998 IF_handle_command_serial_message(pc_command_message , 0);
jah128 0:d6269d17c8cf 999 } else {
jah128 0:d6269d17c8cf 1000 // Message is not a valid command message as 5th byte is not correct; treat whole message as a user message
jah128 0:d6269d17c8cf 1001 pc_command_message_started = 0;
jah128 0:d6269d17c8cf 1002 message_array[0] = COMMAND_MESSAGE_BYTE;
jah128 0:d6269d17c8cf 1003 message_array[1] = pc_command_message[0];
jah128 0:d6269d17c8cf 1004 message_array[2] = pc_command_message[1];
jah128 0:d6269d17c8cf 1005 message_array[3] = pc_command_message[2];
jah128 0:d6269d17c8cf 1006 message_array[4] = tc;
jah128 0:d6269d17c8cf 1007 count = 5;
jah128 0:d6269d17c8cf 1008 }
jah128 0:d6269d17c8cf 1009 } else {
jah128 0:d6269d17c8cf 1010 pc_command_message[pc_command_message_byte] = tc;
jah128 0:d6269d17c8cf 1011 pc_command_message_byte ++;
jah128 0:d6269d17c8cf 1012 }
jah128 0:d6269d17c8cf 1013 } else {
jah128 0:d6269d17c8cf 1014 if(count == 1) {
jah128 0:d6269d17c8cf 1015 if(tc == COMMAND_MESSAGE_BYTE) {
jah128 0:d6269d17c8cf 1016 pc_command_timeout.attach(&IF_pc_rx_command_timeout,command_timeout_period);
jah128 0:d6269d17c8cf 1017 pc_command_message_started = 1;
jah128 0:d6269d17c8cf 1018 pc_command_message_byte = 0;
jah128 0:d6269d17c8cf 1019
jah128 0:d6269d17c8cf 1020 }
jah128 0:d6269d17c8cf 1021 }
jah128 0:d6269d17c8cf 1022 }
jah128 0:d6269d17c8cf 1023 }
jah128 0:d6269d17c8cf 1024 if(!pc_command_message_started && count>0) IF_handle_user_serial_message(message_array, count, 0);
jah128 0:d6269d17c8cf 1025 }
jah128 0:d6269d17c8cf 1026
jah128 0:d6269d17c8cf 1027 Timeout bt_message_timeout;
jah128 0:d6269d17c8cf 1028 static float bt_message_timeout_period = 0.001; // 1 millisecond
jah128 0:d6269d17c8cf 1029 char bt_buffer[255];
jah128 0:d6269d17c8cf 1030 int bt_buffer_index = 0;
jah128 0:d6269d17c8cf 1031
jah128 0:d6269d17c8cf 1032 void IF_bt_message_timeout()
jah128 0:d6269d17c8cf 1033 {
jah128 0:d6269d17c8cf 1034 char buffer[255];
jah128 0:d6269d17c8cf 1035
jah128 0:d6269d17c8cf 1036 sprintf(buffer, bt_buffer, bt_buffer_index);
jah128 0:d6269d17c8cf 1037 buffer[bt_buffer_index] = 0;
jah128 0:d6269d17c8cf 1038 if(file_transfer_mode == 1) {
jah128 0:d6269d17c8cf 1039 IF_handle_file_transfer_serial_message(bt_buffer, bt_buffer_index, 1);
jah128 0:d6269d17c8cf 1040 } else {
jah128 0:d6269d17c8cf 1041 // debug("BT message timeout: %s [%d chars]\n", buffer, bt_buffer_index);
jah128 0:d6269d17c8cf 1042 if(bt_buffer_index == 5 && buffer[0] == COMMAND_MESSAGE_BYTE && buffer[4] == COMMAND_MESSAGE_BYTE) {
jah128 0:d6269d17c8cf 1043 bt_command_message[0] = buffer[1];
jah128 0:d6269d17c8cf 1044 bt_command_message[1] = buffer[2];
jah128 0:d6269d17c8cf 1045 bt_command_message[2] = buffer[3];
jah128 0:d6269d17c8cf 1046 IF_handle_command_serial_message(bt_command_message , 1);
jah128 0:d6269d17c8cf 1047 } else IF_handle_user_serial_message(bt_buffer, bt_buffer_index, 1);
jah128 0:d6269d17c8cf 1048 }
jah128 0:d6269d17c8cf 1049 bt_buffer_index = 0;
jah128 0:d6269d17c8cf 1050 }
jah128 0:d6269d17c8cf 1051
jah128 0:d6269d17c8cf 1052 void IF_bt_rx_callback()
jah128 0:d6269d17c8cf 1053 {
jah128 0:d6269d17c8cf 1054 while(bt.readable()) {
jah128 0:d6269d17c8cf 1055 char byte = bt.getc();
jah128 0:d6269d17c8cf 1056
jah128 0:d6269d17c8cf 1057 bt_buffer[bt_buffer_index] = byte;
jah128 0:d6269d17c8cf 1058 bt_buffer_index++;
jah128 0:d6269d17c8cf 1059 }
jah128 0:d6269d17c8cf 1060
jah128 0:d6269d17c8cf 1061 bt_message_timeout.attach(&IF_bt_message_timeout, bt_message_timeout_period);
jah128 0:d6269d17c8cf 1062 }
jah128 0:d6269d17c8cf 1063
jah128 0:d6269d17c8cf 1064 //void IF_bt_rx_callback()
jah128 0:d6269d17c8cf 1065 //{
jah128 0:d6269d17c8cf 1066 // int count = 0;
jah128 0:d6269d17c8cf 1067 // char message_array[255];
jah128 0:d6269d17c8cf 1068 //
jah128 0:d6269d17c8cf 1069 // wait_ms(500); // Wait 0.5ms to allow a complete message to arrive before atttempting to process it
jah128 0:d6269d17c8cf 1070 //
jah128 0:d6269d17c8cf 1071 // while(bt.readable()) {
jah128 0:d6269d17c8cf 1072 // char tc = bt.getc();
jah128 0:d6269d17c8cf 1073 // message_array[count] = tc;
jah128 0:d6269d17c8cf 1074 // count ++;
jah128 0:d6269d17c8cf 1075 // if(bt_command_message_started == 1) {
jah128 0:d6269d17c8cf 1076 // if(bt_command_message_byte == 3) {
jah128 0:d6269d17c8cf 1077 // bt_command_timeout.detach();
jah128 0:d6269d17c8cf 1078 // if(tc == COMMAND_MESSAGE_BYTE) {
jah128 0:d6269d17c8cf 1079 // // A complete command message succesfully received, call handler
jah128 0:d6269d17c8cf 1080 // bt_command_message_started = 0;
jah128 0:d6269d17c8cf 1081 // count = 0;
jah128 0:d6269d17c8cf 1082 // IF_handle_command_serial_message(bt_command_message , 1);
jah128 0:d6269d17c8cf 1083 // } else {
jah128 0:d6269d17c8cf 1084 // // Message is not a valid command message as 5th byte is not correct; treat whole message as a user message
jah128 0:d6269d17c8cf 1085 // bt_command_message_started = 0;
jah128 0:d6269d17c8cf 1086 // message_array[0] = COMMAND_MESSAGE_BYTE;
jah128 0:d6269d17c8cf 1087 // message_array[1] = bt_command_message[0];
jah128 0:d6269d17c8cf 1088 // message_array[2] = bt_command_message[1];
jah128 0:d6269d17c8cf 1089 // message_array[3] = bt_command_message[2];
jah128 0:d6269d17c8cf 1090 // message_array[4] = tc;
jah128 0:d6269d17c8cf 1091 // count = 5;
jah128 0:d6269d17c8cf 1092 // }
jah128 0:d6269d17c8cf 1093 // } else {
jah128 0:d6269d17c8cf 1094 // bt_command_timeout.attach(&IF_bt_rx_command_timeout,command_timeout_period);
jah128 0:d6269d17c8cf 1095 // bt_command_message[bt_command_message_byte] = tc;
jah128 0:d6269d17c8cf 1096 // bt_command_message_byte ++;
jah128 0:d6269d17c8cf 1097 // }
jah128 0:d6269d17c8cf 1098 // } else {
jah128 0:d6269d17c8cf 1099 // if(count == 1) {
jah128 0:d6269d17c8cf 1100 // if(tc == COMMAND_MESSAGE_BYTE) {
jah128 0:d6269d17c8cf 1101 // bt_command_message_started = 1;
jah128 0:d6269d17c8cf 1102 // bt_command_message_byte = 0;
jah128 0:d6269d17c8cf 1103 //
jah128 0:d6269d17c8cf 1104 // }
jah128 0:d6269d17c8cf 1105 // }
jah128 0:d6269d17c8cf 1106 // }
jah128 0:d6269d17c8cf 1107 // }
jah128 0:d6269d17c8cf 1108 // if(!bt_command_message_started && count>0) IF_handle_user_serial_message(message_array, count, 1);
jah128 0:d6269d17c8cf 1109 //}