Added ESP-IDF Wifi/UDP support (but it still give linker errors)

EspIdf/EspIdfParticipant.cpp

@@ -0,0 +1,161 @@
+#include "EspIdfParticipant.h"
+
+namespace RoboidControl {
+namespace EspIdf {
+
+// #include <stdio.h>
+// #include <string.h>
+#include "esp_log.h"
+// #include "esp_system.h"
+#include "esp_netif.h"
+// #include "esp_event_loop.h"
+#include "esp_wifi.h"
+#include "lwip/sockets.h"
+// #include "lwip/err.h"
+// #include "lwip/sys.h"
+// #include "lwip/ip_addr.h"
+// #include "lwip/inet.h"
+
+void LocalParticipant::Setup(int localPort,
+                             const char* remoteIpAddress,
+                             int remotePort) {
+#if defined(IDF_VER)
+  this->remoteIpAddress = remoteIpAddress;
+  this->remotePort = remotePort;
+  GetBroadcastAddress();
+
+  wifi_ap_record_t ap_info;
+  esp_err_t result = esp_wifi_sta_get_ap_info(&ap_info);
+  if (result != ESP_OK) {
+    std::cout << "No network available!\n";
+    return;
+  }
+
+  struct sockaddr_in server_addr, client_addr;
+  socklen_t addr_len = sizeof(client_addr);
+  char recv_buffer[1024];
+  //   int sockfd;
+
+  // Create a UDP socket
+  sockfd = socket(AF_INET, SOCK_DGRAM, 0);
+  if (sockfd < 0) {
+    std::cout << "Unable to create UDP socket: errno " << errno << "\n";
+    vTaskDelete(NULL);
+    return;
+  }
+
+  // Set socket to non-blocking mode
+  int flags = fcntl(sockfd, F_GETFL, 0);
+  if (flags < 0) {
+    std::cout << "fcntl failed";
+    close(sockfd);
+    return;
+  }
+  fcntl(sockfd, F_SETFL, flags | O_NONBLOCK);
+
+  // Set up the server address structure
+  memset(&server_addr, 0, sizeof(server_addr));
+  server_addr.sin_family = AF_INET;
+  server_addr.sin_port = htons(localPort);
+  server_addr.sin_addr.s_addr =
+      htonl(INADDR_ANY);  // Listen on all available network interfaces
+
+  // Bind the socket to the address and port
+  if (bind(sockfd, (struct sockaddr*)&server_addr, sizeof(server_addr)) < 0) {
+    std::cout << "Unable to bind UDP socket: errno " << errno << "\n";
+    close(sockfd);
+    vTaskDelete(NULL);
+    return;
+  }
+
+  std::cout << "Wifi sync started to port " << this->remotePort << "\n";
+#endif
+}
+
+void LocalParticipant::GetBroadcastAddress() {
+  esp_netif_ip_info_t ip_info;
+  esp_netif_t* esp_netif = esp_netif_get_handle_from_ifkey("WIFI_STA_DEF");
+
+  // Get IP information (IP address, netmask, gateway)
+  if (esp_netif_get_ip_info(esp_netif, &ip_info) != ESP_OK) {
+    std::cout << "Failed to get IP info\n";
+    return;
+  }
+
+  ip_addr_t broadcast_addr;
+  broadcast_addr.u_addr.ip4.addr =
+      (ip_info.ip.addr & ip_info.netmask.addr) | ~ip_info.netmask.addr;
+
+  std::cout << "Broadcast address: " << broadcastIpAddress << "\n";
+}
+
+void LocalParticipant::Receive() {
+#if defined(IDF_VER)
+  struct sockaddr_in client_addr;
+  socklen_t addr_len = sizeof(client_addr);
+  int packetSize = recvfrom(sockfd, buffer, sizeof(buffer) - 1, 0,
+                            (struct sockaddr*)&client_addr, &addr_len);
+  while (packetSize > 0) {
+
+    char sender_ipAddress[16];
+    inet_ntoa_r(client_addr.sin_addr, sender_ipAddress, INET_ADDRSTRLEN);
+    unsigned int sender_port = client_addr.sin_port;
+
+    // Participant* remoteParticipant = this->GetParticipant(sender_ipAddress,
+    // sender_port); if (remoteParticipant == nullptr) {
+    //   remoteParticipant = this->AddParticipant(sender_ipAddress,
+    //   sender_port);
+    //   // std::cout << "New sender " << sender_ipAddress << ":" << sender_port
+    //   //           << "\n";
+    //   // std::cout << "New remote participant " <<
+    //   remoteParticipant->ipAddress
+    //   //           << ":" << remoteParticipant->port << " "
+    //   //           << (int)remoteParticipant->networkId << "\n";
+    // }
+
+    // ReceiveData(packetSize, remoteParticipant);
+    ReceiveData(packetSize, sender_ipAddress, sender_port);
+    packetSize = recvfrom(sockfd, buffer, sizeof(buffer) - 1, 0,
+                            (struct sockaddr*)&client_addr, &addr_len);
+  }
+#endif
+}
+
+bool LocalParticipant::Send(Participant* remoteParticipant, int bufferSize) {
+#if defined(IDF_VER)
+  // std::cout << "Sending to:\n " << remoteParticipant->ipAddress << ":"
+  // << remoteParticipant->port << "\n";
+
+  struct sockaddr_in dest_addr;
+    dest_addr.sin_family = AF_INET;
+    dest_addr.sin_port = htons(remoteParticipant->port);
+    inet_pton(AF_INET, remoteParticipant->ipAddress, &dest_addr.sin_addr.s_addr);
+  int err = sendto(sockfd, buffer, bufferSize, 0, (struct sockaddr *)&dest_addr, sizeof(dest_addr));
+
+#endif
+  return true;
+}
+
+bool LocalParticipant::Publish(IMessage* msg) {
+#if defined(IDF_VER)
+  int bufferSize = msg->Serialize((char*)this->buffer);
+  if (bufferSize <= 0)
+    return true;
+
+    struct sockaddr_in dest_addr;
+    dest_addr.sin_family = AF_INET;
+    dest_addr.sin_port = htons(this->remotePort);
+    inet_pton(AF_INET, this->broadcastIpAddress, &dest_addr.sin_addr.s_addr);
+  int err = sendto(sockfd, buffer, bufferSize, 0, (struct sockaddr *)&dest_addr, sizeof(dest_addr));
+//   udp.beginPacket(this->broadcastIpAddress, this->remotePort);
+//   udp.write((unsigned char*)buffer, bufferSize);
+//   udp.endPacket();
+
+  //   std::cout << "Publish to " << this->broadcastIpAddress << ":"
+  //             << this->remotePort << "\n";
+#endif
+  return true;
+};
+
+}  // namespace EspIdf
+}  // namespace RoboidControl

EspIdf/EspIdfParticipant.h

@@ -0,0 +1,30 @@
+#pragma once
+
+#include "../LocalParticipant.h"
+
+// #if defined(HAS_WIFI)
+// #include <WiFiUdp.h>
+// #endif
+
+namespace RoboidControl {
+namespace EspIdf {
+
+class LocalParticipant : public RoboidControl::LocalParticipant {
+ public:
+  void Setup(int localPort, const char* remoteIpAddress, int remotePort);
+  void Receive();
+  bool Send(Participant* remoteParticipant, int bufferSize);
+  bool Publish(IMessage* msg);
+
+ protected:
+  const char* remoteIpAddress = nullptr;
+  unsigned short remotePort = 0;
+  char* broadcastIpAddress = nullptr;
+
+  int sockfd;
+
+  void GetBroadcastAddress();
+};
+
+}  // namespace EspIdf
+}  // namespace RoboidControl

EspIdf/EspIdfUtils.cpp

@@ -0,0 +1,90 @@
+#include "EspIdfUtils.h"
+
+#if defined(IDF_VER)
+#include <iostream>
+#include "esp_event.h"
+#include "esp_netif.h"
+#include "esp_wifi.h"
+#include "string.h"
+
+const char* hotspotSSID = "Roboid";
+const char* hotspotPassword = "alchemy7000";
+
+esp_netif_t* wifi_netif = nullptr;
+// Semaphore to signal Wi-Fi connection status
+SemaphoreHandle_t wifi_semaphore;
+static bool wifi_connected = false;
+
+static void wifi_event_handler(void* arg,
+                               esp_event_base_t event_base,
+                               int32_t event_id,
+                               void* event_data) {
+  if (event_base == WIFI_EVENT) {
+    if (event_id == WIFI_EVENT_STA_START)
+      esp_wifi_connect();
+    else if (event_id == WIFI_EVENT_STA_DISCONNECTED)
+      esp_wifi_connect();
+  } else if (event_base == IP_EVENT) {
+    if (event_id == IP_EVENT_STA_GOT_IP) {
+      ip_event_got_ip_t* event = (ip_event_got_ip_t*)event_data;
+      std::cout << "Wifi connected, IP address " << (IP2STR(&event->ip_info.ip))
+                << "\n";
+      wifi_connected = true;
+      xSemaphoreGive(wifi_semaphore);  // Signal that connection is established
+    }
+  }
+}
+
+// void log_dots_task(void* pvParameters) {
+//   while (!wifi_connected) {
+//     printf(".");                           // Log a dot
+//     vTaskDelay(500 / portTICK_PERIOD_MS);  // Wait 500ms
+//   }
+//   printf("\nWi-Fi connected!\n");
+//   vTaskDelete(NULL);
+// }
+
+bool StartWifi(const char* wifiSsid, const char* wifiPassword) {
+  std::cout << "Connecting to WiFi" << wifiSsid << "\n";
+
+  esp_netif_init();
+  esp_event_loop_create_default();
+
+  wifi_netif = esp_netif_create_default_wifi_sta();
+  wifi_init_config_t cfg = WIFI_INIT_CONFIG_DEFAULT();
+  esp_wifi_init(&cfg);
+
+  esp_event_handler_register(WIFI_EVENT, ESP_EVENT_ANY_ID, &wifi_event_handler,
+                             NULL);
+  esp_event_handler_register(IP_EVENT, IP_EVENT_STA_GOT_IP, &wifi_event_handler,
+                             NULL);
+
+  wifi_config_t wifi_config = {.sta = {.bssid_set = false}};
+  // Copy the ssid in the config
+  strncpy((char*)wifi_config.sta.ssid, wifiSsid, sizeof(wifiSsid));
+  // Copy the wifiPassword in the config
+  strncpy((char*)wifi_config.sta.password, wifiPassword, sizeof(wifiPassword));
+
+  esp_wifi_set_mode(WIFI_MODE_STA);
+  esp_wifi_set_config(WIFI_IF_STA, &wifi_config);
+  esp_wifi_start();
+
+  // Wait for connection with a timeout of 10 seconds
+  TickType_t xLastWakeTime = xTaskGetTickCount();
+  bool success = false;
+  for (int i = 0; i < 20; i++) {  // 20 iterations, each 500ms
+    if (wifi_connected) {
+      success = true;
+      break;
+    }
+    printf(".");                                          // Log a dot
+    vTaskDelayUntil(&xLastWakeTime, pdMS_TO_TICKS(500));  // Wait 500ms
+  }
+
+  if (!wifi_connected)
+    std::cout << "\nCould not connect to home network.\n";  
+
+  return success;
+}
+
+#endif

EspIdf/EspIdfUtils.h

@@ -0,0 +1,7 @@
+#pragma once
+#if defined(IDF_VER)
+
+bool StartWifi(const char *wifiSsid, const char *wifiPassword,
+               bool hotspotFallback = false);
+
+#endif

LinearAlgebra/Angle.h

@@ -21,7 +21,7 @@ template <typename T>
 class AngleOf {
  public:
   /// @brief Create a new angle with a zero value
-  AngleOf<T>();
+  AngleOf();
 
   /// @brief An zero value angle
   const static AngleOf<T> zero;

LinearAlgebra/Matrix.cpp

@@ -3,7 +3,7 @@
 
 #pragma region Matrix1
 
-LinearAlgebra::Matrix1::Matrix1(int size) : size(size) {
+Matrix1::Matrix1(int size) : size(size) {
   if (this->size == 0)
     data = nullptr;
   else {
@@ -12,7 +12,7 @@ LinearAlgebra::Matrix1::Matrix1(int size) : size(size) {
   }
 }
 
-LinearAlgebra::Matrix1::Matrix1(float* data, int size)
+Matrix1::Matrix1(float* data, int size)
     : data(data), size(size) {
   this->externalData = true;
 }

LinearAlgebra/Matrix.h

@@ -9,8 +9,8 @@ namespace LinearAlgebra {
 /// @brief A 1-dimensional matrix or vector of arbitrary size
 class Matrix1 {
  public:
-  int size = 0;
   float* data = nullptr;
+  int size = 0;
 
   Matrix1(int size);
   Matrix1(float* data, int size);
@@ -74,12 +74,12 @@ class Matrix2 {
     }
     return r;
   }
-  friend Matrix2 operator*(float f, const Matrix2& v) {
+  // friend Matrix2 operator*(float f, const Matrix2& v) {
-    Matrix2 r = Matrix2(v.nRows, v.nCols);
+  //   Matrix2 r = Matrix2(v.nRows, v.nCols);
-    for (int ix = 0; ix < r.nValues; ix++)
+  //   for (int ix = 0; ix < r.nValues; ix++)
-      r.data[ix] = f * v.data[ix];
+  //     r.data[ix] = f * v.data[ix];
-    return r;
+  //   return r;
-  }
+  // }
 
   void SetSlice(int rowStart,
                 int rowStop,
@@ -208,6 +208,6 @@ class MatrixOf {
 };
 
 }  // namespace LinearAlgebra
-using namespace LinearAlgebra;
+//using namespace LinearAlgebra;
 
 #endif

LocalParticipant.cpp

@@ -3,6 +3,7 @@
 #include "Thing.h"
 
 #include "Arduino/ArduinoParticipant.h"
+#include "EspIdf/EspIdfParticipant.h"
 
 #if defined(_WIN32) || defined(_WIN64)
 #include <winsock2.h>
@@ -33,10 +34,10 @@ LocalParticipant::LocalParticipant(int port) {
     this->isIsolated = true;
 }
 
-LocalParticipant::LocalParticipant(const char* ipAddress, int port) {
+LocalParticipant::LocalParticipant(const char* ipAddress,
-  this->ipAddress = "0.0.0.0";  // ipAddress;  // maybe this is not needed
+                                   int port,
-                                // anymore, keeping it to "0.0.0.0"
+                                   int localPort)
-  this->port = port;
+    : Participant("127.0.0.1", localPort) {
   if (this->port == 0)
     this->isIsolated = true;
   else
@@ -73,22 +74,17 @@ void LocalParticipant::SetupUDP(int localPort,
   Arduino::LocalParticipant* thisArduino =
       static_cast<Arduino::LocalParticipant*>(this);
   thisArduino->Setup(localPort, remoteIpAddress, remotePort);
+#elif defined(IDF_VER)
+  EspIdf::LocalParticipant* thisEspIdf =
+      static_cast<EspIdf::LocalParticipant*>(this);
+  thisEspIdf->Setup(localPort, remoteIpAddress, remotePort);
 #endif
   this->connected = true;
 }
 
 void LocalParticipant::Update(unsigned long currentTimeMs) {
-  if (currentTimeMs == 0) {
+  if (currentTimeMs == 0)
     currentTimeMs = Thing::GetTimeMs();
-    // #if defined(ARDUINO)
-    //     currentTimeMs = millis();
-    // #elif defined(__unix__) || defined(__APPLE__)
-    //     auto now = std::chrono::steady_clock::now();
-    //     auto ms =
-    //     std::chrono::duration_cast<std::chrono::milliseconds>(now.time_since_epoch());
-    //     currentTimeMs = static_cast<unsigned long>(ms.count());
-    // #endif
-  }
 
   if (this->isIsolated == false) {
     if (this->connected == false)
@@ -133,6 +129,10 @@ void LocalParticipant::ReceiveUDP() {
   Arduino::LocalParticipant* thisArduino =
       static_cast<Arduino::LocalParticipant*>(this);
   thisArduino->Receive();
+#elif defined(IDF_VER)
+  EspIdf::LocalParticipant* thisEspIdf =
+      static_cast<EspIdf::LocalParticipant*>(this);
+  thisEspIdf->Receive();
 #endif
 }
 
@@ -196,6 +196,10 @@ bool LocalParticipant::Send(Participant* remoteParticipant, IMessage* msg) {
   Arduino::LocalParticipant* thisArduino =
       static_cast<Arduino::LocalParticipant*>(this);
   return thisArduino->Send(remoteParticipant, bufferSize);
+#elif defined(IDF_VER)
+  EspIdf::LocalParticipant* thisEspIdf =
+      static_cast<EspIdf::LocalParticipant*>(this);
+  return thisEspIdf->Send(remoteParticipant, bufferSize);
 #else
   return false;
 #endif
@@ -235,6 +239,10 @@ bool LocalParticipant::Publish(IMessage* msg) {
   Arduino::LocalParticipant* thisArduino =
       static_cast<Arduino::LocalParticipant*>(this);
   return thisArduino->Publish(msg);
+#elif defined(IDF_VER)
+  EspIdf::LocalParticipant* thisEspIdf =
+      static_cast<EspIdf::LocalParticipant*>(this);
+  return thisEspIdf->Publish(msg);
 #else
   return false;
 #endif

LocalParticipant.h

@@ -51,7 +51,7 @@ class LocalParticipant : public Participant {
   /// @brief Create a participant which will try to connect to a site.
   /// @param ipAddress The IP address of the site
   /// @param port The port used by the site
-  LocalParticipant(const char* ipAddress, int port = 7681);
+  LocalParticipant(const char* ipAddress, int port = 7681, int localPort = 7681);
   // Note to self: one cannot specify the port used by the local participant
   // now!!