From 42d3600cd8ebeaca8348d291615d70a797b86297 Mon Sep 17 00:00:00 2001 From: Pascal Serrarens Date: Wed, 2 Apr 2025 12:45:59 +0200 Subject: [PATCH] Added ESP-IDF Wifi/UDP support (but it still give linker errors) --- EspIdf/EspIdfParticipant.cpp | 161 +++++++++++++++++++++++++++++++++++ EspIdf/EspIdfParticipant.h | 30 +++++++ EspIdf/EspIdfUtils.cpp | 90 ++++++++++++++++++++ EspIdf/EspIdfUtils.h | 7 ++ LinearAlgebra/Angle.h | 2 +- LinearAlgebra/Matrix.cpp | 4 +- LinearAlgebra/Matrix.h | 16 ++-- LocalParticipant.cpp | 38 +++++---- LocalParticipant.h | 2 +- 9 files changed, 323 insertions(+), 27 deletions(-) create mode 100644 EspIdf/EspIdfParticipant.cpp create mode 100644 EspIdf/EspIdfParticipant.h create mode 100644 EspIdf/EspIdfUtils.cpp create mode 100644 EspIdf/EspIdfUtils.h diff --git a/EspIdf/EspIdfParticipant.cpp b/EspIdf/EspIdfParticipant.cpp new file mode 100644 index 0000000..ef5c7f7 --- /dev/null +++ b/EspIdf/EspIdfParticipant.cpp @@ -0,0 +1,161 @@ +#include "EspIdfParticipant.h" + +namespace RoboidControl { +namespace EspIdf { + +// #include +// #include +#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 diff --git a/EspIdf/EspIdfParticipant.h b/EspIdf/EspIdfParticipant.h new file mode 100644 index 0000000..ec0d147 --- /dev/null +++ b/EspIdf/EspIdfParticipant.h @@ -0,0 +1,30 @@ +#pragma once + +#include "../LocalParticipant.h" + +// #if defined(HAS_WIFI) +// #include +// #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 diff --git a/EspIdf/EspIdfUtils.cpp b/EspIdf/EspIdfUtils.cpp new file mode 100644 index 0000000..2954e49 --- /dev/null +++ b/EspIdf/EspIdfUtils.cpp @@ -0,0 +1,90 @@ +#include "EspIdfUtils.h" + +#if defined(IDF_VER) +#include +#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 \ No newline at end of file diff --git a/EspIdf/EspIdfUtils.h b/EspIdf/EspIdfUtils.h new file mode 100644 index 0000000..c57f8f4 --- /dev/null +++ b/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 \ No newline at end of file diff --git a/LinearAlgebra/Angle.h b/LinearAlgebra/Angle.h index 1e47726..4e23f9c 100644 --- a/LinearAlgebra/Angle.h +++ b/LinearAlgebra/Angle.h @@ -21,7 +21,7 @@ template class AngleOf { public: /// @brief Create a new angle with a zero value - AngleOf(); + AngleOf(); /// @brief An zero value angle const static AngleOf zero; diff --git a/LinearAlgebra/Matrix.cpp b/LinearAlgebra/Matrix.cpp index c1f60a5..fca4360 100644 --- a/LinearAlgebra/Matrix.cpp +++ b/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; } diff --git a/LinearAlgebra/Matrix.h b/LinearAlgebra/Matrix.h index f66eeeb..a269bee 100644 --- a/LinearAlgebra/Matrix.h +++ b/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) { - Matrix2 r = Matrix2(v.nRows, v.nCols); - for (int ix = 0; ix < r.nValues; ix++) - r.data[ix] = f * v.data[ix]; - return r; - } + // friend Matrix2 operator*(float f, const Matrix2& v) { + // Matrix2 r = Matrix2(v.nRows, v.nCols); + // for (int ix = 0; ix < r.nValues; ix++) + // r.data[ix] = f * v.data[ix]; + // return r; + // } void SetSlice(int rowStart, int rowStop, @@ -208,6 +208,6 @@ class MatrixOf { }; } // namespace LinearAlgebra -using namespace LinearAlgebra; +//using namespace LinearAlgebra; #endif \ No newline at end of file diff --git a/LocalParticipant.cpp b/LocalParticipant.cpp index 5a968c7..5ebf3a6 100644 --- a/LocalParticipant.cpp +++ b/LocalParticipant.cpp @@ -3,6 +3,7 @@ #include "Thing.h" #include "Arduino/ArduinoParticipant.h" +#include "EspIdf/EspIdfParticipant.h" #if defined(_WIN32) || defined(_WIN64) #include @@ -33,10 +34,10 @@ LocalParticipant::LocalParticipant(int port) { this->isIsolated = true; } -LocalParticipant::LocalParticipant(const char* ipAddress, int port) { - this->ipAddress = "0.0.0.0"; // ipAddress; // maybe this is not needed - // anymore, keeping it to "0.0.0.0" - this->port = port; +LocalParticipant::LocalParticipant(const char* ipAddress, + int port, + int localPort) + : 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(this); thisArduino->Setup(localPort, remoteIpAddress, remotePort); +#elif defined(IDF_VER) + EspIdf::LocalParticipant* thisEspIdf = + static_cast(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(now.time_since_epoch()); - // currentTimeMs = static_cast(ms.count()); - // #endif - } if (this->isIsolated == false) { if (this->connected == false) @@ -133,6 +129,10 @@ void LocalParticipant::ReceiveUDP() { Arduino::LocalParticipant* thisArduino = static_cast(this); thisArduino->Receive(); +#elif defined(IDF_VER) + EspIdf::LocalParticipant* thisEspIdf = + static_cast(this); + thisEspIdf->Receive(); #endif } @@ -196,8 +196,12 @@ bool LocalParticipant::Send(Participant* remoteParticipant, IMessage* msg) { Arduino::LocalParticipant* thisArduino = static_cast(this); return thisArduino->Send(remoteParticipant, bufferSize); +#elif defined(IDF_VER) + EspIdf::LocalParticipant* thisEspIdf = + static_cast(this); + return thisEspIdf->Send(remoteParticipant, bufferSize); #else - return false; + return false; #endif } @@ -235,6 +239,10 @@ bool LocalParticipant::Publish(IMessage* msg) { Arduino::LocalParticipant* thisArduino = static_cast(this); return thisArduino->Publish(msg); +#elif defined(IDF_VER) + EspIdf::LocalParticipant* thisEspIdf = + static_cast(this); + return thisEspIdf->Publish(msg); #else return false; #endif diff --git a/LocalParticipant.h b/LocalParticipant.h index 07c0ca2..76742a7 100644 --- a/LocalParticipant.h +++ b/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!!