From e9df3c13ae9eeb6bd034fcdb244bd9b8ad3127eb Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Jurij=20Podgor=C5=A1ek?= <jurij@kompot.si>
Date: Thu, 26 Sep 2024 15:30:24 +0200
Subject: [PATCH] Sprejemnik2 [test]

---
 src/sprejemnik2.cpp | 169 ++++++++++++++++++++++++++++++++++++++++++++
 1 file changed, 169 insertions(+)
 create mode 100644 src/sprejemnik2.cpp

diff --git a/src/sprejemnik2.cpp b/src/sprejemnik2.cpp
new file mode 100644
index 0000000..e22031e
--- /dev/null
+++ b/src/sprejemnik2.cpp
@@ -0,0 +1,169 @@
+#include <Arduino.h>
+#include <esp_now.h>
+#include <esp_wifi.h>
+#include <WiFi.h>
+
+// WIFI paket
+#include "sensor_msg.h"
+
+#include <OSCBundle.h>
+#include <OSCBoards.h>
+
+//#define DEBUG
+
+// LED indicator
+#define LED_PIN 2
+
+#include "SLIPEncodedSerial.h"
+SLIPEncodedUSBSerial SLIPSerial(Serial);
+ 
+// Set your new MAC Address
+// MAC naslov sprejemnika: 08:3A:F2:50:EF:6C
+//uint8_t newMACAddress[] = {0x08, 0x3A, 0xF2, 0x50, 0xEF, 0x6C};
+// MAC naslov sprejemnika2: 08:3A:F2:50:EF:6D
+uint8_t newMACAddress[] = {0x08, 0x3A, 0xF2, 0x50, 0xEF, 0x6D};
+
+// MAC naslov laptopa (linux sprejemnik)
+//uint8_t newMACAddress[] = { 0x9c, 0xb6, 0xd0, 0xc4, 0xe8, 0xb9 };
+
+// Maksimalno stevilo
+#define ST_SPREJEMNIKOV 10
+
+int odcitekId;
+sensor_msg  odcitki[ST_SPREJEMNIKOV];
+bool poslji[ST_SPREJEMNIKOV];
+
+void prejemPodatkov(const uint8_t * mac_addr, const uint8_t * noviPodatki, int len) {
+  
+  char macNaslov[18];
+  snprintf(macNaslov, sizeof(macNaslov), "%02x:%02x:%02x:%02x:%02x:%02x",
+           mac_addr[0], mac_addr[1], mac_addr[2], mac_addr[3], mac_addr[4], mac_addr[5]);
+  
+  #ifdef DEBUG
+  Serial.print("Prejel podatke od ");
+  Serial.println(macNaslov);
+  #endif
+
+  // ID odcitka je na prvem mestu!
+  memcpy(&odcitekId, noviPodatki, sizeof(uint8_t));
+  
+  #ifdef DEBUG
+  Serial.print("RAZBIRAM ID ");
+  Serial.println(odcitekId);
+  #endif
+  
+  memcpy(&odcitki[odcitekId], noviPodatki, sizeof(sensor_msg));
+
+  #ifdef DEBUG
+  Serial.printf("aX: %f \n", odcitki[odcitekId].aX);
+  Serial.printf("aY: %f \n", odcitki[odcitekId].aY);
+  Serial.printf("aZ: %f \n", odcitki[odcitekId].aZ);
+  Serial.printf("qX: %f \n", odcitki[odcitekId].qX);
+  Serial.printf("qY: %f \n", odcitki[odcitekId].qY);
+  Serial.printf("qZ: %f \n", odcitki[odcitekId].qZ);
+  Serial.printf("qW: %f \n", odcitki[odcitekId].qW);
+  Serial.println();
+  #endif  
+
+  poslji[odcitekId] = true;
+}
+
+void setup() {
+  // Nizja CPU frekvenca
+  //setCpuFrequencyMhz(160);
+  //SLIPSerial.begin(115200);
+  SLIPSerial.begin(230400);
+
+
+  // Ne posiljaj preden se podatki napolnijo
+  for (int i = 0; i < ST_SPREJEMNIKOV; i++) {
+    poslji[i] = false;
+  }
+
+  // Init - 3 one second blinks
+  pinMode(LED_PIN, OUTPUT);
+  for (int i = 0; i < 3; i++) {
+    Serial.println(i + 1);
+    digitalWrite(LED_PIN, LOW);
+    delay(500);
+    digitalWrite(LED_PIN, HIGH);
+    delay(500);
+  }
+
+  // ESP32S3 - vklopi LED!
+  digitalWrite(LED_PIN, HIGH);
+
+  Serial.println("Inicializiram WIFI...");
+  WiFi.mode(WIFI_STA);
+
+  esp_wifi_set_mac(WIFI_IF_STA, &newMACAddress[0]);
+  
+  esp_err_t result = esp_now_init();
+  if (result != ESP_OK) {
+    Serial.println("Error initializing ESP-NOW");
+    Serial.println(result);
+    return;
+  }
+
+  // Modem sleep off
+  esp_wifi_set_ps(WIFI_PS_NONE);
+
+  Serial.print("MAC naslov: ");
+  Serial.println(WiFi.macAddress());
+  esp_now_register_recv_cb(prejemPodatkov);
+}
+
+void loop() {
+  /* OSC MSG channels */
+  OSCBundle bundle;
+  char glava[32];
+
+  for (int i = 0; i < ST_SPREJEMNIKOV; i++) {
+    if (poslji[i]) {
+      // Accel in quaternion v bundlu skupaj z ostalim sta velika 396 bytov!
+      sprintf(glava, "/ww/%d/acc", i); 
+      /*
+      Serial.print("Posiljam ");
+      Serial.println(glava);
+      */
+      bundle.add(glava)
+        .add(odcitki[i].aX)
+        .add(odcitki[i].aY)
+        .add(odcitki[i].aZ);
+
+      sprintf(glava, "/ww/%d/quat", i);
+      /*
+      Serial.print("Posiljam ");
+      Serial.println(glava);
+      */
+      bundle.add(glava)
+        .add(odcitki[i].qW)
+        .add(odcitki[i].qX)
+        .add(odcitki[i].qY)
+        .add(odcitki[i].qZ);
+
+      /*
+      Serial.printf("XaX: %i \n", odcitki[i].aX);
+      Serial.printf("XaY: %i \n", odcitki[i].aY);
+      Serial.printf("XaZ: %i \n", odcitki[i].aZ);
+      Serial.printf("XqX: %f \n", odcitki[i].qX);
+      Serial.printf("XqY: %f \n", odcitki[i].qY);
+      Serial.printf("XqZ: %f \n", odcitki[i].qZ);
+      Serial.printf("XqW: %f \n", odcitki[i].qW);
+      Serial.println();
+      */
+
+      sprintf(glava, "/ww/%d/bat", i);
+      bundle.add(glava)
+        .add(odcitki[i].bat);
+
+      SLIPSerial.beginPacket();
+      bundle.send(SLIPSerial);
+      SLIPSerial.endPacket();
+      
+      bundle.empty();
+      poslji[i] = false;
+    }
+  }
+  //delay(10);
+}