slight refactoring to the state machine

src/communication/walkieTalkie.cpp

@@ -7,95 +7,122 @@
 #include <Arduino.h>
 
 namespace WalkieTalkie {
-  // ahem - maybe refactor this inferno!
+  static inline void WaitingSTXRoutine(byte *b) {
-  size_t RecvData(Command *command, uint8_t *payload, uint16_t payloadMax) {
+    if (*b == STX) {
-    int16_t len = 0;
+      Serial2.println(F("First byte is STX"));
-    uint8_t crc = 0;
+      state = RecvCMD;
+      resetBuffer();
+    } else {
+      Serial2.print(F("First byte is not STX: "));
+      Serial2.println(*b);
+    }
+  }
+
+  static inline void RecvCMDRoutine(Command *command, byte *b) {
+    Serial2.println(F("Reading the command byte"));
+
+    *command = Command(*b);
+    Serial2.print("  Command: ");
+    Serial2.println(*b);
+
+    state = RecvLen;
+
+    resetBuffer();
+  }
+
+  static inline void RecvLenRoutine(byte *b) {
+    Serial2.println(F("Reading the len"));
+    buffer[bufferIndex++] = *b;
+    // we are currently using a int16_t for the len so: 2 bytes
+    if (bufferIndex > 1) {
+      len = buffer[0] | (buffer[1] << 8);
+      Serial2.print("Len: ");
+      Serial2.println(len);
+      state = RecvPayload;
+      resetBuffer();
+    }
+  }
+
+  static inline void RecvPayloadRoutine(uint8_t *payload, byte *b) {
+    Serial2.print("Reading payload: ");
+    Serial2.print(bufferIndex);
+    Serial2.print(" - ");
+    Serial2.println(*b);
+    buffer[bufferIndex++] = *b;
+    if (bufferIndex == len) {
+      // put the buffer data somewhere
+      memcpy(payload, buffer, len);
+      state = RecvCRC;
+      resetBuffer();
+    }
+  }
+
+  static inline int RecvCRCRoutine(byte *b) {
+    crc = *b;
+    Serial2.print("Reading CRC:");
+    Serial2.println(crc);
+
+    state = RecvETX;
+    resetBuffer();
+
+    return 0;
+  }
+
+  static inline size_t RecvETXRoutine(uint8_t *payload, byte *b) {
+    state = WaitingSTX;
+
+    Serial2.println("Reading ETX");
+    if (*b != ETX) {
+      Serial2.print("Failure, last byte was: ");
+      Serial2.println(*b);
+      return -1;
+    } else if (crc != CRC8(payload, len)) {
+      Serial2.print("CRC don't match. Expected: ");
+      Serial2.print(crc);
+      Serial2.print(", Got: ");
+      Serial2.println(CRC8(payload, len));
+
+      return -2;
+    } else {
+      return len;
+    }
+  }
+
+  int16_t RecvStream(Command *command, uint8_t *payload, uint16_t payloadMax) {
     int16_t result = 0;
 
     while (Serial.available() > 0) {
       uint8_t byte = Serial.read();
       switch(state) {
-        case WaitingSTX:
+        case WaitingSTX: {
-          if (byte == STX) {
+          WaitingSTXRoutine(&byte);
-            Serial2.println(F("First byte is STX"));
-            state = RecvCMD;
-            resetBuffer();
-          } else {
-            Serial2.print(F("First byte is not STX: "));
-            Serial2.println(byte);
-          }
           break;
+        }
 
-        case RecvCMD:
+        case RecvCMD: {
-          Serial2.println(F("Reading the command byte"));
+          RecvCMDRoutine(command, &byte);
-
-          *command = Command(byte);
-          Serial2.print("  Command: ");
-          Serial2.println(byte);
-
-          state = RecvLen;
-          resetBuffer();
           break;
+        }
 
-        case RecvLen:
+        case RecvLen: {
-          Serial2.println(F("Reading the len"));
+          RecvLenRoutine(&byte);
-          buffer[bufferIndex++] = byte;
-          // we are currently using a int16_t for the len so: 2 bytes
-          if (bufferIndex > 1) {
-            len = buffer[0] | (buffer[1] << 8);
-            Serial2.print("Len: ");
-            Serial2.println(len);
-            state = RecvPayload;
-            resetBuffer();
-          }
           break;
+        }
 
-        case RecvPayload:
+        case RecvPayload: {
-          Serial2.print("Reading payload: ");
+          RecvPayloadRoutine(payload, &byte);
-          Serial2.print(bufferIndex);
-          Serial2.print(" - ");
-          Serial2.println(byte);
-          buffer[bufferIndex++] = byte;
-          if (bufferIndex == len) {
-            // put the buffer data somewhere
-            memcpy(payload, buffer, len);
-            state = RecvCRC;
-            resetBuffer();
-          }
           break;
+        }
 
-        case RecvCRC:
+        case RecvCRC: {
-          crc = byte;
+          RecvCRCRoutine(&byte);
-          Serial2.print("Reading CRC:");
-          Serial2.println(crc);
-
-          if (crc != CRC8(payload, len)) {
-            Serial2.print("CRC don't match. Expected: ");
-            Serial2.print(crc);
-            Serial2.print(", Got: ");
-            Serial2.println(CRC8(payload, len));
-
-            return -1;
-            break;
-          }
-
-          state = RecvETX;
-          resetBuffer();
           break;
+        }
 
-        case RecvETX:
+        case RecvETX: {
-          Serial2.println("Reading ETX");
+          return RecvETXRoutine(payload, &byte);
-          if (byte != ETX) {
-            Serial2.print("Failure, last byte was: ");
-            Serial2.println(byte);
-            state = WaitingSTX;
-            return -1;
-          } else {
-            state = WaitingSTX;
-            return len;
-          }
           break;
+        }
       }
     }
 

src/communication/walkieTalkie.h

@@ -1,7 +1,6 @@
 #ifndef __WALKIE_TALKIE__
 #define __WALKIE_TALKIE__
 
-#include "communication/communication.h"
 #include "communication/commands.h"
 #include <cstdint>
 #include <Arduino.h>
@@ -20,6 +19,8 @@ namespace WalkieTalkie {
   inline uint8_t buffer[4096];
   inline int16_t bufferIndex = 0;
   inline RecvState state = WaitingSTX;
+  inline int16_t len = 0;
+  inline uint8_t crc = 0;
 
   inline static void resetBuffer() {
     bufferIndex = 0;
@@ -37,7 +38,7 @@ namespace WalkieTalkie {
     Serial.flush();
   }
 
-  size_t RecvData(Command *command, uint8_t *payload, uint16_t payloadMax);
+  int16_t RecvStream(Command *command, uint8_t *payload, uint16_t payloadMax);
 };
 
 #endif

src/main.cpp

@@ -192,7 +192,7 @@ void loop(void) {
   uint8_t payload[256] = {0};
 
   if (Serial.available() > 0) {
-    size_t resp = WalkieTalkie::RecvData(&cmd, payload, 256);
+    int16_t resp = WalkieTalkie::RecvStream(&cmd, payload, 256);
     if (resp < 0) {
       Serial2.println(F("Failed to receive data from serial"));
       return;