For coursework of group 3 in SOFT564Z
Dependencies: Motordriver ros_lib_kinetic
Debug.cpp@4:8afc50a3e4ac, 2019-11-30 (annotated)
- Committer:
- Jonathan738
- Date:
- Sat Nov 30 10:59:09 2019 +0000
- Revision:
- 4:8afc50a3e4ac
- Child:
- 5:207e0cd8b9de
Added Debug code, re-factored existing code
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
Jonathan738 | 4:8afc50a3e4ac | 1 | /*------------------------------------------------------------------------------ |
Jonathan738 | 4:8afc50a3e4ac | 2 | Creator : Jonathan Wheadon |
Jonathan738 | 4:8afc50a3e4ac | 3 | Date : 29/11/2019 |
Jonathan738 | 4:8afc50a3e4ac | 4 | ------------------------------------------------------------------------------*/ |
Jonathan738 | 4:8afc50a3e4ac | 5 | #include "mbed.h" |
Jonathan738 | 4:8afc50a3e4ac | 6 | #include "rtos.h" |
Jonathan738 | 4:8afc50a3e4ac | 7 | #include "Debug.hpp" |
Jonathan738 | 4:8afc50a3e4ac | 8 | #include "General.hpp" |
Jonathan738 | 4:8afc50a3e4ac | 9 | #include "Pins.h" |
Jonathan738 | 4:8afc50a3e4ac | 10 | |
Jonathan738 | 4:8afc50a3e4ac | 11 | // queue for Terminal events |
Jonathan738 | 4:8afc50a3e4ac | 12 | EventQueue TerminalQueue(32 * EVENTS_EVENT_SIZE); |
Jonathan738 | 4:8afc50a3e4ac | 13 | |
Jonathan738 | 4:8afc50a3e4ac | 14 | // Create Object PC of class Terminal with Set SERIAL_TX and SERIAL_RX pins |
Jonathan738 | 4:8afc50a3e4ac | 15 | Terminal PC(SERIAL_TX, SERIAL_RX); |
Jonathan738 | 4:8afc50a3e4ac | 16 | |
Jonathan738 | 4:8afc50a3e4ac | 17 | /*------------------------------------------------------------------------------ |
Jonathan738 | 4:8afc50a3e4ac | 18 | Thread for handiling the terminal |
Jonathan738 | 4:8afc50a3e4ac | 19 | ------------------------------------------------------------------------------*/ |
Jonathan738 | 4:8afc50a3e4ac | 20 | void TerminalThread(void) |
Jonathan738 | 4:8afc50a3e4ac | 21 | { |
Jonathan738 | 4:8afc50a3e4ac | 22 | // Initialise the terminal printing the table. |
Jonathan738 | 4:8afc50a3e4ac | 23 | PC.init(); |
Jonathan738 | 4:8afc50a3e4ac | 24 | |
Jonathan738 | 4:8afc50a3e4ac | 25 | // Enter Forever loop |
Jonathan738 | 4:8afc50a3e4ac | 26 | while(1) { |
Jonathan738 | 4:8afc50a3e4ac | 27 | // loop forever dispatching any function called to the queue |
Jonathan738 | 4:8afc50a3e4ac | 28 | TerminalQueue.dispatch_forever(); |
Jonathan738 | 4:8afc50a3e4ac | 29 | } |
Jonathan738 | 4:8afc50a3e4ac | 30 | } |
Jonathan738 | 4:8afc50a3e4ac | 31 | |
Jonathan738 | 4:8afc50a3e4ac | 32 | |
Jonathan738 | 4:8afc50a3e4ac | 33 | /*------------------------------------------------------------------------------ |
Jonathan738 | 4:8afc50a3e4ac | 34 | initialize terminal, print table, initialize variables and attach interupts |
Jonathan738 | 4:8afc50a3e4ac | 35 | ------------------------------------------------------------------------------*/ |
Jonathan738 | 4:8afc50a3e4ac | 36 | void Terminal::init(void) |
Jonathan738 | 4:8afc50a3e4ac | 37 | { |
Jonathan738 | 4:8afc50a3e4ac | 38 | CurrentIDX = 0; |
Jonathan738 | 4:8afc50a3e4ac | 39 | col = false; |
Jonathan738 | 4:8afc50a3e4ac | 40 | |
Jonathan738 | 4:8afc50a3e4ac | 41 | // Set baud rate for serial object pc |
Jonathan738 | 4:8afc50a3e4ac | 42 | pc.baud (115200); |
Jonathan738 | 4:8afc50a3e4ac | 43 | |
Jonathan738 | 4:8afc50a3e4ac | 44 | // Hide cursor, move to x 0 y 0 and change print colour to green |
Jonathan738 | 4:8afc50a3e4ac | 45 | pc.printf("\x1b[?25l"); |
Jonathan738 | 4:8afc50a3e4ac | 46 | Cursor(0,0); |
Jonathan738 | 4:8afc50a3e4ac | 47 | Colour(ColourGREEN); |
Jonathan738 | 4:8afc50a3e4ac | 48 | |
Jonathan738 | 4:8afc50a3e4ac | 49 | // Print DATA table to present data |
Jonathan738 | 4:8afc50a3e4ac | 50 | pc.printf("| PROJ515 : DEBUG MENU | |\n\r" |
Jonathan738 | 4:8afc50a3e4ac | 51 | "|***********************************************************************************************|\n\r" |
Jonathan738 | 4:8afc50a3e4ac | 52 | "| Error Level | Time Stamp | Error MSG |\n\r" |
Jonathan738 | 4:8afc50a3e4ac | 53 | "|-----------------------|-----------------------|-----------------------------------------------|\n\r"); |
Jonathan738 | 4:8afc50a3e4ac | 54 | for(char idx = 0; idx < Rows; idx++) { |
Jonathan738 | 4:8afc50a3e4ac | 55 | pc.printf("| | | |\n\r"); |
Jonathan738 | 4:8afc50a3e4ac | 56 | } |
Jonathan738 | 4:8afc50a3e4ac | 57 | pc.printf("|***********************************************************************************************|\n\r" |
Jonathan738 | 4:8afc50a3e4ac | 58 | "| Input Command Line : |\n\r" |
Jonathan738 | 4:8afc50a3e4ac | 59 | "|-----------------------------------------------------------------------------------------------|\n\r" |
Jonathan738 | 4:8afc50a3e4ac | 60 | "| |\n\r" |
Jonathan738 | 4:8afc50a3e4ac | 61 | "|***********************************************************************************************|\n\r" |
Jonathan738 | 4:8afc50a3e4ac | 62 | " "); |
Jonathan738 | 4:8afc50a3e4ac | 63 | |
Jonathan738 | 4:8afc50a3e4ac | 64 | // initialise variables |
Jonathan738 | 4:8afc50a3e4ac | 65 | buffer_pointer = 0; |
Jonathan738 | 4:8afc50a3e4ac | 66 | |
Jonathan738 | 4:8afc50a3e4ac | 67 | // Attach interupts |
Jonathan738 | 4:8afc50a3e4ac | 68 | pc.attach(this, &Terminal::Input_Handler,pc.RxIrq); // This interupt fires whenever data is added to the terminal buffer and calls function input handler |
Jonathan738 | 4:8afc50a3e4ac | 69 | } |
Jonathan738 | 4:8afc50a3e4ac | 70 | |
Jonathan738 | 4:8afc50a3e4ac | 71 | /*------------------------------------------------------------------------------ |
Jonathan738 | 4:8afc50a3e4ac | 72 | Move cursor of pc terminal to co-ordinates 'X' and 'Y' (between 0 and 255) |
Jonathan738 | 4:8afc50a3e4ac | 73 | ------------------------------------------------------------------------------*/ |
Jonathan738 | 4:8afc50a3e4ac | 74 | void Terminal::Cursor(char X, char Y) |
Jonathan738 | 4:8afc50a3e4ac | 75 | { |
Jonathan738 | 4:8afc50a3e4ac | 76 | pc.printf("\x1b[%d;%dH",Y,X); |
Jonathan738 | 4:8afc50a3e4ac | 77 | } |
Jonathan738 | 4:8afc50a3e4ac | 78 | |
Jonathan738 | 4:8afc50a3e4ac | 79 | /*------------------------------------------------------------------------------ |
Jonathan738 | 4:8afc50a3e4ac | 80 | Change pc terminal print colour (8 bit colour) to colour defined by "COLOUR" |
Jonathan738 | 4:8afc50a3e4ac | 81 | ------------------------------------------------------------------------------*/ |
Jonathan738 | 4:8afc50a3e4ac | 82 | void Terminal::Colour(char COLOUR) |
Jonathan738 | 4:8afc50a3e4ac | 83 | { |
Jonathan738 | 4:8afc50a3e4ac | 84 | pc.printf("\x1b[38;5;%dm",COLOUR); |
Jonathan738 | 4:8afc50a3e4ac | 85 | } |
Jonathan738 | 4:8afc50a3e4ac | 86 | |
Jonathan738 | 4:8afc50a3e4ac | 87 | /*------------------------------------------------------------------------------ |
Jonathan738 | 4:8afc50a3e4ac | 88 | Prints data(STRING) to cell in table defined by IDX |
Jonathan738 | 4:8afc50a3e4ac | 89 | ------------------------------------------------------------------------------*/ |
Jonathan738 | 4:8afc50a3e4ac | 90 | void Terminal::PrintDATA(char* STRING, char IDX) |
Jonathan738 | 4:8afc50a3e4ac | 91 | { |
Jonathan738 | 4:8afc50a3e4ac | 92 | char Y = (IDX/4)+5; |
Jonathan738 | 4:8afc50a3e4ac | 93 | char X = ((IDX%4)*23)+3; |
Jonathan738 | 4:8afc50a3e4ac | 94 | Cursor(X,Y); |
Jonathan738 | 4:8afc50a3e4ac | 95 | pc.printf("%s",STRING); |
Jonathan738 | 4:8afc50a3e4ac | 96 | } |
Jonathan738 | 4:8afc50a3e4ac | 97 | |
Jonathan738 | 4:8afc50a3e4ac | 98 | /*------------------------------------------------------------------------------ |
Jonathan738 | 4:8afc50a3e4ac | 99 | Function checks terminal buffer to see which key has been entered and saves to |
Jonathan738 | 4:8afc50a3e4ac | 100 | internal buffer, if carridge return (0x0D) has been pressed it calls function |
Jonathan738 | 4:8afc50a3e4ac | 101 | to decode the command and add the relavant function to the event QUEUE |
Jonathan738 | 4:8afc50a3e4ac | 102 | ------------------------------------------------------------------------------*/ |
Jonathan738 | 4:8afc50a3e4ac | 103 | void Terminal::checkKEY(void) |
Jonathan738 | 4:8afc50a3e4ac | 104 | { |
Jonathan738 | 4:8afc50a3e4ac | 105 | // read char in buffer and save as gotkey |
Jonathan738 | 4:8afc50a3e4ac | 106 | char gotkey; |
Jonathan738 | 4:8afc50a3e4ac | 107 | gotkey=pc.getc(); |
Jonathan738 | 4:8afc50a3e4ac | 108 | |
Jonathan738 | 4:8afc50a3e4ac | 109 | // if not byte is found do nothing and exit |
Jonathan738 | 4:8afc50a3e4ac | 110 | if(gotkey == NULL) { |
Jonathan738 | 4:8afc50a3e4ac | 111 | // do nothing |
Jonathan738 | 4:8afc50a3e4ac | 112 | |
Jonathan738 | 4:8afc50a3e4ac | 113 | // if key is enter (carridge return) call handleCOMMAND function |
Jonathan738 | 4:8afc50a3e4ac | 114 | } else if (gotkey == 0x0D) { |
Jonathan738 | 4:8afc50a3e4ac | 115 | Terminal_buffer[buffer_pointer] = 0x00; |
Jonathan738 | 4:8afc50a3e4ac | 116 | TerminalQueue.call(&PC, &Terminal::HandleCOMMAND); |
Jonathan738 | 4:8afc50a3e4ac | 117 | buffer_pointer = 0; |
Jonathan738 | 4:8afc50a3e4ac | 118 | Cursor((24),(Rows+6)); |
Jonathan738 | 4:8afc50a3e4ac | 119 | pc.printf(" "); |
Jonathan738 | 4:8afc50a3e4ac | 120 | |
Jonathan738 | 4:8afc50a3e4ac | 121 | // if key entered is backspace print " " over last printed char and shift bufferpointer back |
Jonathan738 | 4:8afc50a3e4ac | 122 | } else if (gotkey == 0x7f) { |
Jonathan738 | 4:8afc50a3e4ac | 123 | if (buffer_pointer > 0) { |
Jonathan738 | 4:8afc50a3e4ac | 124 | buffer_pointer -= 1; |
Jonathan738 | 4:8afc50a3e4ac | 125 | Cursor((24 + buffer_pointer),(Rows+6)); |
Jonathan738 | 4:8afc50a3e4ac | 126 | pc.printf(" "); |
Jonathan738 | 4:8afc50a3e4ac | 127 | } else { |
Jonathan738 | 4:8afc50a3e4ac | 128 | //do nothing |
Jonathan738 | 4:8afc50a3e4ac | 129 | } |
Jonathan738 | 4:8afc50a3e4ac | 130 | |
Jonathan738 | 4:8afc50a3e4ac | 131 | // if input not specific command save the char to the internal buffer |
Jonathan738 | 4:8afc50a3e4ac | 132 | } else { |
Jonathan738 | 4:8afc50a3e4ac | 133 | Colour(ColourWHITE); |
Jonathan738 | 4:8afc50a3e4ac | 134 | if(buffer_pointer == 30) { |
Jonathan738 | 4:8afc50a3e4ac | 135 | // do error thing |
Jonathan738 | 4:8afc50a3e4ac | 136 | } else { |
Jonathan738 | 4:8afc50a3e4ac | 137 | pc.printf("\x1b[4m"); |
Jonathan738 | 4:8afc50a3e4ac | 138 | Cursor((24 + buffer_pointer),(Rows+6)); |
Jonathan738 | 4:8afc50a3e4ac | 139 | Colour(ColourWHITE); |
Jonathan738 | 4:8afc50a3e4ac | 140 | pc.printf("%c",gotkey); |
Jonathan738 | 4:8afc50a3e4ac | 141 | pc.printf("\x1b[24m"); |
Jonathan738 | 4:8afc50a3e4ac | 142 | Terminal_buffer[buffer_pointer] = gotkey; |
Jonathan738 | 4:8afc50a3e4ac | 143 | buffer_pointer++; |
Jonathan738 | 4:8afc50a3e4ac | 144 | } |
Jonathan738 | 4:8afc50a3e4ac | 145 | } |
Jonathan738 | 4:8afc50a3e4ac | 146 | // re attach the interupt on the serial input flag |
Jonathan738 | 4:8afc50a3e4ac | 147 | pc.attach(this, &Terminal::Input_Handler,pc.RxIrq); |
Jonathan738 | 4:8afc50a3e4ac | 148 | } |
Jonathan738 | 4:8afc50a3e4ac | 149 | |
Jonathan738 | 4:8afc50a3e4ac | 150 | /*------------------------------------------------------------------------------ |
Jonathan738 | 4:8afc50a3e4ac | 151 | Function decodes command that has been input on input command line and performs |
Jonathan738 | 4:8afc50a3e4ac | 152 | any necasary action |
Jonathan738 | 4:8afc50a3e4ac | 153 | ------------------------------------------------------------------------------*/ |
Jonathan738 | 4:8afc50a3e4ac | 154 | void Terminal::HandleCOMMAND(void) |
Jonathan738 | 4:8afc50a3e4ac | 155 | { |
Jonathan738 | 4:8afc50a3e4ac | 156 | int commandSize = 0; |
Jonathan738 | 4:8afc50a3e4ac | 157 | int dataSIZE = 0; |
Jonathan738 | 4:8afc50a3e4ac | 158 | bool searchingCOMMAND = true; |
Jonathan738 | 4:8afc50a3e4ac | 159 | bool searchingDATA = true; |
Jonathan738 | 4:8afc50a3e4ac | 160 | while(searchingCOMMAND) { |
Jonathan738 | 4:8afc50a3e4ac | 161 | if((Terminal_buffer[commandSize] == ' ') || (Terminal_buffer[commandSize] == 0x00)) { |
Jonathan738 | 4:8afc50a3e4ac | 162 | searchingCOMMAND = false; |
Jonathan738 | 4:8afc50a3e4ac | 163 | } else { |
Jonathan738 | 4:8afc50a3e4ac | 164 | commandSize++; |
Jonathan738 | 4:8afc50a3e4ac | 165 | } |
Jonathan738 | 4:8afc50a3e4ac | 166 | |
Jonathan738 | 4:8afc50a3e4ac | 167 | // debug break loop if too big |
Jonathan738 | 4:8afc50a3e4ac | 168 | if(commandSize > 8) { |
Jonathan738 | 4:8afc50a3e4ac | 169 | break; |
Jonathan738 | 4:8afc50a3e4ac | 170 | } |
Jonathan738 | 4:8afc50a3e4ac | 171 | } |
Jonathan738 | 4:8afc50a3e4ac | 172 | while(searchingDATA) { |
Jonathan738 | 4:8afc50a3e4ac | 173 | if((Terminal_buffer[commandSize+dataSIZE+1] == ' ') || (Terminal_buffer[commandSize+dataSIZE+1] == 0x00)) { |
Jonathan738 | 4:8afc50a3e4ac | 174 | searchingDATA = false; |
Jonathan738 | 4:8afc50a3e4ac | 175 | } else { |
Jonathan738 | 4:8afc50a3e4ac | 176 | dataSIZE++; |
Jonathan738 | 4:8afc50a3e4ac | 177 | } |
Jonathan738 | 4:8afc50a3e4ac | 178 | |
Jonathan738 | 4:8afc50a3e4ac | 179 | // debug break loop if too big |
Jonathan738 | 4:8afc50a3e4ac | 180 | if(dataSIZE > 8) { |
Jonathan738 | 4:8afc50a3e4ac | 181 | break; |
Jonathan738 | 4:8afc50a3e4ac | 182 | } |
Jonathan738 | 4:8afc50a3e4ac | 183 | } |
Jonathan738 | 4:8afc50a3e4ac | 184 | } |
Jonathan738 | 4:8afc50a3e4ac | 185 | |
Jonathan738 | 4:8afc50a3e4ac | 186 | /*------------------------------------------------------------------------------ |
Jonathan738 | 4:8afc50a3e4ac | 187 | ISR for whenever data is in terminal buffer, add function to terminal queue |
Jonathan738 | 4:8afc50a3e4ac | 188 | to check which charecteter has been entered and add it to a buffer. |
Jonathan738 | 4:8afc50a3e4ac | 189 | ------------------------------------------------------------------------------*/ |
Jonathan738 | 4:8afc50a3e4ac | 190 | void Terminal::Input_Handler(void) |
Jonathan738 | 4:8afc50a3e4ac | 191 | { |
Jonathan738 | 4:8afc50a3e4ac | 192 | // remove the interupt from the serial input flag |
Jonathan738 | 4:8afc50a3e4ac | 193 | pc.attach(NULL,pc.RxIrq); |
Jonathan738 | 4:8afc50a3e4ac | 194 | // if data is in the buffer call function to check which key has been entered |
Jonathan738 | 4:8afc50a3e4ac | 195 | if(pc.readable()!= 0) { |
Jonathan738 | 4:8afc50a3e4ac | 196 | TerminalQueue.call(&PC, &Terminal::checkKEY); |
Jonathan738 | 4:8afc50a3e4ac | 197 | |
Jonathan738 | 4:8afc50a3e4ac | 198 | // if no data present in the buffer re attach the interupt |
Jonathan738 | 4:8afc50a3e4ac | 199 | } else { |
Jonathan738 | 4:8afc50a3e4ac | 200 | pc.attach(this, &Terminal::Input_Handler,pc.RxIrq); |
Jonathan738 | 4:8afc50a3e4ac | 201 | } |
Jonathan738 | 4:8afc50a3e4ac | 202 | } |
Jonathan738 | 4:8afc50a3e4ac | 203 | |
Jonathan738 | 4:8afc50a3e4ac | 204 | /*------------------------------------------------------------------------------ |
Jonathan738 | 4:8afc50a3e4ac | 205 | Function prints debug messages to screen |
Jonathan738 | 4:8afc50a3e4ac | 206 | ------------------------------------------------------------------------------*/ |
Jonathan738 | 4:8afc50a3e4ac | 207 | void Terminal::printDEBUG(char* msgs) |
Jonathan738 | 4:8afc50a3e4ac | 208 | { |
Jonathan738 | 4:8afc50a3e4ac | 209 | // first delete any messages shown in the debug line |
Jonathan738 | 4:8afc50a3e4ac | 210 | Cursor(3,(Rows+8)); |
Jonathan738 | 4:8afc50a3e4ac | 211 | Colour(ColourWHITE); |
Jonathan738 | 4:8afc50a3e4ac | 212 | pc.printf(" "); |
Jonathan738 | 4:8afc50a3e4ac | 213 | // now print the new data to the line |
Jonathan738 | 4:8afc50a3e4ac | 214 | Cursor(3,(Rows+8)); |
Jonathan738 | 4:8afc50a3e4ac | 215 | pc.printf(msgs); |
Jonathan738 | 4:8afc50a3e4ac | 216 | } |
Jonathan738 | 4:8afc50a3e4ac | 217 | |
Jonathan738 | 4:8afc50a3e4ac | 218 | /*------------------------------------------------------------------------------ |
Jonathan738 | 4:8afc50a3e4ac | 219 | Function prints ERROR messages to screen |
Jonathan738 | 4:8afc50a3e4ac | 220 | ------------------------------------------------------------------------------*/ |
Jonathan738 | 4:8afc50a3e4ac | 221 | void Terminal::ERROR_MSGS(Error msgs) |
Jonathan738 | 4:8afc50a3e4ac | 222 | { |
Jonathan738 | 4:8afc50a3e4ac | 223 | // first delete any messages shown in the debug line |
Jonathan738 | 4:8afc50a3e4ac | 224 | Cursor(3,(Rows+8)); |
Jonathan738 | 4:8afc50a3e4ac | 225 | pc.printf(" "); |
Jonathan738 | 4:8afc50a3e4ac | 226 | Cursor(3,(Rows+8)); |
Jonathan738 | 4:8afc50a3e4ac | 227 | |
Jonathan738 | 4:8afc50a3e4ac | 228 | char *TimeStamp = " 10:55-30/11/2019 "; |
Jonathan738 | 4:8afc50a3e4ac | 229 | |
Jonathan738 | 4:8afc50a3e4ac | 230 | // Print diferent messages dependent on the Error code attached in the struct Error |
Jonathan738 | 4:8afc50a3e4ac | 231 | switch(msgs.ErrorCode) { |
Jonathan738 | 4:8afc50a3e4ac | 232 | case warning : |
Jonathan738 | 4:8afc50a3e4ac | 233 | Colour(ColourYELLOW); |
Jonathan738 | 4:8afc50a3e4ac | 234 | PrintDATA(" Warning" , CurrentIDX); |
Jonathan738 | 4:8afc50a3e4ac | 235 | PrintDATA(TimeStamp , CurrentIDX+1); |
Jonathan738 | 4:8afc50a3e4ac | 236 | PrintDATA(msgs.ErrorMSGS , CurrentIDX+2); |
Jonathan738 | 4:8afc50a3e4ac | 237 | CurrentIDX += 4; |
Jonathan738 | 4:8afc50a3e4ac | 238 | break; |
Jonathan738 | 4:8afc50a3e4ac | 239 | |
Jonathan738 | 4:8afc50a3e4ac | 240 | case fault : |
Jonathan738 | 4:8afc50a3e4ac | 241 | Colour(ColourAMBER); |
Jonathan738 | 4:8afc50a3e4ac | 242 | PrintDATA(" Fault" , CurrentIDX); |
Jonathan738 | 4:8afc50a3e4ac | 243 | PrintDATA(TimeStamp , CurrentIDX+1); |
Jonathan738 | 4:8afc50a3e4ac | 244 | PrintDATA(msgs.ErrorMSGS , CurrentIDX+2); |
Jonathan738 | 4:8afc50a3e4ac | 245 | CurrentIDX += 4; |
Jonathan738 | 4:8afc50a3e4ac | 246 | break; |
Jonathan738 | 4:8afc50a3e4ac | 247 | |
Jonathan738 | 4:8afc50a3e4ac | 248 | case criticalERROR : |
Jonathan738 | 4:8afc50a3e4ac | 249 | Colour(ColourRED); |
Jonathan738 | 4:8afc50a3e4ac | 250 | PrintDATA(" Critical ERROR" , CurrentIDX); |
Jonathan738 | 4:8afc50a3e4ac | 251 | PrintDATA(TimeStamp , CurrentIDX+1); |
Jonathan738 | 4:8afc50a3e4ac | 252 | PrintDATA(msgs.ErrorMSGS , CurrentIDX+2); |
Jonathan738 | 4:8afc50a3e4ac | 253 | CurrentIDX += 4; |
Jonathan738 | 4:8afc50a3e4ac | 254 | break; |
Jonathan738 | 4:8afc50a3e4ac | 255 | |
Jonathan738 | 4:8afc50a3e4ac | 256 | case criticalFAILURE : |
Jonathan738 | 4:8afc50a3e4ac | 257 | Colour(ColourRED); |
Jonathan738 | 4:8afc50a3e4ac | 258 | PrintDATA(" Critical FAILURE" , CurrentIDX); |
Jonathan738 | 4:8afc50a3e4ac | 259 | PrintDATA(TimeStamp , CurrentIDX+1); |
Jonathan738 | 4:8afc50a3e4ac | 260 | PrintDATA(msgs.ErrorMSGS , CurrentIDX+2); |
Jonathan738 | 4:8afc50a3e4ac | 261 | CurrentIDX += 4; |
Jonathan738 | 4:8afc50a3e4ac | 262 | // This point should never be reached as it would indicate a full |
Jonathan738 | 4:8afc50a3e4ac | 263 | // failure which would cause the watchdog to reset the system |
Jonathan738 | 4:8afc50a3e4ac | 264 | break; |
Jonathan738 | 4:8afc50a3e4ac | 265 | } |
Jonathan738 | 4:8afc50a3e4ac | 266 | } |
Jonathan738 | 4:8afc50a3e4ac | 267 | |
Jonathan738 | 4:8afc50a3e4ac | 268 | /*------------------------------------------------------------------------------ |
Jonathan738 | 4:8afc50a3e4ac | 269 | Function converts string to float, one decimal place expected must be 4 bytes |
Jonathan738 | 4:8afc50a3e4ac | 270 | long, PAd with '0', example 1.2f would be "01.2" and 12.3f would be "12.3" |
Jonathan738 | 4:8afc50a3e4ac | 271 | ------------------------------------------------------------------------------*/ |
Jonathan738 | 4:8afc50a3e4ac | 272 | float strTOflt(char ary[4]) |
Jonathan738 | 4:8afc50a3e4ac | 273 | { |
Jonathan738 | 4:8afc50a3e4ac | 274 | //check that values entered are in format "00.0" |
Jonathan738 | 4:8afc50a3e4ac | 275 | for(int tester = 0; tester < 4; tester++) { |
Jonathan738 | 4:8afc50a3e4ac | 276 | if(tester != 2) { |
Jonathan738 | 4:8afc50a3e4ac | 277 | int testing = (int)(ary[tester]-'0'); |
Jonathan738 | 4:8afc50a3e4ac | 278 | if((testing > 9) || (testing < 0)) { |
Jonathan738 | 4:8afc50a3e4ac | 279 | return NULL; |
Jonathan738 | 4:8afc50a3e4ac | 280 | } |
Jonathan738 | 4:8afc50a3e4ac | 281 | } else if(ary[tester] != '.') { |
Jonathan738 | 4:8afc50a3e4ac | 282 | return NULL; |
Jonathan738 | 4:8afc50a3e4ac | 283 | } |
Jonathan738 | 4:8afc50a3e4ac | 284 | } |
Jonathan738 | 4:8afc50a3e4ac | 285 | |
Jonathan738 | 4:8afc50a3e4ac | 286 | float retFlt; |
Jonathan738 | 4:8afc50a3e4ac | 287 | |
Jonathan738 | 4:8afc50a3e4ac | 288 | retFlt = ((float)(ary[0]-'0'))*10.0f; |
Jonathan738 | 4:8afc50a3e4ac | 289 | retFlt += ((float)(ary[1]-'0')); |
Jonathan738 | 4:8afc50a3e4ac | 290 | retFlt += ((float)(ary[3]-'0'))/10.0f; |
Jonathan738 | 4:8afc50a3e4ac | 291 | |
Jonathan738 | 4:8afc50a3e4ac | 292 | if(retFlt > 60.0f) { |
Jonathan738 | 4:8afc50a3e4ac | 293 | return NULL; |
Jonathan738 | 4:8afc50a3e4ac | 294 | } else { |
Jonathan738 | 4:8afc50a3e4ac | 295 | return retFlt; |
Jonathan738 | 4:8afc50a3e4ac | 296 | } |
Jonathan738 | 4:8afc50a3e4ac | 297 | } |
Jonathan738 | 4:8afc50a3e4ac | 298 | |
Jonathan738 | 4:8afc50a3e4ac | 299 | /*------------------------------------------------------------------------------ |
Jonathan738 | 4:8afc50a3e4ac | 300 | Function converts string to int. |
Jonathan738 | 4:8afc50a3e4ac | 301 | expected format allows upto 3 digits, 1 to 999 |
Jonathan738 | 4:8afc50a3e4ac | 302 | ------------------------------------------------------------------------------*/ |
Jonathan738 | 4:8afc50a3e4ac | 303 | int strTOint(char ary[3]) |
Jonathan738 | 4:8afc50a3e4ac | 304 | { |
Jonathan738 | 4:8afc50a3e4ac | 305 | //check that values entered are in format "000" |
Jonathan738 | 4:8afc50a3e4ac | 306 | for(int tester = 0; tester < 3; tester++) { |
Jonathan738 | 4:8afc50a3e4ac | 307 | int testing = (int)(ary[tester]-'0'); |
Jonathan738 | 4:8afc50a3e4ac | 308 | if((testing > 9) || (testing < 0)) { |
Jonathan738 | 4:8afc50a3e4ac | 309 | return NULL; |
Jonathan738 | 4:8afc50a3e4ac | 310 | } |
Jonathan738 | 4:8afc50a3e4ac | 311 | } |
Jonathan738 | 4:8afc50a3e4ac | 312 | |
Jonathan738 | 4:8afc50a3e4ac | 313 | int temp_int = (int)(ary[0]-'0')*100; |
Jonathan738 | 4:8afc50a3e4ac | 314 | temp_int += (int)(ary[1]-'0')*10; |
Jonathan738 | 4:8afc50a3e4ac | 315 | temp_int += (int)(ary[2]-'0'); |
Jonathan738 | 4:8afc50a3e4ac | 316 | |
Jonathan738 | 4:8afc50a3e4ac | 317 | return temp_int; |
Jonathan738 | 4:8afc50a3e4ac | 318 | } |
Jonathan738 | 4:8afc50a3e4ac | 319 | /*------------------------------------------------------------------------------ |
Jonathan738 | 4:8afc50a3e4ac | 320 | Function to create and send an error message |
Jonathan738 | 4:8afc50a3e4ac | 321 | ------------------------------------------------------------------------------*/ |
Jonathan738 | 4:8afc50a3e4ac | 322 | void Flag_Error(int ErrorCode, char* ErrorMSG) |
Jonathan738 | 4:8afc50a3e4ac | 323 | { |
Jonathan738 | 4:8afc50a3e4ac | 324 | Error msgs; |
Jonathan738 | 4:8afc50a3e4ac | 325 | msgs.ErrorCode = ErrorCode; |
Jonathan738 | 4:8afc50a3e4ac | 326 | msgs.ErrorMSGS = ErrorMSG; |
Jonathan738 | 4:8afc50a3e4ac | 327 | TerminalQueue.call(&PC, &Terminal::ERROR_MSGS, msgs); |
Jonathan738 | 4:8afc50a3e4ac | 328 | } |