Reworking the communcation

2025-12-23 23:36:51 +00:00
parent ce15563aee
commit dc23bbaf5b
10 changed files with 409 additions and 140 deletions
@@ -0,0 +1,104 @@
+#include "walkieTalkie.h"
+#include "HardwareSerial.h"
+#include "communication/commands.h"
+#include "communication/communication.h"
+#include <cstdint>
+#include <stdint.h>
+#include <Arduino.h>
+
+namespace WalkieTalkie {
+  // ahem - maybe refactor this inferno!
+  size_t RecvData(Command *command, uint8_t *payload, uint16_t payloadMax) {
+    int16_t len = 0;
+    uint8_t crc = 0;
+    int16_t result = 0;
+
+    while (Serial.available() > 0) {
+      uint8_t byte = Serial.read();
+      switch(state) {
+        case WaitingSTX:
+          if (byte == STX) {
+            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:
+          Serial2.println(F("Reading the command byte"));
+
+          *command = Command(byte);
+          Serial2.print("  Command: ");
+          Serial2.println(byte);
+
+          state = RecvLen;
+          resetBuffer();
+          break;
+
+        case RecvLen:
+          Serial2.println(F("Reading the len"));
+          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:
+          Serial2.print("Reading payload: ");
+          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:
+          crc = 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:
+          Serial2.println("Reading ETX");
+          if (byte != ETX) {
+            Serial2.print("Failure, last byte was: ");
+            Serial2.println(byte);
+            state = WaitingSTX;
+            return -1;
+          } else {
+            state = WaitingSTX;
+            return len;
+          }
+          break;
+      }
+    }
+
+    return 0;
+  }
+};