James Hilder / PsiSwarmV9-Fork

Psi Swarm robot library version 0.9

Fork of PsiSwarmV9 by Psi Swarm Robot

serial.cpp@20:1bc6c6dc477b, 2018-05-14 (annotated)

Committer:
jah128
Date:
Mon May 14 15:35:38 2018 +0000
Revision:
20:1bc6c6dc477b
Parent:
16:50686c07ad07 
Updated?

Who changed what in which revision?

UserRevisionLine numberNew contents of line
jah128 11:312663037b8c 1 /* University of York Robotics Laboratory PsiSwarm Library: SerialControlControlControlControlControl Control Source File
jah128 10:e58323951c08 2 *
jah128 16:50686c07ad07 3 * Copyright 2017 University of York
jah128 6:b340a527add9 4 *
jah128 10:e58323951c08 5 * Licensed under the Apache License, Version 2.0 (the "License"); you may not use this file except in compliance with the License.
jah128 6:b340a527add9 6 * You may obtain a copy of the License at http://www.apache.org/licenses/LICENSE-2.0
jah128 6:b340a527add9 7 * Unless required by applicable law or agreed to in writing, software distributed under the License is distributed on an "AS IS" BASIS
jah128 10:e58323951c08 8 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
jah128 6:b340a527add9 9 * See the License for the specific language governing permissions and limitations under the License.
jah128 0:d6269d17c8cf 10 *
jah128 0:d6269d17c8cf 11 * File: serial.cpp
jah128 0:d6269d17c8cf 12 *
jah128 0:d6269d17c8cf 13 * (C) Dept. Electronics & Computer Science, University of York
jah128 0:d6269d17c8cf 14 * James Hilder, Alan Millard, Alexander Horsfield, Homero Elizondo, Jon Timmis
jah128 0:d6269d17c8cf 15 *
jah128 16:50686c07ad07 16 * PsiSwarm Library Version: 0.9
jah128 0:d6269d17c8cf 17 *
jah128 16:50686c07ad07 18 * June 2017
jah128 0:d6269d17c8cf 19 *
jah128 0:d6269d17c8cf 20 *
jah128 0:d6269d17c8cf 21 */
jah128 0:d6269d17c8cf 22
jah128 0:d6269d17c8cf 23 #include "psiswarm.h"
jah128 0:d6269d17c8cf 24
jah128 0:d6269d17c8cf 25 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 26 char pc_command_message_started = 0;
jah128 0:d6269d17c8cf 27 char pc_command_message_byte = 0;
jah128 0:d6269d17c8cf 28 char pc_command_message[3];
jah128 0:d6269d17c8cf 29 char bt_command_message_started = 0;
jah128 0:d6269d17c8cf 30 char bt_command_message_byte = 0;
jah128 0:d6269d17c8cf 31 char bt_command_message[3];
jah128 0:d6269d17c8cf 32
jah128 0:d6269d17c8cf 33 char allow_commands = 1;
jah128 0:d6269d17c8cf 34 char allow_requests = 1;
jah128 2:c6986ee3c7c5 35 char file_transfer_state = 0;
jah128 0:d6269d17c8cf 36
jah128 2:c6986ee3c7c5 37 int block_size = 88; // The data block size for file transfer
jah128 2:c6986ee3c7c5 38 char data_block[89]; // Stores the data block to write for Bluetooth file transfer
jah128 2:c6986ee3c7c5 39 int data_written; // Stores if partial data has been written to a file
jah128 2:c6986ee3c7c5 40 int file_length; // Stores the file length for a Bluetooth file transfer
jah128 2:c6986ee3c7c5 41 int final_block; // Stores the index of the final data block for a Bluetooth file transfer
jah128 2:c6986ee3c7c5 42 int block_index; // Stores the current block index for a Bluetooth file transfer
jah128 2:c6986ee3c7c5 43 char filename [21]; // Stores the filename for a Bluetooth file transfer
jah128 1:060690a934a9 44
jah128 1:060690a934a9 45 Timeout ft_timeout;
jah128 0:d6269d17c8cf 46 Timeout pc_command_timeout;
jah128 0:d6269d17c8cf 47 Timeout bt_command_timeout;
jah128 0:d6269d17c8cf 48
jah128 0:d6269d17c8cf 49 // A predefined message structure for command messages is as follows:
jah128 0:d6269d17c8cf 50 // [Byte 0][Byte 1][Byte 2][Byte 3][Byte 4]
jah128 0:d6269d17c8cf 51 // 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 52
jah128 0:d6269d17c8cf 53
jah128 11:312663037b8c 54 void SerialControl::setup_serial_interfaces()
jah128 11:312663037b8c 55 {
jah128 11:312663037b8c 56 if(ENABLE_PC_SERIAL) {
jah128 11:312663037b8c 57 pc.baud(PC_BAUD);
jah128 11:312663037b8c 58 pc.attach(this,&SerialControl::IF_pc_rx_callback, Serial::RxIrq);
jah128 11:312663037b8c 59 }
jah128 11:312663037b8c 60 if(ENABLE_BLUETOOTH) {
jah128 11:312663037b8c 61 bt.baud(BLUETOOTH_BAUD);
jah128 11:312663037b8c 62 bt.attach(this,&SerialControl::IF_bt_rx_callback, Serial::RxIrq);
jah128 11:312663037b8c 63 }
jah128 11:312663037b8c 64 }
jah128 0:d6269d17c8cf 65
jah128 0:d6269d17c8cf 66
jah128 11:312663037b8c 67 void SerialControl::IF_start_file_transfer_mode()
jah128 1:060690a934a9 68 {
jah128 1:060690a934a9 69 display.clear_display();
jah128 1:060690a934a9 70 display.set_position(0,0);
jah128 1:060690a934a9 71 display.write_string("FILE TRANSFER");
jah128 1:060690a934a9 72 display.set_position(1,0);
jah128 1:060690a934a9 73 display.write_string("MODE...");
jah128 2:c6986ee3c7c5 74 data_written = 0;
jah128 1:060690a934a9 75 file_transfer_mode = 1;
jah128 1:060690a934a9 76 file_transfer_state = 0;
jah128 1:060690a934a9 77 file_length = 0;
jah128 1:060690a934a9 78 user_code_restore_mode = user_code_running;
jah128 1:060690a934a9 79 user_code_running = 0;
jah128 11:312663037b8c 80 ft_timeout.attach(this,&SerialControl::IF_file_transfer_timeout,2.0);
jah128 1:060690a934a9 81 }
jah128 1:060690a934a9 82
jah128 1:060690a934a9 83
jah128 11:312663037b8c 84 void SerialControl::IF_invalid_transfer(void)
jah128 1:060690a934a9 85 {
jah128 12:878c6e9d9e60 86 psi.debug("File transfer failed\n");
jah128 10:e58323951c08 87 if(data_written == 1) {
jah128 12:878c6e9d9e60 88 psi.debug("Deleting corrupted file\n");
jah128 2:c6986ee3c7c5 89 remove(filename);
jah128 2:c6986ee3c7c5 90 }
jah128 1:060690a934a9 91 display.clear_display();
jah128 1:060690a934a9 92 display.set_position(0,0);
jah128 1:060690a934a9 93 display.write_string("TRANSFER FAILED");
jah128 1:060690a934a9 94 wait(0.5);
jah128 1:060690a934a9 95 IF_end_file_transfer_mode();
jah128 1:060690a934a9 96 }
jah128 1:060690a934a9 97
jah128 11:312663037b8c 98 void SerialControl::IF_file_transfer_timeout(void)
jah128 1:060690a934a9 99 {
jah128 12:878c6e9d9e60 100 psi.debug("File transfer failed: timeout\n");
jah128 1:060690a934a9 101 display.clear_display();
jah128 1:060690a934a9 102 display.set_position(0,0);
jah128 1:060690a934a9 103 display.write_string("TRANSFER TIMEOUT");
jah128 1:060690a934a9 104 wait(0.5);
jah128 1:060690a934a9 105 IF_end_file_transfer_mode();
jah128 1:060690a934a9 106 }
jah128 1:060690a934a9 107
jah128 11:312663037b8c 108 void SerialControl::IF_end_file_transfer_mode(void)
jah128 1:060690a934a9 109 {
jah128 1:060690a934a9 110 display.clear_display();
jah128 1:060690a934a9 111 file_transfer_mode = 0;
jah128 1:060690a934a9 112 user_code_running = user_code_restore_mode;
jah128 1:060690a934a9 113 }
jah128 1:060690a934a9 114
jah128 0:d6269d17c8cf 115
jah128 11:312663037b8c 116 void SerialControl::IF_handle_file_transfer_serial_message(char * message, char length, char interface)
jah128 0:d6269d17c8cf 117 {
jah128 0:d6269d17c8cf 118 // Code for handling a serial (Bluetooth) message when in file-transfer mode
jah128 0:d6269d17c8cf 119 //
jah128 0:d6269d17c8cf 120 // message = pointer to message char array
jah128 0:d6269d17c8cf 121 // length = length of message
jah128 0:d6269d17c8cf 122 // interface = 0 for PC serial connection, 1 for Bluetooth [NB only Bluetooth used for file transfer in this version]
jah128 0:d6269d17c8cf 123
jah128 12:878c6e9d9e60 124 if(file_transfer_state < 2)psi.debug("FTM Message:%.*s [%d]\n",length,message,length);
jah128 12:878c6e9d9e60 125 else psi.debug("FTM data block received (%d bytes)\n",length);
jah128 1:060690a934a9 126 int expected_size;
jah128 1:060690a934a9 127 // The first byte in EVERY message received should be 33; if it isn't, abort the transfer
jah128 1:060690a934a9 128 if(message[0] != 33) {
jah128 1:060690a934a9 129 IF_invalid_transfer();
jah128 1:060690a934a9 130 } else {
jah128 1:060690a934a9 131 switch(file_transfer_state) {
jah128 1:060690a934a9 132 case 0: //First message received is the target filename
jah128 1:060690a934a9 133 //The filenames cannot be more that 8.3 characters long (FAT12 format)
jah128 1:060690a934a9 134 if(length == 1 || length > 13) IF_invalid_transfer();
jah128 1:060690a934a9 135 else {
jah128 2:c6986ee3c7c5 136 strcpy(filename, "/local/");
jah128 2:c6986ee3c7c5 137 strncat(filename, message + 1, length - 1);
jah128 12:878c6e9d9e60 138 psi.debug("Target filename:%s\n",filename);
jah128 1:060690a934a9 139 //Send acknowledge ("FN")
jah128 1:060690a934a9 140 ft_timeout.detach();
jah128 11:312663037b8c 141 ft_timeout.attach(this,&SerialControl::IF_file_transfer_timeout,2.0);
jah128 1:060690a934a9 142 bt.printf("%c%c%s",RESPONSE_MESSAGE_BYTE,2,"FN");
jah128 1:060690a934a9 143 file_transfer_state = 1;
jah128 1:060690a934a9 144 }
jah128 1:060690a934a9 145 break;
jah128 1:060690a934a9 146 case 1: //Second message is the length of the file in bytes
jah128 1:060690a934a9 147 //Length is encoded as a 3-byte value
jah128 1:060690a934a9 148 if(length != 4) IF_invalid_transfer();
jah128 1:060690a934a9 149 else {
jah128 1:060690a934a9 150 file_length = (message[1]) * 256;
jah128 1:060690a934a9 151 file_length += (message[2]);
jah128 1:060690a934a9 152 file_length *= 256;
jah128 1:060690a934a9 153 file_length += message[3];
jah128 1:060690a934a9 154 file_transfer_state = 2;
jah128 1:060690a934a9 155 display.clear_display();
jah128 1:060690a934a9 156 char display_message[17];
jah128 1:060690a934a9 157 sprintf(display_message,"F:%s",filename);
jah128 1:060690a934a9 158 display.set_position(0,0);
jah128 1:060690a934a9 159 display.write_string(display_message);
jah128 1:060690a934a9 160 display.set_position(1,0);
jah128 1:060690a934a9 161 sprintf(display_message,"S:%d b",file_length);
jah128 1:060690a934a9 162 display.write_string(display_message);
jah128 1:060690a934a9 163 block_index = 0;
jah128 1:060690a934a9 164 //Work out how many blocks the file will be sent in (size = block_size, tested at 100 bytes)
jah128 1:060690a934a9 165 //Allocate memory for the file up to a limit of 16 blocks; larger files will be split across
jah128 1:060690a934a9 166 //multiple blocks....
jah128 1:060690a934a9 167 final_block = file_length / block_size;
jah128 1:060690a934a9 168 if(file_length % block_size != 0) final_block ++;
jah128 2:c6986ee3c7c5 169 //int target_size = file_length;
jah128 2:c6986ee3c7c5 170 //if(file_length > (block_size * 16)) target_size = block_size * 16;
jah128 2:c6986ee3c7c5 171 //file_data = (char *) malloc(target_size);
jah128 12:878c6e9d9e60 172 psi.debug("File size %d bytes (%d blocks of %d bytes)\n",file_length,final_block,block_size);
jah128 1:060690a934a9 173 ft_timeout.detach();
jah128 11:312663037b8c 174 ft_timeout.attach(this,&SerialControl::IF_file_transfer_timeout,1.0);
jah128 1:060690a934a9 175 //Send acknowledge (size of file)
jah128 1:060690a934a9 176 bt.printf("%c%c%c%c%c",RESPONSE_MESSAGE_BYTE,3,message[1],message[2],message[3]);
jah128 1:060690a934a9 177 }
jah128 1:060690a934a9 178 break;
jah128 1:060690a934a9 179 case 2:
jah128 1:060690a934a9 180 block_index ++;
jah128 2:c6986ee3c7c5 181 display.clear_display();
jah128 2:c6986ee3c7c5 182 display.set_position(0,0);
jah128 2:c6986ee3c7c5 183 display.write_string("FILE TRANSFER");
jah128 2:c6986ee3c7c5 184 display.set_position(1,0);
jah128 2:c6986ee3c7c5 185 char details_string[17];
jah128 2:c6986ee3c7c5 186 sprintf(details_string,"BLOCK %d OF %d",block_index,final_block);
jah128 2:c6986ee3c7c5 187 display.write_string(details_string);
jah128 2:c6986ee3c7c5 188 expected_size = block_size;
jah128 1:060690a934a9 189 if(block_index == final_block) expected_size = file_length % block_size;
jah128 1:060690a934a9 190 if(expected_size == 0) expected_size = block_size;
jah128 10:e58323951c08 191 if(length!=expected_size + 1) {
jah128 10:e58323951c08 192 // Unexpected length
jah128 12:878c6e9d9e60 193 psi.debug("File data unexpected length in packet %d (%d bytes received, %d bytes expected)\n",block_index,length-1,expected_size);
jah128 10:e58323951c08 194 } else {
jah128 10:e58323951c08 195 char transfer_mode[2]= {'a'};
jah128 10:e58323951c08 196 if(block_index == 1) {
jah128 2:c6986ee3c7c5 197 transfer_mode[0]='w';
jah128 2:c6986ee3c7c5 198 }
jah128 2:c6986ee3c7c5 199 FILE *fp = fopen(filename,transfer_mode);
jah128 2:c6986ee3c7c5 200 //strncpy(data_block,message+1,length);
jah128 2:c6986ee3c7c5 201 //data_block[length]=0;
jah128 2:c6986ee3c7c5 202 //fprintf(fp,data_block);
jah128 2:c6986ee3c7c5 203 int bytes_written;
jah128 2:c6986ee3c7c5 204 bytes_written = fwrite(message+1,expected_size,1,fp);
jah128 2:c6986ee3c7c5 205 fclose(fp);
jah128 2:c6986ee3c7c5 206 if(data_written == false && bytes_written > 0) data_written = true;
jah128 12:878c6e9d9e60 207 psi.debug("Bytes written: %d\n",expected_size * bytes_written);
jah128 10:e58323951c08 208 if(block_index < final_block) {
jah128 12:878c6e9d9e60 209 psi.debug("Message packet %d received and written\n",block_index);
jah128 10:e58323951c08 210 //Send acknowledge ("D")
jah128 10:e58323951c08 211 ft_timeout.detach();
jah128 11:312663037b8c 212 ft_timeout.attach(this,&SerialControl::IF_file_transfer_timeout,1.0);
jah128 10:e58323951c08 213 bt.printf("%c%c%s",RESPONSE_MESSAGE_BYTE,1,"D");
jah128 10:e58323951c08 214 } else {
jah128 2:c6986ee3c7c5 215 //Last data block written
jah128 2:c6986ee3c7c5 216 //[Put file checking code here]
jah128 2:c6986ee3c7c5 217 //Send acknowledge ("P");
jah128 2:c6986ee3c7c5 218 bt.printf("%c%c%s",RESPONSE_MESSAGE_BYTE,1,"F");
jah128 2:c6986ee3c7c5 219 ft_timeout.detach();
jah128 12:878c6e9d9e60 220 psi.debug("File transfer completed successfully\n");
jah128 2:c6986ee3c7c5 221 wait(0.25);
jah128 2:c6986ee3c7c5 222 //Calculate CRC16 value for file
jah128 2:c6986ee3c7c5 223 IF_calculateCRC16(file_length);
jah128 10:e58323951c08 224
jah128 2:c6986ee3c7c5 225 display.clear_display();
jah128 2:c6986ee3c7c5 226 display.write_string("FILE TRANSFER");
jah128 2:c6986ee3c7c5 227 display.set_position(1,0);
jah128 10:e58323951c08 228 display.write_string("COMPLETE");
jah128 2:c6986ee3c7c5 229 wait(1);
jah128 12:878c6e9d9e60 230 psi.debug("File transfer mode ended\n");
jah128 2:c6986ee3c7c5 231 IF_end_file_transfer_mode();
jah128 2:c6986ee3c7c5 232 }
jah128 1:060690a934a9 233 }
jah128 2:c6986ee3c7c5 234 break;
jah128 1:060690a934a9 235 }
jah128 1:060690a934a9 236 }
jah128 0:d6269d17c8cf 237 }
jah128 0:d6269d17c8cf 238
jah128 0:d6269d17c8cf 239
jah128 11:312663037b8c 240 void SerialControl::IF_handle_user_serial_message(char * message, char length, char interface)
jah128 0:d6269d17c8cf 241 {
jah128 0:d6269d17c8cf 242 char buffer[255];
jah128 0:d6269d17c8cf 243 sprintf(buffer,message,length);
jah128 0:d6269d17c8cf 244 for(int i=0; i<length; i++) {
jah128 0:d6269d17c8cf 245 buffer[i]=message[i];
jah128 0:d6269d17c8cf 246 }
jah128 0:d6269d17c8cf 247 buffer[length]=0;
jah128 0:d6269d17c8cf 248 // if(interface) debug("Received BT message:%s [%d chars]\n",buffer,length);
jah128 0:d6269d17c8cf 249 // else debug("Received USB message:%s [%d chars]\n",buffer,length);
jah128 0:d6269d17c8cf 250 handle_user_serial_message(message,length,interface);
jah128 0:d6269d17c8cf 251 }
jah128 0:d6269d17c8cf 252
jah128 11:312663037b8c 253 void SerialControl::IF_handle_command_serial_message(char message[3], char interface)
jah128 0:d6269d17c8cf 254 {
jah128 0:d6269d17c8cf 255 char iface [4];
jah128 0:d6269d17c8cf 256 if(interface) strcpy(iface,"BT");
jah128 0:d6269d17c8cf 257 else strcpy(iface,"USB");
jah128 0:d6269d17c8cf 258 char command [26];
jah128 0:d6269d17c8cf 259 char subcommand[30];
jah128 0:d6269d17c8cf 260 float dec;
jah128 0:d6269d17c8cf 261 float l_dec;
jah128 0:d6269d17c8cf 262 float r_dec;
jah128 0:d6269d17c8cf 263 int irp_delay;
jah128 0:d6269d17c8cf 264 char colour_string[7];
jah128 0:d6269d17c8cf 265 char ret_message[50];
jah128 0:d6269d17c8cf 266 char send_message = 0;
jah128 0:d6269d17c8cf 267 char command_status = 0;
jah128 0:d6269d17c8cf 268 // command_status values:
jah128 0:d6269d17c8cf 269 // 0 - unrecognised command
jah128 0:d6269d17c8cf 270 // 1 - command actioned
jah128 0:d6269d17c8cf 271 // 2 - command blocked
jah128 0:d6269d17c8cf 272 // 3 - invalid parameters
jah128 0:d6269d17c8cf 273
jah128 0:d6269d17c8cf 274 subcommand[0]=0;
jah128 0:d6269d17c8cf 275 command[0]=0;
jah128 0:d6269d17c8cf 276 switch(message[0]) {
jah128 0:d6269d17c8cf 277
jah128 0:d6269d17c8cf 278 // MOTOR COMMANDS
jah128 0:d6269d17c8cf 279
jah128 0:d6269d17c8cf 280 case 1:
jah128 0:d6269d17c8cf 281 strcpy(command,"SET LEFT MOTOR");
jah128 0:d6269d17c8cf 282 dec = IF_decode_float(message[1],message[2]);
jah128 0:d6269d17c8cf 283 sprintf(subcommand,"%1.5f",dec);
jah128 0:d6269d17c8cf 284 if(allow_commands) {
jah128 0:d6269d17c8cf 285 command_status = 1;
jah128 8:6c92789d5f87 286 motors.set_left_motor_speed(dec);
jah128 0:d6269d17c8cf 287 } else command_status = 2;
jah128 0:d6269d17c8cf 288 break;
jah128 0:d6269d17c8cf 289 case 2:
jah128 0:d6269d17c8cf 290 strcpy(command,"SET RIGHT MOTOR");
jah128 0:d6269d17c8cf 291 dec = IF_decode_float(message[1],message[2]);
jah128 0:d6269d17c8cf 292 sprintf(subcommand,"%1.5f",dec);
jah128 0:d6269d17c8cf 293 if(allow_commands) {
jah128 8:6c92789d5f87 294 motors.set_right_motor_speed(dec);
jah128 0:d6269d17c8cf 295 command_status = 1;
jah128 0:d6269d17c8cf 296 } else command_status = 2;
jah128 0:d6269d17c8cf 297 break;
jah128 0:d6269d17c8cf 298 case 3:
jah128 0:d6269d17c8cf 299 strcpy(command,"SET BOTH MOTORS");
jah128 0:d6269d17c8cf 300 dec = IF_decode_float(message[1],message[2]);
jah128 0:d6269d17c8cf 301 sprintf(subcommand,"%1.5f",dec);
jah128 0:d6269d17c8cf 302 if(allow_commands) {
jah128 0:d6269d17c8cf 303 command_status = 1;
jah128 8:6c92789d5f87 304 motors.forward(dec);
jah128 0:d6269d17c8cf 305 } else command_status = 2;
jah128 0:d6269d17c8cf 306 break;
jah128 0:d6269d17c8cf 307 case 4:
jah128 0:d6269d17c8cf 308 strcpy(command,"BRAKE LEFT MOTOR");
jah128 0:d6269d17c8cf 309 sprintf(subcommand,"");
jah128 0:d6269d17c8cf 310 if(allow_commands) {
jah128 0:d6269d17c8cf 311 command_status = 1;
jah128 8:6c92789d5f87 312 motors.brake_left_motor();
jah128 0:d6269d17c8cf 313 } else command_status = 2;
jah128 0:d6269d17c8cf 314 break;
jah128 0:d6269d17c8cf 315 case 5:
jah128 0:d6269d17c8cf 316 strcpy(command,"BRAKE RIGHT MOTOR");
jah128 0:d6269d17c8cf 317 sprintf(subcommand,"");
jah128 0:d6269d17c8cf 318 if(allow_commands) {
jah128 0:d6269d17c8cf 319 command_status = 1;
jah128 8:6c92789d5f87 320 motors.brake_right_motor();
jah128 0:d6269d17c8cf 321 } else command_status = 2;
jah128 0:d6269d17c8cf 322 break;
jah128 0:d6269d17c8cf 323 case 6:
jah128 0:d6269d17c8cf 324 strcpy(command,"BRAKE BOTH MOTORS");
jah128 0:d6269d17c8cf 325 sprintf(subcommand,"");
jah128 0:d6269d17c8cf 326 if(allow_commands) {
jah128 0:d6269d17c8cf 327 command_status = 1;
jah128 8:6c92789d5f87 328 motors.brake();
jah128 0:d6269d17c8cf 329 } else command_status = 2;
jah128 0:d6269d17c8cf 330 break;
jah128 0:d6269d17c8cf 331 case 7:
jah128 0:d6269d17c8cf 332 strcpy(command,"STOP BOTH MOTORS");
jah128 0:d6269d17c8cf 333 sprintf(subcommand,"");
jah128 0:d6269d17c8cf 334 if(allow_commands) {
jah128 0:d6269d17c8cf 335 command_status = 1;
jah128 8:6c92789d5f87 336 motors.stop();
jah128 0:d6269d17c8cf 337 } else command_status = 2;
jah128 0:d6269d17c8cf 338 break;
jah128 0:d6269d17c8cf 339 case 8:
jah128 0:d6269d17c8cf 340 strcpy(command,"TURN ON SPOT");
jah128 0:d6269d17c8cf 341 dec = IF_decode_float(message[1],message[2]);
jah128 0:d6269d17c8cf 342 sprintf(subcommand,"%1.5f",dec);
jah128 0:d6269d17c8cf 343 if(allow_commands) {
jah128 0:d6269d17c8cf 344 command_status = 1;
jah128 8:6c92789d5f87 345 motors.turn(dec);
jah128 0:d6269d17c8cf 346 } else command_status = 2;
jah128 0:d6269d17c8cf 347 break;
jah128 0:d6269d17c8cf 348 case 9:
jah128 0:d6269d17c8cf 349 strcpy(command,"SET EACH MOTOR");
jah128 0:d6269d17c8cf 350 l_dec = IF_decode_float(message[1]);
jah128 0:d6269d17c8cf 351 r_dec = IF_decode_float(message[2]);
jah128 0:d6269d17c8cf 352 sprintf(subcommand,"L=%1.3f R=%1.3f",l_dec,r_dec);
jah128 0:d6269d17c8cf 353 if(allow_commands) {
jah128 0:d6269d17c8cf 354 command_status = 1;
jah128 8:6c92789d5f87 355 motors.set_left_motor_speed(l_dec);
jah128 8:6c92789d5f87 356 motors.set_right_motor_speed(r_dec);
jah128 0:d6269d17c8cf 357 } else command_status = 2;
jah128 0:d6269d17c8cf 358 break;
jah128 0:d6269d17c8cf 359 // LED COMMANDS
jah128 0:d6269d17c8cf 360
jah128 0:d6269d17c8cf 361 case 10:
jah128 0:d6269d17c8cf 362 strcpy(command,"SET LED STATES");
jah128 0:d6269d17c8cf 363 sprintf(subcommand,"G:%s R:%s",IF_char_to_binary_char(message[1]), IF_char_to_binary_char(message[2]));
jah128 0:d6269d17c8cf 364 if(allow_commands) {
jah128 0:d6269d17c8cf 365 command_status = 1;
jah128 9:dde9e21030eb 366 led.set_leds(message[1],message[2]);
jah128 0:d6269d17c8cf 367 } else command_status = 2;
jah128 0:d6269d17c8cf 368 break;
jah128 0:d6269d17c8cf 369 case 11:
jah128 0:d6269d17c8cf 370 strcpy(command,"SET RED LED STATES");
jah128 0:d6269d17c8cf 371 sprintf(subcommand,"%s",IF_char_to_binary_char(message[1]));
jah128 0:d6269d17c8cf 372 if(allow_commands) {
jah128 0:d6269d17c8cf 373 command_status = 1;
jah128 9:dde9e21030eb 374 led.set_red_leds(message[1]);
jah128 0:d6269d17c8cf 375 } else command_status = 2;
jah128 0:d6269d17c8cf 376 break;
jah128 0:d6269d17c8cf 377 case 12:
jah128 0:d6269d17c8cf 378 strcpy(command,"SET GREEN LED STATES");
jah128 0:d6269d17c8cf 379 sprintf(subcommand,"%s",IF_char_to_binary_char(message[1]));
jah128 0:d6269d17c8cf 380 if(allow_commands) {
jah128 0:d6269d17c8cf 381 command_status = 1;
jah128 9:dde9e21030eb 382 led.set_green_leds(message[1]);
jah128 0:d6269d17c8cf 383 } else command_status = 2;
jah128 0:d6269d17c8cf 384 break;
jah128 0:d6269d17c8cf 385 case 13:
jah128 0:d6269d17c8cf 386 strcpy(command,"SET LED");
jah128 0:d6269d17c8cf 387 switch(message[2]) {
jah128 0:d6269d17c8cf 388 case 1:
jah128 0:d6269d17c8cf 389 strcpy(colour_string,"RED");
jah128 0:d6269d17c8cf 390 break;
jah128 0:d6269d17c8cf 391 case 2:
jah128 0:d6269d17c8cf 392 strcpy(colour_string,"GREEN");
jah128 0:d6269d17c8cf 393 break;
jah128 0:d6269d17c8cf 394 case 3:
jah128 0:d6269d17c8cf 395 strcpy(colour_string,"BOTH");
jah128 0:d6269d17c8cf 396 break;
jah128 0:d6269d17c8cf 397 case 0:
jah128 0:d6269d17c8cf 398 strcpy(colour_string,"OFF");
jah128 0:d6269d17c8cf 399 break;
jah128 0:d6269d17c8cf 400 }
jah128 0:d6269d17c8cf 401 if(message[1] < 8 && message[2] < 4) {
jah128 0:d6269d17c8cf 402 sprintf(subcommand,"%d %s",message[1],colour_string);
jah128 0:d6269d17c8cf 403 if(allow_commands) {
jah128 0:d6269d17c8cf 404 command_status = 1;
jah128 9:dde9e21030eb 405 led.set_led(message[1],message[2]);
jah128 0:d6269d17c8cf 406 } else command_status = 2;
jah128 0:d6269d17c8cf 407 } else {
jah128 0:d6269d17c8cf 408 sprintf(subcommand,"[INVALID CODE]");
jah128 0:d6269d17c8cf 409 command_status = 3;
jah128 0:d6269d17c8cf 410 }
jah128 0:d6269d17c8cf 411 break;
jah128 0:d6269d17c8cf 412 case 14:
jah128 0:d6269d17c8cf 413 strcpy(command,"SET CENTER LED STATE");
jah128 0:d6269d17c8cf 414 switch(message[1]) {
jah128 0:d6269d17c8cf 415 case 1:
jah128 0:d6269d17c8cf 416 strcpy(colour_string,"RED");
jah128 0:d6269d17c8cf 417 break;
jah128 0:d6269d17c8cf 418 case 2:
jah128 0:d6269d17c8cf 419 strcpy(colour_string,"GREEN");
jah128 0:d6269d17c8cf 420 break;
jah128 0:d6269d17c8cf 421 case 3:
jah128 0:d6269d17c8cf 422 strcpy(colour_string,"BOTH");
jah128 0:d6269d17c8cf 423 break;
jah128 0:d6269d17c8cf 424 case 0:
jah128 0:d6269d17c8cf 425 strcpy(colour_string,"OFF");
jah128 0:d6269d17c8cf 426 break;
jah128 0:d6269d17c8cf 427 }
jah128 0:d6269d17c8cf 428 if(message[1] < 4) {
jah128 0:d6269d17c8cf 429 sprintf(subcommand,"%s",colour_string);
jah128 0:d6269d17c8cf 430 if(allow_commands) {
jah128 0:d6269d17c8cf 431 command_status = 1;
jah128 9:dde9e21030eb 432 led.set_center_led(message[1]);
jah128 0:d6269d17c8cf 433 } else command_status = 2;
jah128 0:d6269d17c8cf 434 } else {
jah128 0:d6269d17c8cf 435 sprintf(subcommand,"[INVALID CODE]");
jah128 0:d6269d17c8cf 436 command_status = 3;
jah128 0:d6269d17c8cf 437 }
jah128 0:d6269d17c8cf 438 break;
jah128 0:d6269d17c8cf 439 case 15:
jah128 0:d6269d17c8cf 440 strcpy(command,"SET C.LED BRIGHTNESS");
jah128 0:d6269d17c8cf 441 dec = IF_decode_unsigned_float(message[1],message[2]);
jah128 0:d6269d17c8cf 442 sprintf(subcommand,"%1.5f",dec);
jah128 0:d6269d17c8cf 443 if(allow_commands) {
jah128 0:d6269d17c8cf 444 command_status = 1;
jah128 9:dde9e21030eb 445 led.set_center_led_brightness(dec);
jah128 0:d6269d17c8cf 446 } else command_status = 2;
jah128 0:d6269d17c8cf 447 break;
jah128 0:d6269d17c8cf 448 case 16:
jah128 0:d6269d17c8cf 449 strcpy(command,"SET MBED LEDS");
jah128 0:d6269d17c8cf 450 sprintf(subcommand,"%s",IF_nibble_to_binary_char(message[1]));
jah128 0:d6269d17c8cf 451 if(allow_commands) {
jah128 0:d6269d17c8cf 452 command_status = 1;
jah128 0:d6269d17c8cf 453 mbed_led1 = (message[1] & 128) >> 7;
jah128 0:d6269d17c8cf 454 mbed_led2 = (message[1] & 64) >> 6;
jah128 0:d6269d17c8cf 455 mbed_led3 = (message[1] & 32) >> 5;
jah128 0:d6269d17c8cf 456 mbed_led4 = (message[1] & 16) >> 4;
jah128 0:d6269d17c8cf 457 } else command_status = 2;
jah128 0:d6269d17c8cf 458 break;
jah128 0:d6269d17c8cf 459 case 17:
jah128 0:d6269d17c8cf 460 strcpy(command,"BLINK OUTER LEDS");
jah128 0:d6269d17c8cf 461 dec = IF_decode_unsigned_float(message[1],message[2]);
jah128 0:d6269d17c8cf 462 sprintf(subcommand,"FOR %1.5fS",dec);
jah128 0:d6269d17c8cf 463 if(allow_commands) {
jah128 0:d6269d17c8cf 464 command_status = 1;
jah128 9:dde9e21030eb 465 led.blink_leds(dec);
jah128 0:d6269d17c8cf 466 } else command_status = 2;
jah128 0:d6269d17c8cf 467 break;
jah128 0:d6269d17c8cf 468 case 18:
jah128 0:d6269d17c8cf 469 strcpy(command,"SET BASE LED STATE");
jah128 0:d6269d17c8cf 470 switch(message[1]) {
jah128 0:d6269d17c8cf 471 case 1:
jah128 0:d6269d17c8cf 472 strcpy(subcommand,"ON");
jah128 0:d6269d17c8cf 473 break;
jah128 0:d6269d17c8cf 474 case 0:
jah128 0:d6269d17c8cf 475 strcpy(subcommand,"OFF");
jah128 0:d6269d17c8cf 476 break;
jah128 0:d6269d17c8cf 477 }
jah128 9:dde9e21030eb 478 if(allow_commands) {
jah128 9:dde9e21030eb 479 command_status = 1;
jah128 9:dde9e21030eb 480 led.set_base_led(message[1]);
jah128 9:dde9e21030eb 481 } else command_status = 2;
jah128 0:d6269d17c8cf 482 break;
jah128 0:d6269d17c8cf 483 case 19:
jah128 0:d6269d17c8cf 484 strcpy(command,"SET CENTER LED ");
jah128 0:d6269d17c8cf 485 switch(message[1]) {
jah128 0:d6269d17c8cf 486 case 1:
jah128 0:d6269d17c8cf 487 strcpy(colour_string,"RED");
jah128 0:d6269d17c8cf 488 break;
jah128 0:d6269d17c8cf 489 case 2:
jah128 0:d6269d17c8cf 490 strcpy(colour_string,"GREEN");
jah128 0:d6269d17c8cf 491 break;
jah128 0:d6269d17c8cf 492 case 3:
jah128 0:d6269d17c8cf 493 strcpy(colour_string,"BOTH");
jah128 0:d6269d17c8cf 494 break;
jah128 0:d6269d17c8cf 495 case 0:
jah128 0:d6269d17c8cf 496 strcpy(colour_string,"OFF");
jah128 0:d6269d17c8cf 497 break;
jah128 0:d6269d17c8cf 498 }
jah128 0:d6269d17c8cf 499 dec = IF_decode_unsigned_float(message[2]);
jah128 0:d6269d17c8cf 500 sprintf(subcommand,"%s @ %1.5f brightness",colour_string,dec);
jah128 0:d6269d17c8cf 501 if(allow_commands) {
jah128 0:d6269d17c8cf 502 command_status = 1;
jah128 9:dde9e21030eb 503 led.set_center_led(message[1],dec);
jah128 0:d6269d17c8cf 504 } else command_status = 2;
jah128 0:d6269d17c8cf 505 break;
jah128 0:d6269d17c8cf 506
jah128 0:d6269d17c8cf 507 // DISPLAY COMMANDS
jah128 0:d6269d17c8cf 508
jah128 0:d6269d17c8cf 509 case 20:
jah128 0:d6269d17c8cf 510 strcpy(command,"SET DISPLAY ");
jah128 0:d6269d17c8cf 511 switch(message[1]) {
jah128 0:d6269d17c8cf 512 case 0:
jah128 0:d6269d17c8cf 513 strcpy(subcommand,"CLEAR");
jah128 0:d6269d17c8cf 514 if(allow_commands) {
jah128 0:d6269d17c8cf 515 command_status = 1;
jah128 0:d6269d17c8cf 516 display.clear_display();
jah128 0:d6269d17c8cf 517 } else command_status = 2;
jah128 0:d6269d17c8cf 518 break;
jah128 0:d6269d17c8cf 519 case 1:
jah128 0:d6269d17c8cf 520 strcpy(subcommand,"MESSAGE 1");
jah128 0:d6269d17c8cf 521 if(allow_commands) {
jah128 0:d6269d17c8cf 522 command_status = 1;
jah128 0:d6269d17c8cf 523 display.clear_display();
jah128 0:d6269d17c8cf 524 display.home();
jah128 0:d6269d17c8cf 525 display.write_string("PC CONNECTION");
jah128 0:d6269d17c8cf 526 display.set_position(1,0);
jah128 0:d6269d17c8cf 527 display.write_string("STARTED");
jah128 0:d6269d17c8cf 528 } else command_status = 2;
jah128 0:d6269d17c8cf 529 break;
jah128 0:d6269d17c8cf 530 case 2:
jah128 0:d6269d17c8cf 531 strcpy(subcommand,"MESSAGE 2");
jah128 0:d6269d17c8cf 532 if(allow_commands) {
jah128 0:d6269d17c8cf 533 command_status = 1;
jah128 0:d6269d17c8cf 534 display.clear_display();
jah128 0:d6269d17c8cf 535 display.home();
jah128 0:d6269d17c8cf 536 display.write_string("PC CONNECTION");
jah128 0:d6269d17c8cf 537 display.set_position(1,0);
jah128 0:d6269d17c8cf 538 display.write_string("TERMINATED");
jah128 0:d6269d17c8cf 539 } else command_status = 2;
jah128 0:d6269d17c8cf 540 break;
jah128 0:d6269d17c8cf 541 case 3:
jah128 0:d6269d17c8cf 542 strcpy(subcommand,"MESSAGE 3");
jah128 0:d6269d17c8cf 543 if(allow_commands) {
jah128 0:d6269d17c8cf 544 command_status = 1;
jah128 0:d6269d17c8cf 545 display.clear_display();
jah128 0:d6269d17c8cf 546 display.home();
jah128 0:d6269d17c8cf 547 display.write_string("ANDROID DEVICE");
jah128 0:d6269d17c8cf 548 display.set_position(1,0);
jah128 0:d6269d17c8cf 549 display.write_string("CONNECTED");
jah128 0:d6269d17c8cf 550 } else command_status = 2;
jah128 0:d6269d17c8cf 551 break;
jah128 0:d6269d17c8cf 552 case 4:
jah128 0:d6269d17c8cf 553 strcpy(subcommand,"MESSAGE 4");
jah128 0:d6269d17c8cf 554 if(allow_commands) {
jah128 0:d6269d17c8cf 555 command_status = 1;
jah128 0:d6269d17c8cf 556 display.clear_display();
jah128 0:d6269d17c8cf 557 display.home();
jah128 0:d6269d17c8cf 558 display.write_string("ANDROID DEVICE");
jah128 0:d6269d17c8cf 559 display.set_position(1,0);
jah128 0:d6269d17c8cf 560 display.write_string("DISCONNECTED");
jah128 0:d6269d17c8cf 561 } else command_status = 2;
jah128 0:d6269d17c8cf 562 break;
jah128 3:7c0d1f581757 563 case 5:
jah128 3:7c0d1f581757 564 strcpy(subcommand,"MESSAGE 5");
jah128 3:7c0d1f581757 565 if(allow_commands) {
jah128 3:7c0d1f581757 566 command_status = 1;
jah128 3:7c0d1f581757 567 display.clear_display();
jah128 3:7c0d1f581757 568 display.home();
jah128 3:7c0d1f581757 569 display.write_string("PSI CONSOLE");
jah128 3:7c0d1f581757 570 display.set_position(1,0);
jah128 3:7c0d1f581757 571 display.write_string("CONNECTED");
jah128 3:7c0d1f581757 572 } else command_status = 2;
jah128 3:7c0d1f581757 573 break;
jah128 10:e58323951c08 574 case 6:
jah128 3:7c0d1f581757 575 strcpy(subcommand,"MESSAGE 6");
jah128 3:7c0d1f581757 576 if(allow_commands) {
jah128 3:7c0d1f581757 577 command_status = 1;
jah128 3:7c0d1f581757 578 display.clear_display();
jah128 3:7c0d1f581757 579 display.home();
jah128 3:7c0d1f581757 580 display.write_string("PSI CONSOLE");
jah128 3:7c0d1f581757 581 display.set_position(1,0);
jah128 3:7c0d1f581757 582 display.write_string("DISCONNECTED");
jah128 3:7c0d1f581757 583 } else command_status = 2;
jah128 10:e58323951c08 584 break;
jah128 0:d6269d17c8cf 585 }
jah128 0:d6269d17c8cf 586 break;
jah128 0:d6269d17c8cf 587 case 21:
jah128 0:d6269d17c8cf 588 strcpy(command,"SET CURSOR ");
jah128 0:d6269d17c8cf 589 if(message[1] < 2 && message[2] < 16) {
jah128 0:d6269d17c8cf 590 sprintf(subcommand,"[%d,%d]",message[1],message[2]);
jah128 0:d6269d17c8cf 591 if(allow_commands) {
jah128 0:d6269d17c8cf 592 display.set_position(message[1],message[2]);
jah128 0:d6269d17c8cf 593 } else command_status = 2;
jah128 0:d6269d17c8cf 594 } else {
jah128 0:d6269d17c8cf 595 sprintf(subcommand,"[INVALID]");
jah128 0:d6269d17c8cf 596 command_status = 3;
jah128 0:d6269d17c8cf 597 }
jah128 0:d6269d17c8cf 598 break;
jah128 10:e58323951c08 599 case 22: {
jah128 0:d6269d17c8cf 600 strcpy(command,"PRINT CHARACTERS ");
jah128 0:d6269d17c8cf 601 char print_message[2];
jah128 0:d6269d17c8cf 602 print_message[0]=message[1];
jah128 0:d6269d17c8cf 603 print_message[1]=message[2];
jah128 0:d6269d17c8cf 604 sprintf(subcommand,"[%c,%c]",message[1],message[2]);
jah128 0:d6269d17c8cf 605 if(allow_commands) {
jah128 0:d6269d17c8cf 606 display.write_string(print_message,2);
jah128 0:d6269d17c8cf 607 } else command_status = 2;
jah128 0:d6269d17c8cf 608 break;
jah128 10:e58323951c08 609 }
jah128 0:d6269d17c8cf 610 case 23:
jah128 0:d6269d17c8cf 611 strcpy(command,"SET DISPLAY B.NESS");
jah128 0:d6269d17c8cf 612 dec = IF_decode_unsigned_float(message[1],message[2]);
jah128 0:d6269d17c8cf 613 sprintf(subcommand,"%1.5f",dec);
jah128 0:d6269d17c8cf 614 if(allow_commands) {
jah128 0:d6269d17c8cf 615 command_status = 1;
jah128 0:d6269d17c8cf 616 display.set_backlight_brightness(dec);
jah128 0:d6269d17c8cf 617 } else command_status = 2;
jah128 0:d6269d17c8cf 618 break;
jah128 0:d6269d17c8cf 619
jah128 0:d6269d17c8cf 620 case 30:
jah128 0:d6269d17c8cf 621 strcpy(command,"SET DEBUG MODE");
jah128 0:d6269d17c8cf 622 switch(message[1]) {
jah128 0:d6269d17c8cf 623 case 1:
jah128 0:d6269d17c8cf 624 strcpy(subcommand,"ON");
jah128 0:d6269d17c8cf 625 break;
jah128 0:d6269d17c8cf 626 case 0:
jah128 0:d6269d17c8cf 627 strcpy(subcommand,"OFF");
jah128 0:d6269d17c8cf 628 break;
jah128 0:d6269d17c8cf 629 }
jah128 0:d6269d17c8cf 630 if(message[2] & 1) strcat (subcommand,"-PC");
jah128 0:d6269d17c8cf 631 if(message[2] & 2) strcat (subcommand,"-BT");
jah128 0:d6269d17c8cf 632 if(message[2] & 4) strcat (subcommand,"-DISP");
jah128 0:d6269d17c8cf 633 if(allow_commands) {
jah128 0:d6269d17c8cf 634 command_status = 1;
jah128 0:d6269d17c8cf 635 debug_mode = message[1];
jah128 0:d6269d17c8cf 636 debug_output = message[2];
jah128 0:d6269d17c8cf 637 } else command_status = 2;
jah128 0:d6269d17c8cf 638 break;
jah128 0:d6269d17c8cf 639 case 31:
jah128 0:d6269d17c8cf 640 strcpy(command,"SET DEMO MODE");
jah128 0:d6269d17c8cf 641 switch(message[1] % 2) {
jah128 0:d6269d17c8cf 642 case 1:
jah128 0:d6269d17c8cf 643 strcpy(subcommand,"ON");
jah128 0:d6269d17c8cf 644 break;
jah128 0:d6269d17c8cf 645 case 0:
jah128 0:d6269d17c8cf 646 strcpy(subcommand,"OFF");
jah128 0:d6269d17c8cf 647 break;
jah128 0:d6269d17c8cf 648 }
jah128 0:d6269d17c8cf 649 if(allow_commands) {
jah128 0:d6269d17c8cf 650 command_status = 1;
jah128 0:d6269d17c8cf 651 demo_on = message[1] % 2;
jah128 0:d6269d17c8cf 652 if(demo_on == 1) {
jah128 0:d6269d17c8cf 653 user_code_restore_mode = user_code_running;
jah128 0:d6269d17c8cf 654 user_code_running = 0;
jah128 0:d6269d17c8cf 655 } else {
jah128 0:d6269d17c8cf 656 user_code_running = user_code_restore_mode;
jah128 0:d6269d17c8cf 657 }
jah128 0:d6269d17c8cf 658 } else command_status = 2;
jah128 0:d6269d17c8cf 659 break;
jah128 0:d6269d17c8cf 660 case 32:
jah128 0:d6269d17c8cf 661 strcpy(command,"SET USER CODE");
jah128 0:d6269d17c8cf 662 switch(message[1] % 2) {
jah128 0:d6269d17c8cf 663 case 1:
jah128 0:d6269d17c8cf 664 strcpy(subcommand,"ON");
jah128 0:d6269d17c8cf 665 break;
jah128 0:d6269d17c8cf 666 case 0:
jah128 0:d6269d17c8cf 667 strcpy(subcommand,"OFF");
jah128 0:d6269d17c8cf 668 break;
jah128 0:d6269d17c8cf 669 }
jah128 0:d6269d17c8cf 670 if(allow_commands) {
jah128 0:d6269d17c8cf 671 command_status = 1;
jah128 0:d6269d17c8cf 672 user_code_running = message[1] % 2;
jah128 0:d6269d17c8cf 673 } else command_status = 2;
jah128 0:d6269d17c8cf 674 break;
jah128 0:d6269d17c8cf 675 case 33:
jah128 0:d6269d17c8cf 676 strcpy(command,"PAUSE USER CODE");
jah128 0:d6269d17c8cf 677 dec = IF_decode_unsigned_float(message[1],message[2]) * 10;
jah128 0:d6269d17c8cf 678 sprintf(subcommand,"FOR %2.3fS",dec);
jah128 0:d6269d17c8cf 679 if(allow_commands) {
jah128 0:d6269d17c8cf 680 command_status = 1;
jah128 12:878c6e9d9e60 681 psi.pause_user_code(dec);
jah128 0:d6269d17c8cf 682 } else command_status = 2;
jah128 0:d6269d17c8cf 683 break;
jah128 0:d6269d17c8cf 684
jah128 0:d6269d17c8cf 685 case 34:
jah128 0:d6269d17c8cf 686 strcpy(command,"RESET ENCODERS");
jah128 0:d6269d17c8cf 687 if(allow_commands) {
jah128 0:d6269d17c8cf 688 command_status = 1;
jah128 12:878c6e9d9e60 689 psi.reset_encoders();
jah128 0:d6269d17c8cf 690 } else command_status = 2;
jah128 0:d6269d17c8cf 691 break;
jah128 0:d6269d17c8cf 692
jah128 0:d6269d17c8cf 693 case 35:
jah128 0:d6269d17c8cf 694 strcpy(command,"SET ALLOW COMMANDS");
jah128 0:d6269d17c8cf 695 switch(message[1] % 2) {
jah128 0:d6269d17c8cf 696 case 1:
jah128 0:d6269d17c8cf 697 strcpy(subcommand,"ON");
jah128 0:d6269d17c8cf 698 break;
jah128 0:d6269d17c8cf 699 case 0:
jah128 0:d6269d17c8cf 700 strcpy(subcommand,"OFF");
jah128 0:d6269d17c8cf 701 break;
jah128 0:d6269d17c8cf 702 }
jah128 0:d6269d17c8cf 703 allow_commands = message[1] % 2;
jah128 0:d6269d17c8cf 704 command_status = 1;
jah128 0:d6269d17c8cf 705 break;
jah128 0:d6269d17c8cf 706
jah128 0:d6269d17c8cf 707 case 36:
jah128 0:d6269d17c8cf 708 irp_delay = (message[1] << 8) + message[2];
jah128 0:d6269d17c8cf 709 sprintf(command,"SET IR PULSE DELAY %d MS",irp_delay);
jah128 0:d6269d17c8cf 710 if(allow_commands) {
jah128 0:d6269d17c8cf 711 command_status = 1;
jah128 0:d6269d17c8cf 712 ir_pulse_delay = irp_delay;
jah128 0:d6269d17c8cf 713 } else command_status = 2;
jah128 0:d6269d17c8cf 714 break;
jah128 0:d6269d17c8cf 715 case 37:
jah128 0:d6269d17c8cf 716 irp_delay = (message[1] << 8) + message[2];
jah128 0:d6269d17c8cf 717 sprintf(command,"SET BASE IR PULSE DELAY %d MS",irp_delay);
jah128 0:d6269d17c8cf 718 if(allow_commands) {
jah128 0:d6269d17c8cf 719 command_status = 1;
jah128 0:d6269d17c8cf 720 base_ir_pulse_delay = irp_delay;
jah128 0:d6269d17c8cf 721 } else command_status = 2;
jah128 0:d6269d17c8cf 722 break;
jah128 0:d6269d17c8cf 723
jah128 0:d6269d17c8cf 724 // MOTOR REQUESTS
jah128 0:d6269d17c8cf 725 case 40:
jah128 0:d6269d17c8cf 726 strcpy(command,"GET LEFT MOTOR SPEED");
jah128 0:d6269d17c8cf 727 sprintf(ret_message,"%1.5f",motor_left_speed);
jah128 0:d6269d17c8cf 728 send_message = 1;
jah128 0:d6269d17c8cf 729 break;
jah128 0:d6269d17c8cf 730
jah128 0:d6269d17c8cf 731 case 41:
jah128 0:d6269d17c8cf 732 strcpy(command,"GET RIGHT MOTOR SPEED");
jah128 0:d6269d17c8cf 733 sprintf(ret_message,"%1.5f",motor_right_speed);
jah128 0:d6269d17c8cf 734 send_message = 1;
jah128 0:d6269d17c8cf 735 break;
jah128 0:d6269d17c8cf 736 case 42:
jah128 0:d6269d17c8cf 737 strcpy(command,"GET BRAKE STATES");
jah128 0:d6269d17c8cf 738 sprintf(ret_message,"%d,%d",motor_left_brake,motor_right_brake);
jah128 0:d6269d17c8cf 739 send_message = 1;
jah128 0:d6269d17c8cf 740 break;
jah128 0:d6269d17c8cf 741 case 43:
jah128 0:d6269d17c8cf 742 strcpy(command,"GET MOTOR STATES");
jah128 0:d6269d17c8cf 743 //sprintf(ret_message,"%d,%d",motor_left_brake,motor_right_brake);
jah128 0:d6269d17c8cf 744 send_message = 1;
jah128 0:d6269d17c8cf 745 break;
jah128 0:d6269d17c8cf 746 case 44:
jah128 0:d6269d17c8cf 747 strcpy(command,"GET ENCODERS");
jah128 0:d6269d17c8cf 748 sprintf(ret_message,"%d,%d",left_encoder,right_encoder);
jah128 0:d6269d17c8cf 749 send_message = 1;
jah128 0:d6269d17c8cf 750 break;
jah128 0:d6269d17c8cf 751
jah128 0:d6269d17c8cf 752 // LED REQUESTS
jah128 0:d6269d17c8cf 753 case 50:
jah128 0:d6269d17c8cf 754 strcpy(command,"GET LED STATES");
jah128 9:dde9e21030eb 755 sprintf(ret_message,"%04x",led.get_led_states());
jah128 0:d6269d17c8cf 756 send_message = 1;
jah128 0:d6269d17c8cf 757 break;
jah128 0:d6269d17c8cf 758
jah128 0:d6269d17c8cf 759 // GENERAL REQUESTS
jah128 0:d6269d17c8cf 760 case 60:
jah128 0:d6269d17c8cf 761 strcpy(command,"GET SOFTWARE VERSION");
jah128 0:d6269d17c8cf 762 sprintf(ret_message,"%1.2f",SOFTWARE_VERSION_CODE);
jah128 0:d6269d17c8cf 763 send_message = 1;
jah128 0:d6269d17c8cf 764 break;
jah128 0:d6269d17c8cf 765
jah128 0:d6269d17c8cf 766 case 61:
jah128 0:d6269d17c8cf 767 strcpy(command,"GET UPTIME");
jah128 12:878c6e9d9e60 768 sprintf(ret_message,"%6.2f",psi.get_uptime());
jah128 0:d6269d17c8cf 769 send_message = 1;
jah128 0:d6269d17c8cf 770 break;
jah128 0:d6269d17c8cf 771
jah128 0:d6269d17c8cf 772 case 62:
jah128 0:d6269d17c8cf 773 strcpy(command,"GET ID");
jah128 0:d6269d17c8cf 774 sprintf(ret_message,"%d",robot_id);
jah128 0:d6269d17c8cf 775 send_message = 1;
jah128 0:d6269d17c8cf 776 break;
jah128 0:d6269d17c8cf 777
jah128 0:d6269d17c8cf 778 case 63:
jah128 0:d6269d17c8cf 779 strcpy(command,"GET SWITCH BYTE");
jah128 0:d6269d17c8cf 780 sprintf(ret_message,"%02x",switch_byte);
jah128 0:d6269d17c8cf 781 send_message = 1;
jah128 0:d6269d17c8cf 782 break;
jah128 0:d6269d17c8cf 783 case 64:
jah128 0:d6269d17c8cf 784 strcpy(command,"GET USER CODE");
jah128 0:d6269d17c8cf 785 sprintf(ret_message,"%d",user_code_running);
jah128 0:d6269d17c8cf 786 send_message = 1;
jah128 0:d6269d17c8cf 787 break;
jah128 0:d6269d17c8cf 788 case 65:
jah128 0:d6269d17c8cf 789 strcpy(command,"GET RESPONSE STRING");
jah128 0:d6269d17c8cf 790 sprintf(ret_message,"PSI");
jah128 0:d6269d17c8cf 791 send_message = 1;
jah128 0:d6269d17c8cf 792 break;
jah128 0:d6269d17c8cf 793 case 66:
jah128 0:d6269d17c8cf 794 strcpy(command,"GET PROGRAM NAME");
jah128 0:d6269d17c8cf 795 sprintf(ret_message,"%s",program_name);
jah128 0:d6269d17c8cf 796 send_message = 1;
jah128 0:d6269d17c8cf 797 break;
jah128 0:d6269d17c8cf 798 case 67:
jah128 0:d6269d17c8cf 799 strcpy(command,"GET AUTHOR NAME");
jah128 0:d6269d17c8cf 800 sprintf(ret_message,"%s",author_name);
jah128 0:d6269d17c8cf 801 send_message = 1;
jah128 0:d6269d17c8cf 802 break;
jah128 0:d6269d17c8cf 803 case 68:
jah128 0:d6269d17c8cf 804 strcpy(command,"GET DEBUG MODE");
jah128 0:d6269d17c8cf 805 sprintf(ret_message,"%1d%1d",debug_mode,debug_output);
jah128 0:d6269d17c8cf 806 send_message = 1;
jah128 0:d6269d17c8cf 807 break;
jah128 0:d6269d17c8cf 808 case 69:
jah128 0:d6269d17c8cf 809 strcpy(command,"GET SYSTEM WARNINGS");
jah128 0:d6269d17c8cf 810 sprintf(ret_message,"%d",system_warnings);
jah128 0:d6269d17c8cf 811 send_message = 1;
jah128 0:d6269d17c8cf 812 break;
jah128 0:d6269d17c8cf 813
jah128 0:d6269d17c8cf 814
jah128 0:d6269d17c8cf 815 // Sensors
jah128 0:d6269d17c8cf 816 case 80:
jah128 0:d6269d17c8cf 817 strcpy(command,"STORE BG. IR VALUES");
jah128 0:d6269d17c8cf 818 if(allow_commands) {
jah128 0:d6269d17c8cf 819 command_status = 1;
jah128 10:e58323951c08 820 sensors.store_background_raw_ir_values();
jah128 0:d6269d17c8cf 821 } else command_status = 2;
jah128 0:d6269d17c8cf 822 break;
jah128 0:d6269d17c8cf 823 case 81:
jah128 0:d6269d17c8cf 824 strcpy(command,"STORE IL. IR VALUES");
jah128 0:d6269d17c8cf 825 if(allow_commands) {
jah128 0:d6269d17c8cf 826 command_status = 1;
jah128 10:e58323951c08 827 sensors.store_illuminated_raw_ir_values();
jah128 0:d6269d17c8cf 828 } else command_status = 2;
jah128 0:d6269d17c8cf 829 break;
jah128 0:d6269d17c8cf 830 case 82:
jah128 0:d6269d17c8cf 831 strcpy(command,"STORE IR VALUES");
jah128 0:d6269d17c8cf 832 if(allow_commands) {
jah128 0:d6269d17c8cf 833 command_status = 1;
jah128 10:e58323951c08 834 sensors.store_ir_values();
jah128 0:d6269d17c8cf 835 } else command_status = 2;
jah128 0:d6269d17c8cf 836 break;
jah128 0:d6269d17c8cf 837 case 83:
jah128 0:d6269d17c8cf 838 strcpy(command,"STORE BG BASE IR VALUES");
jah128 0:d6269d17c8cf 839 if(allow_commands) {
jah128 0:d6269d17c8cf 840 command_status = 1;
jah128 10:e58323951c08 841 sensors.store_background_base_ir_values();
jah128 0:d6269d17c8cf 842 } else command_status = 2;
jah128 0:d6269d17c8cf 843 break;
jah128 0:d6269d17c8cf 844 case 84:
jah128 0:d6269d17c8cf 845 strcpy(command,"STORE IL. BASE IR VALUES");
jah128 0:d6269d17c8cf 846 if(allow_commands) {
jah128 0:d6269d17c8cf 847 command_status = 1;
jah128 10:e58323951c08 848 sensors.store_illuminated_base_ir_values();
jah128 0:d6269d17c8cf 849 } else command_status = 2;
jah128 0:d6269d17c8cf 850 break;
jah128 0:d6269d17c8cf 851 case 85:
jah128 0:d6269d17c8cf 852 strcpy(command,"STORE BASE IR VALUES");
jah128 0:d6269d17c8cf 853 if(allow_commands) {
jah128 0:d6269d17c8cf 854 command_status = 1;
jah128 10:e58323951c08 855 sensors.store_base_ir_values();
jah128 0:d6269d17c8cf 856 } else command_status = 2;
jah128 0:d6269d17c8cf 857 break;
jah128 0:d6269d17c8cf 858 case 86:
jah128 0:d6269d17c8cf 859 strcpy(command,"STORE ALL IR VALUES");
jah128 0:d6269d17c8cf 860 if(allow_commands) {
jah128 0:d6269d17c8cf 861 command_status = 1;
jah128 10:e58323951c08 862 sensors.store_ir_values();
jah128 10:e58323951c08 863 sensors.store_base_ir_values();
jah128 0:d6269d17c8cf 864 } else command_status = 2;
jah128 0:d6269d17c8cf 865 break;
jah128 0:d6269d17c8cf 866 case 90:
jah128 0:d6269d17c8cf 867 sprintf(command,"%s %d","GET BG IR VALUE",message[1]);
jah128 10:e58323951c08 868 sprintf(ret_message,"%d",sensors.get_background_raw_ir_value(message[1]));
jah128 0:d6269d17c8cf 869 send_message = 1;
jah128 0:d6269d17c8cf 870 break;
jah128 0:d6269d17c8cf 871 case 91:
jah128 0:d6269d17c8cf 872 sprintf(command,"%s %d","GET IL IR VALUE",message[1]);
jah128 10:e58323951c08 873 sprintf(ret_message,"%d",sensors.get_illuminated_raw_ir_value(message[1]));
jah128 0:d6269d17c8cf 874 send_message = 1;
jah128 0:d6269d17c8cf 875 break;
jah128 0:d6269d17c8cf 876 case 92:
jah128 0:d6269d17c8cf 877 strcpy(command,"GET BG IR VALUES");
jah128 10:e58323951c08 878 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));
jah128 0:d6269d17c8cf 879 send_message = 1;
jah128 0:d6269d17c8cf 880 break;
jah128 0:d6269d17c8cf 881 case 93:
jah128 0:d6269d17c8cf 882 strcpy(command,"GET ILLUMINATED IR VALUES");
jah128 10:e58323951c08 883 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));
jah128 0:d6269d17c8cf 884 send_message = 1;
jah128 0:d6269d17c8cf 885 break;
jah128 0:d6269d17c8cf 886 case 94:
jah128 0:d6269d17c8cf 887 sprintf(command,"%s %d","GET BG BASE IR VALUE",message[1]);
jah128 10:e58323951c08 888 sprintf(ret_message,"%d",sensors.get_background_base_ir_value(message[1]));
jah128 0:d6269d17c8cf 889 send_message = 1;
jah128 0:d6269d17c8cf 890 break;
jah128 0:d6269d17c8cf 891 case 95:
jah128 0:d6269d17c8cf 892 sprintf(command,"%s %d","GET IL BASE IR VALUE",message[1]);
jah128 10:e58323951c08 893 sprintf(ret_message,"%d",sensors.get_illuminated_base_ir_value(message[1]));
jah128 0:d6269d17c8cf 894 send_message = 1;
jah128 0:d6269d17c8cf 895 break;
jah128 0:d6269d17c8cf 896 case 96:
jah128 0:d6269d17c8cf 897 strcpy(command,"GET BG BASE IR VALUES");
jah128 10:e58323951c08 898 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));
jah128 0:d6269d17c8cf 899 send_message = 1;
jah128 0:d6269d17c8cf 900 break;
jah128 0:d6269d17c8cf 901 case 97:
jah128 0:d6269d17c8cf 902 strcpy(command,"GET IL BASE IR VALUES");
jah128 10:e58323951c08 903 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));
jah128 0:d6269d17c8cf 904 send_message = 1;
jah128 0:d6269d17c8cf 905 break;
jah128 2:c6986ee3c7c5 906 case 98:
jah128 2:c6986ee3c7c5 907 strcpy(command,"CALCULATE BASE IR VALUES");
jah128 10:e58323951c08 908 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));
jah128 2:c6986ee3c7c5 909 send_message = 1;
jah128 2:c6986ee3c7c5 910 break;
jah128 2:c6986ee3c7c5 911 case 99:
jah128 2:c6986ee3c7c5 912 strcpy(command,"CALCULATE SIDE IR VALUES");
jah128 10:e58323951c08 913 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));
jah128 2:c6986ee3c7c5 914 send_message = 1;
jah128 2:c6986ee3c7c5 915 break;
jah128 0:d6269d17c8cf 916 case 100:
jah128 0:d6269d17c8cf 917 strcpy(command,"START FILE TRANSFER MODE");
jah128 0:d6269d17c8cf 918 if(allow_commands) {
jah128 0:d6269d17c8cf 919 command_status = 1;
jah128 1:060690a934a9 920 IF_start_file_transfer_mode();
jah128 0:d6269d17c8cf 921 sprintf(ret_message,"OK");
jah128 0:d6269d17c8cf 922 send_message = 1;
jah128 0:d6269d17c8cf 923 } else command_status = 2;
jah128 0:d6269d17c8cf 924 break;
jah128 3:7c0d1f581757 925 case 110:
jah128 3:7c0d1f581757 926 strcpy(command,"GET FIRMWARE VERSION");
jah128 3:7c0d1f581757 927 sprintf(ret_message,"%1.2f",firmware_version);
jah128 3:7c0d1f581757 928 send_message = 1;
jah128 3:7c0d1f581757 929 break;
jah128 3:7c0d1f581757 930 case 111:
jah128 3:7c0d1f581757 931 strcpy(command,"GET SERIAL NUMBER");
jah128 3:7c0d1f581757 932 sprintf(ret_message,"%2.3f",serial_number);
jah128 3:7c0d1f581757 933 send_message = 1;
jah128 3:7c0d1f581757 934 break;
jah128 3:7c0d1f581757 935 case 112:
jah128 3:7c0d1f581757 936 strcpy(command,"GET HAS SIDE IR");
jah128 3:7c0d1f581757 937 sprintf(ret_message,"%d",has_side_ir);
jah128 3:7c0d1f581757 938 send_message = 1;
jah128 3:7c0d1f581757 939 break;
jah128 3:7c0d1f581757 940 case 113:
jah128 3:7c0d1f581757 941 strcpy(command,"GET HAS BASE IR");
jah128 3:7c0d1f581757 942 sprintf(ret_message,"%d",has_base_ir);
jah128 3:7c0d1f581757 943 send_message = 1;
jah128 3:7c0d1f581757 944 break;
jah128 3:7c0d1f581757 945 case 114:
jah128 3:7c0d1f581757 946 strcpy(command,"GET HAS ENCODERS");
jah128 3:7c0d1f581757 947 sprintf(ret_message,"%d",has_wheel_encoders);
jah128 3:7c0d1f581757 948 send_message = 1;
jah128 3:7c0d1f581757 949 break;
jah128 3:7c0d1f581757 950 case 115:
jah128 3:7c0d1f581757 951 strcpy(command,"GET HAS AUDIO");
jah128 3:7c0d1f581757 952 sprintf(ret_message,"%d",has_audio_pic);
jah128 3:7c0d1f581757 953 send_message = 1;
jah128 3:7c0d1f581757 954 break;
jah128 3:7c0d1f581757 955 case 116:
jah128 3:7c0d1f581757 956 strcpy(command,"GET HAS RECHARGING");
jah128 3:7c0d1f581757 957 sprintf(ret_message,"%d",has_recharging_circuit);
jah128 3:7c0d1f581757 958 send_message = 1;
jah128 3:7c0d1f581757 959 break;
jah128 3:7c0d1f581757 960 case 117:
jah128 3:7c0d1f581757 961 strcpy(command,"GET HAS COMPASS");
jah128 3:7c0d1f581757 962 sprintf(ret_message,"%d",has_compass);
jah128 3:7c0d1f581757 963 send_message = 1;
jah128 3:7c0d1f581757 964 break;
jah128 3:7c0d1f581757 965 case 118:
jah128 3:7c0d1f581757 966 strcpy(command,"GET HAS ULTRASONIC");
jah128 3:7c0d1f581757 967 sprintf(ret_message,"%d",has_ultrasonic_sensor);
jah128 3:7c0d1f581757 968 send_message = 1;
jah128 3:7c0d1f581757 969 break;
jah128 3:7c0d1f581757 970 case 119:
jah128 3:7c0d1f581757 971 strcpy(command,"GET HAS TEMPERATURE");
jah128 3:7c0d1f581757 972 sprintf(ret_message,"%d",has_temperature_sensor);
jah128 3:7c0d1f581757 973 send_message = 1;
jah128 3:7c0d1f581757 974 break;
jah128 3:7c0d1f581757 975 case 120:
jah128 3:7c0d1f581757 976 strcpy(command,"GET HAS BASE COLOUR");
jah128 3:7c0d1f581757 977 sprintf(ret_message,"%d",has_base_colour_sensor);
jah128 3:7c0d1f581757 978 send_message = 1;
jah128 3:7c0d1f581757 979 break;
jah128 3:7c0d1f581757 980 case 121:
jah128 3:7c0d1f581757 981 strcpy(command,"GET HAS TOP COLOUR");
jah128 3:7c0d1f581757 982 sprintf(ret_message,"%d",has_top_colour_sensor);
jah128 3:7c0d1f581757 983 send_message = 1;
jah128 3:7c0d1f581757 984 break;
jah128 3:7c0d1f581757 985 case 122:
jah128 3:7c0d1f581757 986 strcpy(command,"GET HAS RADIO");
jah128 3:7c0d1f581757 987 sprintf(ret_message,"%d",has_433_radio);
jah128 3:7c0d1f581757 988 send_message = 1;
jah128 3:7c0d1f581757 989 break;
jah128 10:e58323951c08 990 case 123: {
jah128 3:7c0d1f581757 991 strcpy(command,"GET FIRMWARE H-DESC");
jah128 3:7c0d1f581757 992 char byte0 = 0;
jah128 3:7c0d1f581757 993 char byte1 = 1;
jah128 3:7c0d1f581757 994 if(has_side_ir)byte0+=128;
jah128 3:7c0d1f581757 995 if(has_base_ir)byte0+=64;
jah128 3:7c0d1f581757 996 if(has_wheel_encoders)byte0+=32;
jah128 3:7c0d1f581757 997 if(has_audio_pic)byte0+=16;
jah128 3:7c0d1f581757 998 if(has_recharging_circuit)byte0+=8;
jah128 3:7c0d1f581757 999 if(has_compass)byte0+=4;
jah128 3:7c0d1f581757 1000 if(has_ultrasonic_sensor)byte0+=2;
jah128 3:7c0d1f581757 1001 if(has_temperature_sensor)byte0+=1;
jah128 3:7c0d1f581757 1002 if(has_base_colour_sensor)byte1+=128;
jah128 3:7c0d1f581757 1003 if(has_top_colour_sensor)byte1+=64;
jah128 3:7c0d1f581757 1004 if(has_433_radio)byte1+=32;
jah128 3:7c0d1f581757 1005 sprintf(ret_message,"%c%c",byte0,byte1);
jah128 3:7c0d1f581757 1006 send_message = 1;
jah128 3:7c0d1f581757 1007 break;
jah128 10:e58323951c08 1008 }
jah128 3:7c0d1f581757 1009 case 124:
jah128 3:7c0d1f581757 1010 strcpy(command,"GET PCB VERSION");
jah128 3:7c0d1f581757 1011 sprintf(ret_message,"%1.2f",pcb_version);
jah128 3:7c0d1f581757 1012 send_message = 1;
jah128 3:7c0d1f581757 1013 break;
jah128 0:d6269d17c8cf 1014 }
jah128 0:d6269d17c8cf 1015
jah128 0:d6269d17c8cf 1016
jah128 0:d6269d17c8cf 1017 if(send_message) {
jah128 0:d6269d17c8cf 1018 char message_length = strlen(ret_message);
jah128 0:d6269d17c8cf 1019 switch(interface) {
jah128 0:d6269d17c8cf 1020 case 0:
jah128 0:d6269d17c8cf 1021 pc.printf("%c%c%s",RESPONSE_MESSAGE_BYTE,message_length,ret_message);
jah128 0:d6269d17c8cf 1022 break;
jah128 0:d6269d17c8cf 1023 case 1:
jah128 0:d6269d17c8cf 1024 bt.printf("%c%c%s",RESPONSE_MESSAGE_BYTE,message_length,ret_message);
jah128 0:d6269d17c8cf 1025 break;
jah128 0:d6269d17c8cf 1026 }
jah128 12:878c6e9d9e60 1027 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);
jah128 0:d6269d17c8cf 1028 } else {
jah128 0:d6269d17c8cf 1029 switch(interface) {
jah128 0:d6269d17c8cf 1030 case 0:
jah128 0:d6269d17c8cf 1031 pc.printf("%c%c",ACKNOWLEDGE_MESSAGE_BYTE,command_status);
jah128 0:d6269d17c8cf 1032 break;
jah128 0:d6269d17c8cf 1033 case 1:
jah128 0:d6269d17c8cf 1034 bt.printf("%c%c",ACKNOWLEDGE_MESSAGE_BYTE,command_status);
jah128 0:d6269d17c8cf 1035 break;
jah128 0:d6269d17c8cf 1036 }
jah128 0:d6269d17c8cf 1037 switch(command_status) {
jah128 0:d6269d17c8cf 1038 case 0:
jah128 12:878c6e9d9e60 1039 psi.debug("Unrecognised %s command message [%02x%02x%02x]\n",iface,message[0],message[1],message[2]);
jah128 0:d6269d17c8cf 1040 break;
jah128 0:d6269d17c8cf 1041 case 1:
jah128 12:878c6e9d9e60 1042 psi.debug("Actioned %s command message:%s %s [%02x%02x%02x]\n",iface, command, subcommand,message[0],message[1],message[2]);
jah128 0:d6269d17c8cf 1043 break;
jah128 0:d6269d17c8cf 1044 case 2:
jah128 12:878c6e9d9e60 1045 psi.debug("Blocked %s command message:%s %s [%02x%02x%02x]\n",iface, command, subcommand,message[0],message[1],message[2]);
jah128 0:d6269d17c8cf 1046 break;
jah128 0:d6269d17c8cf 1047 case 3:
jah128 12:878c6e9d9e60 1048 psi.debug("Invalid %s command message:%s %s [%02x%02x%02x]\n",iface, command, subcommand,message[0],message[1],message[2]);
jah128 0:d6269d17c8cf 1049 break;
jah128 0:d6269d17c8cf 1050 }
jah128 0:d6269d17c8cf 1051 }
jah128 0:d6269d17c8cf 1052 }
jah128 0:d6269d17c8cf 1053
jah128 11:312663037b8c 1054 char * SerialControl::IF_nibble_to_binary_char(char in)
jah128 0:d6269d17c8cf 1055 {
jah128 0:d6269d17c8cf 1056 char * ret = (char*)malloc(sizeof(char)*5);
jah128 0:d6269d17c8cf 1057 for(int i=0; i<4; i++) {
jah128 0:d6269d17c8cf 1058 if(in & (128 >> i)) ret[i]='1';
jah128 0:d6269d17c8cf 1059 else ret[i]='0';
jah128 0:d6269d17c8cf 1060 }
jah128 0:d6269d17c8cf 1061 ret[4]=0;
jah128 0:d6269d17c8cf 1062 return ret;
jah128 0:d6269d17c8cf 1063 }
jah128 0:d6269d17c8cf 1064
jah128 11:312663037b8c 1065 char * SerialControl::IF_char_to_binary_char(char in)
jah128 0:d6269d17c8cf 1066 {
jah128 0:d6269d17c8cf 1067 char * ret = (char*)malloc(sizeof(char)*9);
jah128 0:d6269d17c8cf 1068 for(int i=0; i<8; i++) {
jah128 0:d6269d17c8cf 1069 if(in & (128 >> i)) ret[i]='1';
jah128 0:d6269d17c8cf 1070 else ret[i]='0';
jah128 0:d6269d17c8cf 1071 }
jah128 0:d6269d17c8cf 1072 ret[8]=0;
jah128 0:d6269d17c8cf 1073 return ret;
jah128 0:d6269d17c8cf 1074 }
jah128 0:d6269d17c8cf 1075
jah128 11:312663037b8c 1076 float SerialControl::IF_decode_unsigned_float(char byte0, char byte1)
jah128 0:d6269d17c8cf 1077 {
jah128 0:d6269d17c8cf 1078 unsigned short sval = (byte0) << 8;
jah128 0:d6269d17c8cf 1079 sval += byte1;
jah128 0:d6269d17c8cf 1080 float scaled = sval / 65535.0f;
jah128 0:d6269d17c8cf 1081 return scaled;
jah128 0:d6269d17c8cf 1082 }
jah128 0:d6269d17c8cf 1083
jah128 11:312663037b8c 1084 float SerialControl::IF_decode_float(char byte0, char byte1)
jah128 0:d6269d17c8cf 1085 {
jah128 0:d6269d17c8cf 1086 // MSB is byte 0 is sign, rest is linear spread between 0 and 1
jah128 0:d6269d17c8cf 1087 char sign = byte0 / 128;
jah128 0:d6269d17c8cf 1088 short sval = (byte0 % 128) << 8;
jah128 0:d6269d17c8cf 1089 sval += byte1;
jah128 0:d6269d17c8cf 1090 float scaled = sval / 32767.0f;
jah128 0:d6269d17c8cf 1091 if(sign == 0) scaled = 0-scaled;
jah128 0:d6269d17c8cf 1092 return scaled;
jah128 0:d6269d17c8cf 1093 }
jah128 0:d6269d17c8cf 1094
jah128 11:312663037b8c 1095 float SerialControl::IF_decode_unsigned_float(char byte0)
jah128 0:d6269d17c8cf 1096 {
jah128 0:d6269d17c8cf 1097 unsigned short sval = (byte0);
jah128 0:d6269d17c8cf 1098 float scaled = sval / 255.0f;
jah128 0:d6269d17c8cf 1099 return scaled;
jah128 0:d6269d17c8cf 1100 }
jah128 0:d6269d17c8cf 1101
jah128 11:312663037b8c 1102 float SerialControl::IF_decode_float(char byte0)
jah128 0:d6269d17c8cf 1103 {
jah128 0:d6269d17c8cf 1104 // MSB is byte 0 is sign, rest is linear spread between 0 and 1
jah128 0:d6269d17c8cf 1105 char sign = byte0 / 128;
jah128 0:d6269d17c8cf 1106 short sval = (byte0 % 128);
jah128 0:d6269d17c8cf 1107 float scaled = sval / 127.0f;
jah128 0:d6269d17c8cf 1108 if(sign == 0) scaled = 0-scaled;
jah128 0:d6269d17c8cf 1109 return scaled;
jah128 0:d6269d17c8cf 1110 }
jah128 0:d6269d17c8cf 1111
jah128 0:d6269d17c8cf 1112
jah128 11:312663037b8c 1113 void SerialControl::IF_pc_rx_command_timeout()
jah128 0:d6269d17c8cf 1114 {
jah128 0:d6269d17c8cf 1115 char message_array[6];
jah128 0:d6269d17c8cf 1116 char length = 1 + pc_command_message_byte;
jah128 0:d6269d17c8cf 1117 pc_command_message_started = 0;
jah128 0:d6269d17c8cf 1118 message_array[0] = COMMAND_MESSAGE_BYTE;
jah128 0:d6269d17c8cf 1119 for(int k=0; k<pc_command_message_byte; k++) {
jah128 0:d6269d17c8cf 1120 message_array[k+1] = pc_command_message[k];
jah128 0:d6269d17c8cf 1121 }
jah128 0:d6269d17c8cf 1122 IF_handle_user_serial_message(message_array, length, 0);
jah128 0:d6269d17c8cf 1123 }
jah128 0:d6269d17c8cf 1124
jah128 11:312663037b8c 1125 void SerialControl::IF_bt_rx_command_timeout()
jah128 0:d6269d17c8cf 1126 {
jah128 0:d6269d17c8cf 1127 char message_array[6];
jah128 0:d6269d17c8cf 1128 char length = 1 + bt_command_message_byte;
jah128 0:d6269d17c8cf 1129 bt_command_message_started = 0;
jah128 0:d6269d17c8cf 1130 message_array[0] = COMMAND_MESSAGE_BYTE;
jah128 0:d6269d17c8cf 1131 for(int k=0; k<bt_command_message_byte; k++) {
jah128 0:d6269d17c8cf 1132 message_array[k+1] = bt_command_message[k];
jah128 0:d6269d17c8cf 1133 }
jah128 0:d6269d17c8cf 1134 IF_handle_user_serial_message(message_array, length, 1);
jah128 0:d6269d17c8cf 1135 }
jah128 0:d6269d17c8cf 1136
jah128 11:312663037b8c 1137 void SerialControl::IF_pc_rx_callback()
jah128 0:d6269d17c8cf 1138 {
jah128 0:d6269d17c8cf 1139 int count = 0;
jah128 0:d6269d17c8cf 1140 char message_array[255];
jah128 0:d6269d17c8cf 1141
jah128 0:d6269d17c8cf 1142 while(pc.readable()) {
jah128 0:d6269d17c8cf 1143 char tc = pc.getc();
jah128 0:d6269d17c8cf 1144 message_array[count] = tc;
jah128 0:d6269d17c8cf 1145 count ++;
jah128 0:d6269d17c8cf 1146 if(pc_command_message_started == 1) {
jah128 0:d6269d17c8cf 1147 if(pc_command_message_byte == 3) {
jah128 0:d6269d17c8cf 1148 pc_command_timeout.detach();
jah128 0:d6269d17c8cf 1149 if(tc == COMMAND_MESSAGE_BYTE) {
jah128 0:d6269d17c8cf 1150 // A complete command message succesfully received, call handler
jah128 0:d6269d17c8cf 1151 pc_command_message_started = 0;
jah128 0:d6269d17c8cf 1152 count = 0;
jah128 0:d6269d17c8cf 1153 IF_handle_command_serial_message(pc_command_message , 0);
jah128 0:d6269d17c8cf 1154 } else {
jah128 0:d6269d17c8cf 1155 // Message is not a valid command message as 5th byte is not correct; treat whole message as a user message
jah128 0:d6269d17c8cf 1156 pc_command_message_started = 0;
jah128 0:d6269d17c8cf 1157 message_array[0] = COMMAND_MESSAGE_BYTE;
jah128 0:d6269d17c8cf 1158 message_array[1] = pc_command_message[0];
jah128 0:d6269d17c8cf 1159 message_array[2] = pc_command_message[1];
jah128 0:d6269d17c8cf 1160 message_array[3] = pc_command_message[2];
jah128 0:d6269d17c8cf 1161 message_array[4] = tc;
jah128 0:d6269d17c8cf 1162 count = 5;
jah128 0:d6269d17c8cf 1163 }
jah128 0:d6269d17c8cf 1164 } else {
jah128 0:d6269d17c8cf 1165 pc_command_message[pc_command_message_byte] = tc;
jah128 0:d6269d17c8cf 1166 pc_command_message_byte ++;
jah128 0:d6269d17c8cf 1167 }
jah128 0:d6269d17c8cf 1168 } else {
jah128 0:d6269d17c8cf 1169 if(count == 1) {
jah128 0:d6269d17c8cf 1170 if(tc == COMMAND_MESSAGE_BYTE) {
jah128 11:312663037b8c 1171 pc_command_timeout.attach(this,&SerialControl::IF_pc_rx_command_timeout,command_timeout_period);
jah128 0:d6269d17c8cf 1172 pc_command_message_started = 1;
jah128 0:d6269d17c8cf 1173 pc_command_message_byte = 0;
jah128 0:d6269d17c8cf 1174
jah128 0:d6269d17c8cf 1175 }
jah128 0:d6269d17c8cf 1176 }
jah128 0:d6269d17c8cf 1177 }
jah128 0:d6269d17c8cf 1178 }
jah128 0:d6269d17c8cf 1179 if(!pc_command_message_started && count>0) IF_handle_user_serial_message(message_array, count, 0);
jah128 0:d6269d17c8cf 1180 }
jah128 0:d6269d17c8cf 1181
jah128 0:d6269d17c8cf 1182 Timeout bt_message_timeout;
jah128 10:e58323951c08 1183 //static float bt_message_timeout_period = 0.001; // 1 millisecond
jah128 0:d6269d17c8cf 1184 char bt_buffer[255];
jah128 0:d6269d17c8cf 1185 int bt_buffer_index = 0;
jah128 0:d6269d17c8cf 1186
jah128 11:312663037b8c 1187 void SerialControl::IF_bt_message_timeout()
jah128 0:d6269d17c8cf 1188 {
jah128 0:d6269d17c8cf 1189 char buffer[255];
jah128 0:d6269d17c8cf 1190
jah128 0:d6269d17c8cf 1191 sprintf(buffer, bt_buffer, bt_buffer_index);
jah128 0:d6269d17c8cf 1192 buffer[bt_buffer_index] = 0;
jah128 0:d6269d17c8cf 1193 if(file_transfer_mode == 1) {
jah128 0:d6269d17c8cf 1194 IF_handle_file_transfer_serial_message(bt_buffer, bt_buffer_index, 1);
jah128 0:d6269d17c8cf 1195 } else {
jah128 0:d6269d17c8cf 1196 // debug("BT message timeout: %s [%d chars]\n", buffer, bt_buffer_index);
jah128 0:d6269d17c8cf 1197 if(bt_buffer_index == 5 && buffer[0] == COMMAND_MESSAGE_BYTE && buffer[4] == COMMAND_MESSAGE_BYTE) {
jah128 0:d6269d17c8cf 1198 bt_command_message[0] = buffer[1];
jah128 0:d6269d17c8cf 1199 bt_command_message[1] = buffer[2];
jah128 0:d6269d17c8cf 1200 bt_command_message[2] = buffer[3];
jah128 0:d6269d17c8cf 1201 IF_handle_command_serial_message(bt_command_message , 1);
jah128 0:d6269d17c8cf 1202 } else IF_handle_user_serial_message(bt_buffer, bt_buffer_index, 1);
jah128 0:d6269d17c8cf 1203 }
jah128 0:d6269d17c8cf 1204 bt_buffer_index = 0;
jah128 0:d6269d17c8cf 1205 }
jah128 0:d6269d17c8cf 1206
jah128 0:d6269d17c8cf 1207
jah128 3:7c0d1f581757 1208 //void IF_bt_rx_callback()
jah128 3:7c0d1f581757 1209 //{
jah128 3:7c0d1f581757 1210 // while(bt.readable()) {
jah128 3:7c0d1f581757 1211 // char byte = bt.getc();
jah128 3:7c0d1f581757 1212 //
jah128 3:7c0d1f581757 1213 // bt_buffer[bt_buffer_index] = byte;
jah128 3:7c0d1f581757 1214 // bt_buffer_index++;
jah128 3:7c0d1f581757 1215 // }
jah128 3:7c0d1f581757 1216 //
jah128 3:7c0d1f581757 1217 // bt_message_timeout.attach(&IF_bt_message_timeout, bt_message_timeout_period);
jah128 3:7c0d1f581757 1218 //}
jah128 0:d6269d17c8cf 1219
jah128 0:d6269d17c8cf 1220
jah128 11:312663037b8c 1221 void SerialControl::IF_set_filename(char * filename_in)
jah128 10:e58323951c08 1222 {
jah128 2:c6986ee3c7c5 1223 strcpy(filename,filename_in);
jah128 2:c6986ee3c7c5 1224 }
jah128 10:e58323951c08 1225
jah128 11:312663037b8c 1226 unsigned short SerialControl::IF_calculateCRC16(int file_length)
jah128 10:e58323951c08 1227 {
jah128 10:e58323951c08 1228 unsigned short crc16table[256] = {
jah128 2:c6986ee3c7c5 1229 0x0000, 0x8005, 0x800F, 0x000A, 0x801B, 0x001E, 0x0014, 0x8011,
jah128 2:c6986ee3c7c5 1230 0x8033, 0x0036, 0x003C, 0x8039, 0x0028, 0x802D, 0x8027, 0x0022,
jah128 2:c6986ee3c7c5 1231 0x8063, 0x0066, 0x006C, 0x8069, 0x0078, 0x807D, 0x8077, 0x0072,
jah128 2:c6986ee3c7c5 1232 0x0050, 0x8055, 0x805F, 0x005A, 0x804B, 0x004E, 0x0044, 0x8041,
jah128 2:c6986ee3c7c5 1233 0x80C3, 0x00C6, 0x00CC, 0x80C9, 0x00D8, 0x80DD, 0x80D7, 0x00D2,
jah128 2:c6986ee3c7c5 1234 0x00F0, 0x80F5, 0x80FF, 0x00FA, 0x80EB, 0x00EE, 0x00E4, 0x80E1,
jah128 2:c6986ee3c7c5 1235 0x00A0, 0x80A5, 0x80AF, 0x00AA, 0x80BB, 0x00BE, 0x00B4, 0x80B1,
jah128 2:c6986ee3c7c5 1236 0x8093, 0x0096, 0x009C, 0x8099, 0x0088, 0x808D, 0x8087, 0x0082,
jah128 2:c6986ee3c7c5 1237 0x8183, 0x0186, 0x018C, 0x8189, 0x0198, 0x819D, 0x8197, 0x0192,
jah128 2:c6986ee3c7c5 1238 0x01B0, 0x81B5, 0x81BF, 0x01BA, 0x81AB, 0x01AE, 0x01A4, 0x81A1,
jah128 2:c6986ee3c7c5 1239 0x01E0, 0x81E5, 0x81EF, 0x01EA, 0x81FB, 0x01FE, 0x01F4, 0x81F1,
jah128 2:c6986ee3c7c5 1240 0x81D3, 0x01D6, 0x01DC, 0x81D9, 0x01C8, 0x81CD, 0x81C7, 0x01C2,
jah128 2:c6986ee3c7c5 1241 0x0140, 0x8145, 0x814F, 0x014A, 0x815B, 0x015E, 0x0154, 0x8151,
jah128 2:c6986ee3c7c5 1242 0x8173, 0x0176, 0x017C, 0x8179, 0x0168, 0x816D, 0x8167, 0x0162,
jah128 2:c6986ee3c7c5 1243 0x8123, 0x0126, 0x012C, 0x8129, 0x0138, 0x813D, 0x8137, 0x0132,
jah128 10:e58323951c08 1244 0x0110, 0x8115, 0x811F, 0x011A, 0x810B, 0x010E, 0x0104, 0x8101,
jah128 2:c6986ee3c7c5 1245 0x8303, 0x0306, 0x030C, 0x8309, 0x0318, 0x831D, 0x8317, 0x0312,
jah128 2:c6986ee3c7c5 1246 0x0330, 0x8335, 0x833F, 0x033A, 0x832B, 0x032E, 0x0324, 0x8321,
jah128 2:c6986ee3c7c5 1247 0x0360, 0x8365, 0x836F, 0x036A, 0x837B, 0x037E, 0x0374, 0x8371,
jah128 2:c6986ee3c7c5 1248 0x8353, 0x0356, 0x035C, 0x8359, 0x0348, 0x834D, 0x8347, 0x0342,
jah128 2:c6986ee3c7c5 1249 0x03C0, 0x83C5, 0x83CF, 0x03CA, 0x83DB, 0x03DE, 0x03D4, 0x83D1,
jah128 2:c6986ee3c7c5 1250 0x83F3, 0x03F6, 0x03FC, 0x83F9, 0x03E8, 0x83ED, 0x83E7, 0x03E2,
jah128 2:c6986ee3c7c5 1251 0x83A3, 0x03A6, 0x03AC, 0x83A9, 0x03B8, 0x83BD, 0x83B7, 0x03B2,
jah128 2:c6986ee3c7c5 1252 0x0390, 0x8395, 0x839F, 0x039A, 0x838B, 0x038E, 0x0384, 0x8381,
jah128 2:c6986ee3c7c5 1253 0x0280, 0x8285, 0x828F, 0x028A, 0x829B, 0x029E, 0x0294, 0x8291,
jah128 2:c6986ee3c7c5 1254 0x82B3, 0x02B6, 0x02BC, 0x82B9, 0x02A8, 0x82AD, 0x82A7, 0x02A2,
jah128 2:c6986ee3c7c5 1255 0x82E3, 0x02E6, 0x02EC, 0x82E9, 0x02F8, 0x82FD, 0x82F7, 0x02F2,
jah128 2:c6986ee3c7c5 1256 0x02D0, 0x82D5, 0x82DF, 0x02DA, 0x82CB, 0x02CE, 0x02C4, 0x82C1,
jah128 2:c6986ee3c7c5 1257 0x8243, 0x0246, 0x024C, 0x8249, 0x0258, 0x825D, 0x8257, 0x0252,
jah128 2:c6986ee3c7c5 1258 0x0270, 0x8275, 0x827F, 0x027A, 0x826B, 0x026E, 0x0264, 0x8261,
jah128 2:c6986ee3c7c5 1259 0x0220, 0x8225, 0x822F, 0x022A, 0x823B, 0x023E, 0x0234, 0x8231,
jah128 10:e58323951c08 1260 0x8213, 0x0216, 0x021C, 0x8219, 0x0208, 0x820D, 0x8207, 0x0202
jah128 10:e58323951c08 1261 };
jah128 10:e58323951c08 1262
jah128 2:c6986ee3c7c5 1263 //Opens, reads and calculates the CRC16 value for file pointed to by filename
jah128 2:c6986ee3c7c5 1264 unsigned short crc_value = 0;
jah128 2:c6986ee3c7c5 1265 FILE *fp = fopen(filename,"r");
jah128 2:c6986ee3c7c5 1266 char * buffer;
jah128 2:c6986ee3c7c5 1267 int limit = 512;
jah128 2:c6986ee3c7c5 1268 if(file_length < 512) limit = file_length;
jah128 2:c6986ee3c7c5 1269 buffer = (char*) malloc (sizeof(char)*limit);
jah128 2:c6986ee3c7c5 1270 int blocks = 1;
jah128 2:c6986ee3c7c5 1271 if(file_length > limit) blocks += file_length / limit;
jah128 10:e58323951c08 1272 for(int i=0; i<blocks; i++) {
jah128 2:c6986ee3c7c5 1273 //Determine size of this block
jah128 2:c6986ee3c7c5 1274 int blocksize = limit;
jah128 10:e58323951c08 1275 if(i == blocks-1) {
jah128 10:e58323951c08 1276 if((file_length % limit) != 0) blocksize = file_length % limit;
jah128 10:e58323951c08 1277 }
jah128 12:878c6e9d9e60 1278 psi.debug("Calculating %d bytes of CRC data...\n",blocksize);
jah128 2:c6986ee3c7c5 1279 int result;
jah128 2:c6986ee3c7c5 1280 result = fread(buffer,1,blocksize,fp);
jah128 12:878c6e9d9e60 1281 psi.debug("Data read: %d\n",result);
jah128 10:e58323951c08 1282 for(int j=0; j<blocksize; j++) {
jah128 2:c6986ee3c7c5 1283 int subindex = ((crc_value>>8)^*(char *)(buffer[j]))&0x00FF;
jah128 2:c6986ee3c7c5 1284 //debug("J:%d Subindex:%d\n",j,subindex);
jah128 2:c6986ee3c7c5 1285 unsigned short table_value = crc16table[subindex];
jah128 10:e58323951c08 1286 crc_value=(crc_value<<8)^table_value;
jah128 2:c6986ee3c7c5 1287 }
jah128 2:c6986ee3c7c5 1288 }
jah128 2:c6986ee3c7c5 1289 fclose(fp);
jah128 12:878c6e9d9e60 1290 psi.debug("CRC Calculated: %x\n",crc_value);
jah128 10:e58323951c08 1291 return crc_value;
jah128 2:c6986ee3c7c5 1292 }
jah128 2:c6986ee3c7c5 1293
jah128 11:312663037b8c 1294 void SerialControl::IF_bt_rx_callback()
jah128 3:7c0d1f581757 1295 {
jah128 3:7c0d1f581757 1296 int count = 0;
jah128 3:7c0d1f581757 1297 char message_array[255];
jah128 3:7c0d1f581757 1298
jah128 3:7c0d1f581757 1299 wait_ms(500); // Wait 0.5ms to allow a complete message to arrive before atttempting to process it
jah128 3:7c0d1f581757 1300
jah128 3:7c0d1f581757 1301 while(bt.readable()) {
jah128 3:7c0d1f581757 1302 char tc = bt.getc();
jah128 3:7c0d1f581757 1303 message_array[count] = tc;
jah128 3:7c0d1f581757 1304 count ++;
jah128 3:7c0d1f581757 1305 if(bt_command_message_started == 1) {
jah128 3:7c0d1f581757 1306 if(bt_command_message_byte == 3) {
jah128 3:7c0d1f581757 1307 bt_command_timeout.detach();
jah128 3:7c0d1f581757 1308 if(tc == COMMAND_MESSAGE_BYTE) {
jah128 3:7c0d1f581757 1309 // A complete command message succesfully received, call handler
jah128 3:7c0d1f581757 1310 bt_command_message_started = 0;
jah128 3:7c0d1f581757 1311 count = 0;
jah128 3:7c0d1f581757 1312 IF_handle_command_serial_message(bt_command_message , 1);
jah128 3:7c0d1f581757 1313 } else {
jah128 3:7c0d1f581757 1314 // Message is not a valid command message as 5th byte is not correct; treat whole message as a user message
jah128 3:7c0d1f581757 1315 bt_command_message_started = 0;
jah128 3:7c0d1f581757 1316 message_array[0] = COMMAND_MESSAGE_BYTE;
jah128 3:7c0d1f581757 1317 message_array[1] = bt_command_message[0];
jah128 3:7c0d1f581757 1318 message_array[2] = bt_command_message[1];
jah128 3:7c0d1f581757 1319 message_array[3] = bt_command_message[2];
jah128 3:7c0d1f581757 1320 message_array[4] = tc;
jah128 3:7c0d1f581757 1321 count = 5;
jah128 3:7c0d1f581757 1322 }
jah128 3:7c0d1f581757 1323 } else {
jah128 11:312663037b8c 1324 bt_command_timeout.attach(this,&SerialControl::IF_bt_rx_command_timeout,command_timeout_period);
jah128 3:7c0d1f581757 1325 bt_command_message[bt_command_message_byte] = tc;
jah128 3:7c0d1f581757 1326 bt_command_message_byte ++;
jah128 3:7c0d1f581757 1327 }
jah128 3:7c0d1f581757 1328 } else {
jah128 3:7c0d1f581757 1329 if(count == 1) {
jah128 3:7c0d1f581757 1330 if(tc == COMMAND_MESSAGE_BYTE) {
jah128 3:7c0d1f581757 1331 bt_command_message_started = 1;
jah128 3:7c0d1f581757 1332 bt_command_message_byte = 0;
jah128 3:7c0d1f581757 1333
jah128 3:7c0d1f581757 1334 }
jah128 3:7c0d1f581757 1335 }
jah128 3:7c0d1f581757 1336 }
jah128 3:7c0d1f581757 1337 }
jah128 3:7c0d1f581757 1338 if(!bt_command_message_started && count>0) IF_handle_user_serial_message(message_array, count, 1);
jah128 3:7c0d1f581757 1339 }