void ProcessTerminal()
{
BYTE i,j;
BYTE Out;
//Make sure to prime the UART Pump if there is data in the queue
//but TX IRQ is disabled
if(BytesInQueue(&TERMINAL_OUT_QUEUE)>0 && (UART2_S1 & UART_S1_TDRE_MASK))
{
UART2_C2 |= UART_C2_TIE_MASK; //Enable Reciever Interrupts
}
if(BytesInQueue(&TERMINAL_IN_QUEUE)>0)
{
ByteDequeue(&TERMINAL_IN_QUEUE,&NextCharIn);
switch(NextCharIn)
{
case '\r':
TerminalLineBuf[TerminalPos++] = 0x0;
ByteEnqueue(&TERMINAL_OUT_QUEUE,NextCharIn);
if(TerminalPos > 1)
{
//find the command
i=0;
while(TerminalLineBuf[i]>0x20 && TerminalLineBuf[i]<0x7f)
{
TerminalCmdBuf[i] = TerminalLineBuf[i];
i++;
if(i==MAX_TERMINAL_CMD_CHARS)
{
break;
}
}
TerminalCmdBuf[i] = 0;
TerminalCmdBuf[i+1] = 0;
strcpy(TerminalArgs,&TerminalLineBuf[i]);
CmdFound = FALSE;
for(j=0;j<NUM_TERMINAL_CMDS;j++)
{
if(strcmp(TerminalCmdBuf,TerminalCommands[j]) == 0)
{
printf_Q(&TERMINAL_OUT_QUEUE,"\r\n");
if(TerminalCallbacks[j] != NULL)
TerminalCallbacks[j](TerminalArgs);
CmdFound = TRUE;
break;
}
}
if(CmdFound == FALSE)
{
printf_Q(&TERMINAL_OUT_QUEUE,"\r\n%s command not recognized.\r\n", TerminalCmdBuf);
}
}
ByteEnqueue(&TERMINAL_OUT_QUEUE,'\r\n');
ByteEnqueue(&TERMINAL_OUT_QUEUE,'>');
TerminalPos = 0;
break;
case '\b':
if(TerminalPos > 0)
{
TerminalPos--;
ByteEnqueue(&TERMINAL_OUT_QUEUE,NextCharIn);
}
break;
default:
if(TerminalPos == 0 && NextCharIn == 0x020)
{
//Do nothing if space bar is pressed at beginning of line
}
else if(NextCharIn >= 0x20 && NextCharIn<0x7F)
{
if(TerminalPos < MAX_TERMINAL_LINE_CHARS-1)
{
TerminalLineBuf[TerminalPos++] = NextCharIn;
ByteEnqueue(&TERMINAL_OUT_QUEUE,NextCharIn);
}
}
break;
}
}
}
void distantio_decode_rx_frame(ByteQueue* rx_queue)
{
//Get payload byte by byte
uint8_t byte = ForcedByteDequeue(rx_queue);
uint8_t byte2;
uint16_t id;
void* ptr;
float* tmp_float;
uint32_t* tmp_uint32;
float* to_float;
int32_t* to_int;
uint8_t bytes[4];
uint8_t type;
//If command is return table to master
if(byte == 0x02)
{
distantio_send_table();
}
//If command is write variable value
else if(byte == 0x01)
{
type = ForcedByteDequeue(rx_queue);
byte = ForcedByteDequeue(rx_queue);
byte2 = ForcedByteDequeue(rx_queue);
id = byte;
//TOCHECK : 4 or 8 shift ?
id = byte + (byte2<<8);
if(id < Log.current_index)
{
if(Log.variables[id].writeable == 1 && BytesInQueue(rx_queue) >= 4)
{
bytes[0] = ForcedByteDequeue(rx_queue);
bytes[1] = ForcedByteDequeue(rx_queue);
bytes[2] = ForcedByteDequeue(rx_queue);
bytes[3] = ForcedByteDequeue(rx_queue);
if(type == 0x00)
{
to_float = (float *)(&bytes[0]);
ptr = (void *)(Log.variables[id].ptr);
tmp_float = (float *)(ptr);
*tmp_float = *to_float;
}
else if(type == 0x06)
{
//TODO : Use void ptr
to_int = (int32_t*)(&bytes[0]);
ptr = (void *)(Log.variables[id].ptr);
tmp_uint32 = (uint32_t*)(ptr);
*tmp_uint32 = *to_int;
}
}
else
{
}
}
}
//If command is return variable
else if(byte == 0x00 && BytesInQueue(rx_queue) >= 3)
{
type = ForcedByteDequeue(rx_queue);
byte = ForcedByteDequeue(rx_queue);
byte2 = ForcedByteDequeue(rx_queue);
id = byte;
//TOCHECK : 4 or 8 shift ?
id = byte + (byte2<<8);
if(id < Log.current_index)
{
Log.variables[id].send = 1;
}
}
//Clean queue
while(BytesInQueue(rx_queue))
ForcedByteDequeue(rx_queue);
}
