diff --git a/platformio.ini b/platformio.ini index eb52872..0fe26dd 100644 --- a/platformio.ini +++ b/platformio.ini @@ -25,7 +25,7 @@ build_unflags = -std=gnu++11 board_build.arduino.memory_type = qio_opi lib_deps = - ../lib/logger/ + ../lib/embeslogger/ ../lib/eslabsCurses/ lib_extra_dirs = ../lib diff --git a/src/communication/walkieTalkie.cpp b/src/communication/walkieTalkie.cpp index 6503575..4b01db2 100644 --- a/src/communication/walkieTalkie.cpp +++ b/src/communication/walkieTalkie.cpp @@ -16,12 +16,23 @@ namespace WalkieTalkie { } } - static inline void RecvCMDRoutine(Command *command, byte *b) { + static inline void ResetStateMachine() { + state = WaitingSTX; + resetBuffer(); + } + + static inline void RecvCMDRoutine(byte *b) { LOG_TRACE(F("Reading the command byte\n")); - *command = Command(*b); - LOG_TRACE(F(" Command: %d\n"), *b); + uint8_t cmdVal = Command(*b); + if (cmdVal == 0 || cmdVal > CMDData) { + LOG_TRACE(F("Invalid CMD byte: 0x%02x. Resetting parser.\n"), cmdVal); + ResetStateMachine(); + return; + } + LOG_TRACE(F(" Command: %d\n"), *b); + tempCommand = (Command)cmdVal; state = RecvLen; resetBuffer(); @@ -33,6 +44,13 @@ namespace WalkieTalkie { // we are currently using a int16_t for the len so: 2 bytes if (bufferIndex > 1) { len = buffer[0] | (buffer[1] << 8); + + if (len > 2056 || len == 0) { + LOG_WARN(F("Corrupt length received: %d. Resetting parser.\n"), len); + ResetStateMachine(); + return; + } + LOG_TRACE(F("Len: %d\n"), len); state = RecvPayload; resetBuffer(); @@ -61,7 +79,7 @@ namespace WalkieTalkie { return 0; } - static inline int RecvETXRoutine(uint8_t *payload, byte *b) { + static inline int RecvETXRoutine(Command *command, uint8_t *payload, byte *b) { LOG_TRACE(F("READING ETX\n")); state = WaitingSTX; @@ -73,15 +91,21 @@ namespace WalkieTalkie { LOG_TRACE(F("CRC don't match. Expected: %d, got: %d\n"), crc, computedCRC); return -2; } else { + *command = tempCommand; return len; } } int16_t RecvStream(Command *command, uint8_t *payload, uint16_t payloadMax) { - int16_t result = 0; + if (state != WaitingSTX && (millis() - lastByteTime > SERIAL_TIMEOUT_MS)) { + state = WaitingSTX; + resetBuffer(); + } while (Serial.available() > 0) { + lastByteTime = millis(); uint8_t byte = Serial.read(); + switch(state) { case WaitingSTX: { WaitingSTXRoutine(&byte); @@ -89,7 +113,7 @@ namespace WalkieTalkie { } case RecvCMD: { - RecvCMDRoutine(command, &byte); + RecvCMDRoutine(&byte); break; } @@ -109,12 +133,11 @@ namespace WalkieTalkie { } case RecvETX: { - result = RecvETXRoutine(payload, &byte); - break; + return RecvETXRoutine(command, payload, &byte); } } } - return result; + return 0; } }; diff --git a/src/communication/walkieTalkie.h b/src/communication/walkieTalkie.h index 9bbb5b1..0be0751 100644 --- a/src/communication/walkieTalkie.h +++ b/src/communication/walkieTalkie.h @@ -21,6 +21,10 @@ namespace WalkieTalkie { inline RecvState state = WaitingSTX; inline int16_t len = 0; inline uint8_t crc = 0; + inline Command tempCommand = CmdUnknown; + // Timing control + inline int64_t lastByteTime = 0; + inline int64_t SERIAL_TIMEOUT_MS = 15; inline static void resetBuffer() { bufferIndex = 0; diff --git a/src/main.cpp b/src/main.cpp index c2a6252..ac8e080 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -73,6 +73,7 @@ Curses::Screen mainScreen(gfx, UIDimensions(0, 0, TFT_HOR_RES, TFT_VER_RES), void setup() { psramInit(); + Serial.setRxBufferSize(8192); Serial.begin(115200); // Initialize the debug serial object @@ -126,7 +127,6 @@ void loop(void) { LOG_WARN(F("Failed to receive data from serial")); return; } else if (resp == 0) { - LOG_INFO(F("No data to receive\r\n")); return; } @@ -187,32 +187,51 @@ void loop(void) { case CMDData: { LOG_DEBUG(F("Data Received: %d bytes\r\n"), resp); - int pos = 0; - - for (int k = 0; k < resp; k++) { - // Write 2 hex digits and a space to our local buffer - // %02X ensures leading zeros (e.g., 0A instead of A) - pos += sprintf(lineBuffer + pos, "%02X ", payload[k]); - - // Every 8 bytes OR if it's the very last byte in the payload - if ((k + 1) % 8 == 0 || k == resp - 1) { - // Send the completed line to the logger - // We use LOG_DEBUG or similar so we don't spam [WARN] on every line - LOG_DEBUG(F("%s\r\n"), lineBuffer); - - // Reset buffer position for the next line - pos = 0; - memset(lineBuffer, 0, sizeof(lineBuffer)); - } - } - LOG_DEBUG(F("\r\n")); + // int pos = 0; + // for (int k = 0; k < resp; k++) { + // // Write 2 hex digits and a space to our local buffer + // // %02X ensures leading zeros (e.g., 0A instead of A) + // pos += sprintf(lineBuffer + pos, "%02X ", payload[k]); + // + // // Every 8 bytes OR if it's the very last byte in the payload + // if ((k + 1) % 8 == 0 || k == resp - 1) { + // // Send the completed line to the logger + // // We use LOG_DEBUG or similar so we don't spam [WARN] on every line + // LOG_DEBUG(F("%s\r\n"), lineBuffer); + // + // // Reset buffer position for the next line + // pos = 0; + // memset(lineBuffer, 0, sizeof(lineBuffer)); + // } + // } + // LOG_DEBUG(F("\r\n")); Data::Parse(payload, resp, &mainScreen); break; } default: - LOG_WARN(F("Unknown Command: `%d` has not been implemented yet."), cmd); + LOG_ERROR(F("Command: %s\n"), CommandToStr(cmd)); + LOG_ERROR(F("Unknown Command: `%02X` has not been implemented yet.\n"), cmd); + int pos = 0; + for (int k = 0; k < resp; k++) { + // Write 2 hex digits and a space to our local buffer + // %02X ensures leading zeros (e.g., 0A instead of A) + pos += sprintf(lineBuffer + pos, "%02X ", payload[k]); + + // Every 8 bytes OR if it's the very last byte in the payload + if ((k + 1) % 8 == 0 || k == resp - 1) { + // Send the completed line to the logger + // We use LOG_DEBUG or similar so we don't spam [WARN] on every line + LOG_WARN(F("%s\r\n"), lineBuffer); + + // Reset buffer position for the next line + pos = 0; + memset(lineBuffer, 0, sizeof(lineBuffer)); + } + } + LOG_WARN(F("\r\n")); + break; } } }