diff --git a/components/constants/include/constants/udp.h b/components/constants/include/constants/udp.h new file mode 100644 index 0000000..becd171 --- /dev/null +++ b/components/constants/include/constants/udp.h @@ -0,0 +1,18 @@ +// +// Created by Johnathon Slightham on 2025-12-21. +// + +#ifndef UDP_H +#define UDP_H + +#define SLEEP_AFTER_FAIL_MS 5000 + +#define NO_CLIENT_SLEEP_MS 400 + +#define RECV_MCAST "239.1.1.1" +#define RECV_PORT 3101 + +#define SEND_MCAST "239.1.1.2" +#define SEND_PORT 3100 + +#endif //UDP_H diff --git a/components/dataLink/DataLinkRIP.cpp b/components/dataLink/DataLinkRIP.cpp index 51d5d5d..adb8bfe 100644 --- a/components/dataLink/DataLinkRIP.cpp +++ b/components/dataLink/DataLinkRIP.cpp @@ -4,7 +4,7 @@ /** * @brief Initializes the RIP table - * + * */ void DataLinkManager::init_rip(){ for (size_t i = 0; i < RIP_MAX_ROUTES; i++){ @@ -54,11 +54,11 @@ esp_err_t DataLinkManager::rip_add_entry(uint8_t board_id, uint8_t hops, uint8_t (*entry)->ttl = RIP_TTL_START; (*entry)->valid = 1; - + // ESP_LOGI(DEBUG_LINK_TAG, "board_id %d now has hops %d from channel %d", (*entry)->info.board_id, (*entry)->info.hops, channel); - + xSemaphoreGive((*entry)->row_sem); - + if (uxQueueMessagesWaiting(manual_broadcasts) == 0){ bool dummy = true; xQueueSend(manual_broadcasts, &dummy, 0); //new row - send broadcast @@ -108,9 +108,9 @@ esp_err_t DataLinkManager::rip_update_entry(uint8_t new_hop, uint8_t channel, RI (*entry)->channel = channel; // ESP_LOGI(DEBUG_LINK_TAG, "updated board_id %d now has hops %d from channel %d", (*entry)->info.board_id, (*entry)->info.hops, channel); } - + (*entry)->ttl = RIP_TTL_START; - (*entry)->valid = 1; + (*entry)->valid = 1; // ESP_LOGI(DEBUG_LINK_TAG, "refreshed board_id %d ttl", (*entry)->info.board_id); @@ -128,10 +128,10 @@ esp_err_t DataLinkManager::rip_update_entry(uint8_t new_hop, uint8_t channel, RI /** * @brief Finds the board_id in the table if it exists and stores that row in `entry` * TODO: use an unordered map instead of an array? - * - * @param board_id - * @param entry - * @return esp_err_t + * + * @param board_id + * @param entry + * @return esp_err_t */ esp_err_t DataLinkManager::rip_find_entry(uint8_t board_id, RIPRow** entry, bool reserve_row = false){ RIPRow* free_slot = nullptr; @@ -144,7 +144,7 @@ esp_err_t DataLinkManager::rip_find_entry(uint8_t board_id, RIPRow** entry, bool xSemaphoreGive(rip_table[i].row_sem); // ESP_LOGI(DEBUG_LINK_TAG, "Found %d in table at row %d", board_id, i); return ESP_OK; - } + } if (rip_table[i].valid == RIP_INVALID_ROW && free_slot == nullptr){ free_slot = &rip_table[i]; } @@ -176,10 +176,10 @@ esp_err_t DataLinkManager::rip_find_entry(uint8_t board_id, RIPRow** entry, bool /** * @brief Returns the associated RIP Table row by row number. Information returned is read only. - * - * @param entry - * @param row_num - * @return esp_err_t + * + * @param entry + * @param row_num + * @return esp_err_t */ esp_err_t DataLinkManager::rip_get_row(RIPRow** entry, uint8_t row_num){ if (entry == nullptr){ @@ -216,10 +216,10 @@ esp_err_t DataLinkManager::rip_get_row(RIPRow** entry, uint8_t row_num){ /** * @brief Sends RIP frame - * + * * @param broadcast True - broadcasts (sends rip table to all available channels); False - sends rip table via routing based on `dest_id` * @param dest_id Destination board (requesting board) to send the rip table to (ignored if `broadcast is true`) - * @return esp_err_t + * @return esp_err_t */ esp_err_t DataLinkManager::send_rip_frame(bool broadcast, uint8_t dest_id){ //use the control frame for the demo (as the number of rows increase, we will need to use the generic frame) @@ -236,17 +236,17 @@ esp_err_t DataLinkManager::send_rip_frame(bool broadcast, uint8_t dest_id){ for (size_t channel = 0; channel < num_channels; channel++){ for (size_t i = 0; i < RIP_MAX_ROUTES; i++){ res = rip_get_row(&entry, i); - + if (res != ESP_OK){ continue; } - + if (entry == nullptr){ continue; } // ESP_LOGI(DEBUG_LINK_TAG, "Found entry for board %d with hops %d", entry->info.board_id, entry->info.hops); - + if (entry->channel == channel){ //poisoned reverse rip_message[message_idx++] = entry->info.board_id; @@ -264,7 +264,7 @@ esp_err_t DataLinkManager::send_rip_frame(bool broadcast, uint8_t dest_id){ } memset(send_data, 0, message_idx); memcpy(send_data, rip_message, message_idx); - + SchedulerMetadata metadata = { .header = { .preamble = START_OF_FRAME, @@ -314,10 +314,10 @@ esp_err_t DataLinkManager::send_rip_frame(bool broadcast, uint8_t dest_id){ /** * @brief Determines which channel to route the frame to, depending on the dest (board) id - * - * @param dest_id - * @param channel_to_send - * @return esp_err_t + * + * @param dest_id + * @param channel_to_send + * @return esp_err_t */ esp_err_t DataLinkManager::route_frame(uint8_t dest_id, uint8_t* channel_to_send){ RIPRow* entry = nullptr; @@ -364,7 +364,7 @@ esp_err_t DataLinkManager::get_routing_table(RIPRow_public* table, size_t* table table[i].info = rip_table[i].info; table[i].channel = rip_table[i].channel; curr_size++; - } + } xSemaphoreGive(rip_table[i].row_sem); } @@ -427,7 +427,7 @@ esp_err_t DataLinkManager::get_routing_table(RIPRow_public* table, size_t* table link_layer_obj->rip_table[i].ttl_flush = RIP_FLUSH_COUNT; broadcast = true; } - } + } xSemaphoreGive(link_layer_obj->rip_table[i].row_sem); } @@ -452,5 +452,5 @@ void DataLinkManager::start_rip_tasks(){ ESP_LOGI(DEBUG_LINK_TAG, "Starting RIP Broadcast task"); xTaskCreate(DataLinkManager::rip_broadcast_timer_function, "RIPBroadcast", 4096, static_cast(this), 5, &rip_broadcast_task); ESP_LOGI(DEBUG_LINK_TAG, "Starting RIP TTL task"); - xTaskCreate(DataLinkManager::rip_ttl_decrement_task, "RIPTTL", 2048, static_cast(this), 5, &rip_ttl_task); -} \ No newline at end of file + xTaskCreate(DataLinkManager::rip_ttl_decrement_task, "RIPTTL", 4096, static_cast(this), 5, &rip_ttl_task); +} diff --git a/components/rpc/CommunicationFactory.cpp b/components/rpc/CommunicationFactory.cpp index 875b784..dc7e154 100644 --- a/components/rpc/CommunicationFactory.cpp +++ b/components/rpc/CommunicationFactory.cpp @@ -6,8 +6,10 @@ #include "CommunicationFactory.h" #include "constants/tcp.h" +#include "constants/udp.h" #include "wireless/mDNSDiscoveryService.h" #include "wireless/TCPServer.h" +#include "wireless/UDPServer.h" #include "wireless/WifiManager.h" std::unique_ptr CommunicationFactory::create_connection_manager(const CommunicationMethod type) { @@ -31,7 +33,7 @@ std::unique_ptr CommunicationFactory::create_discovery_servic std::unique_ptr CommunicationFactory::create_lossy_server(const CommunicationMethod type, const std::shared_ptr>>& rx_queue) { switch (type) { case Wireless: - return std::make_unique(TCP_PORT, rx_queue); // todo: replace with udp server + return std::make_unique(RECV_PORT, SEND_PORT, rx_queue); default: return nullptr; } diff --git a/components/rpc/CommunicationRouter.cpp b/components/rpc/CommunicationRouter.cpp index 89e1d95..96507c3 100644 --- a/components/rpc/CommunicationRouter.cpp +++ b/components/rpc/CommunicationRouter.cpp @@ -85,11 +85,11 @@ void CommunicationRouter::update_leader() { if (max == m_module_id) { m_pc_connection->connect(); m_lossless_server->startup(); - // todo: BTS-22 add lossy server + m_lossy_server->startup(); } else if (this->m_leader == m_module_id) { m_pc_connection->disconnect(); m_lossless_server->shutdown(); - // todo: BTS-22 add lossy server + m_lossless_server->shutdown(); } } @@ -113,7 +113,11 @@ void CommunicationRouter::route(uint8_t* buffer, const size_t length) const { this->m_rx_callback(reinterpret_cast(buffer), 512); } else if (mpi_message->destination() == PC_ADDR && this->m_leader == m_module_id) { ESP_LOGD(TAG, "Routing to wifi [dest: %d, length: %d]", static_cast(mpi_message->destination()), length); - this->m_lossless_server->send_msg(reinterpret_cast(buffer), 512); + if (mpi_message->is_durable()) { + this->m_lossless_server->send_msg(reinterpret_cast(buffer), 512); + } else { + this->m_lossy_server->send_msg(reinterpret_cast(buffer), 512); + } } else if (mpi_message->destination() == PC_ADDR) { ESP_LOGD(TAG, "Routing to wireline for wifi [dest: %d, length: %d]", static_cast(mpi_message->destination()), length); this->m_data_link_manager->send(this->m_leader, buffer, length, FrameType::MOTOR_TYPE, 0); diff --git a/components/rpc/include/CommunicationRouter.h b/components/rpc/include/CommunicationRouter.h index dbf425f..05c1546 100644 --- a/components/rpc/include/CommunicationRouter.h +++ b/components/rpc/include/CommunicationRouter.h @@ -37,6 +37,7 @@ public: m_config_manager(ConfigManager::get_instance()), m_pc_connection(CommunicationFactory::create_connection_manager(m_config_manager.get_communication_method())), m_lossless_server(CommunicationFactory::create_lossless_server(m_config_manager.get_communication_method(), m_tcp_rx_queue)), + m_lossy_server(CommunicationFactory::create_lossy_server(m_config_manager.get_communication_method(), m_tcp_rx_queue)), m_data_link_manager(std::make_unique(m_config_manager.get_module_id(), MODULE_TO_NUM_CHANNELS_MAP[m_config_manager.get_module_type()])), m_module_id(m_config_manager.get_module_id()), m_last_leader_updated(std::chrono::system_clock::now()), @@ -44,14 +45,14 @@ public: OrientationDetection::init(); update_leader(); - xTaskCreate(router_thread, "communication_router", 4096, this, 3, &this->m_router_thread); + xTaskCreate(router_thread, "router", 4096, this, 3, &this->m_router_thread); - const auto num_channels = MODULE_TO_NUM_CHANNELS_MAP[m_config_manager.get_module_type()]; - this->m_link_layer_threads.resize(num_channels); - for (int i = 0; i < num_channels; i++) { - auto *params = new link_layer_thread_params(this, i); - xTaskCreate(link_layer_thread, "communication_router_rmt", 4096, params, 3, &this->m_link_layer_threads[i]); - } + // const auto num_channels = MODULE_TO_NUM_CHANNELS_MAP[m_config_manager.get_module_type()]; + // this->m_link_layer_threads.resize(num_channels); + // for (int i = 0; i < num_channels; i++) { + // auto *params = new link_layer_thread_params(this, i); + // xTaskCreate(link_layer_thread, "router_rmt", 4096, params, 3, &this->m_link_layer_threads[i]); + // } } ~CommunicationRouter(); @@ -65,7 +66,7 @@ public: [[nodiscard]] uint8_t get_leader() const; // todo: does this really need to be here (so i can access from thread)? - std::shared_ptr>> m_tcp_rx_queue; + std::shared_ptr>> m_tcp_rx_queue; // todo: this should probably be thread safe std::function m_rx_callback; private: TaskHandle_t m_router_thread = nullptr; @@ -73,6 +74,7 @@ private: std::unique_ptr m_pc_connection; std::vector m_link_layer_threads; std::unique_ptr m_lossless_server; + std::unique_ptr m_lossy_server; std::unique_ptr m_data_link_manager; uint8_t m_leader = 0; uint8_t m_module_id; diff --git a/components/rpc/include/wireless/UDPServer.h b/components/rpc/include/wireless/UDPServer.h new file mode 100644 index 0000000..9c76c4f --- /dev/null +++ b/components/rpc/include/wireless/UDPServer.h @@ -0,0 +1,41 @@ +// +// Created by Johnathon Slightham on 2025-05-25. +// + +#ifndef UDPSERVER_H +#define UDPSERVER_H + +#include +#include +#include + +#include "freertos/FreeRTOS.h" +#include "IRPCServer.h" +#include "PtrQueue.h" + +class UDPServer final : public IRPCServer { +public: + UDPServer(int rx_port, int tx_port, const std::shared_ptr>>& rx_queue); + ~UDPServer() override; + void startup() override; + void shutdown() override; + int send_msg(char* buffer, uint32_t length) const override; + +private: + bool authenticate_client(int client_sock); + + static bool is_network_connected(); + [[noreturn]] static void socket_monitor_thread(void *args); + + int m_tx_port; + int m_rx_port; + + int m_tx_server_sock; + int m_rx_server_sock; + + TaskHandle_t m_rx_task; + + std::shared_ptr>> m_rx_queue; +}; + +#endif //UDPSERVER_H diff --git a/components/rpc/wireless/TCPServer.cpp b/components/rpc/wireless/TCPServer.cpp index 85ca928..4a61ffb 100644 --- a/components/rpc/wireless/TCPServer.cpp +++ b/components/rpc/wireless/TCPServer.cpp @@ -84,7 +84,7 @@ void TCPServer::shutdown() { constexpr int opt = 1; setsockopt(that->m_server_sock, SOL_SOCKET, SO_REUSEADDR, &opt, sizeof(opt)); - ESP_LOGI(TAG, "Socket created\n"); + ESP_LOGI(TAG, "Socket created"); sockaddr_in server_addr = { .sin_family = AF_INET, diff --git a/components/rpc/wireless/UDPServer.cpp b/components/rpc/wireless/UDPServer.cpp new file mode 100644 index 0000000..90da67b --- /dev/null +++ b/components/rpc/wireless/UDPServer.cpp @@ -0,0 +1,214 @@ +#include +#include + +#include "esp_log.h" +#include "sys/param.h" +#include "freertos/FreeRTOS.h" +#include "freertos/task.h" +#include "esp_event.h" +#include "esp_netif.h" +#include "lwip/err.h" +#include "lwip/sockets.h" +#include "lwip/sys.h" +#include "lwip/netdb.h" +#include "wireless/UDPServer.h" +#include "bits/shared_ptr_base.h" +#include "constants/app_comms.h" +#include "constants/udp.h" +#include "esp_netif.h" + +#define TAG "UDPServer" + +#define MIN(X, Y) (((X) < (Y)) ? (X) : (Y)) + +// todo: - authenticate + +UDPServer::UDPServer(const int rx_port, const int tx_port, const std::shared_ptr>>& rx_queue) { + this->m_rx_port = rx_port; + this->m_tx_port = tx_port; + this->m_rx_task = nullptr; + this->m_rx_queue = rx_queue; + this->m_rx_server_sock = 0; + this->m_tx_server_sock = 0; +} + +UDPServer::~UDPServer() { + this->shutdown(); +} + +void UDPServer::startup() { + ESP_LOGI(TAG, "Starting UDP server on port %d", this->m_rx_port); + if (nullptr != this->m_rx_task) { + ESP_LOGW(TAG, "Attempted to start UDP server when already started, ignoring start request"); + return; + } + + xTaskCreate(socket_monitor_thread, "udp_rx", 4096, this, 5, &this->m_rx_task); +} + +void UDPServer::shutdown() { + ESP_LOGI(TAG, "Shutting down UDP server"); + if (nullptr != this->m_rx_task) { + vTaskDelete(this->m_rx_task); + } +} + +[[noreturn]] void UDPServer::socket_monitor_thread(void *args) { + const auto that = static_cast(args); + + while(true) { + ESP_LOGI(TAG, "Attempting to start UDP Server on %d", that->m_rx_port); + + if (!is_network_connected()) { + ESP_LOGW(TAG, "Network is disconnected"); + vTaskDelay(SLEEP_AFTER_FAIL_MS / portTICK_PERIOD_MS); + continue; + } + + sockaddr_in saddr = {0}; + sockaddr_in from_addr = {0}; + + that->m_rx_server_sock = socket(AF_INET, SOCK_DGRAM, 0); + if (that->m_rx_server_sock == -1) { + ESP_LOGE(TAG, "Create UDP socket fail"); + vTaskDelay(SLEEP_AFTER_FAIL_MS / portTICK_PERIOD_MS); + continue; + } + + that->m_tx_server_sock = socket(AF_INET, SOCK_DGRAM, 0); + if (that->m_tx_server_sock < 0) { + ESP_LOGE(TAG, "Unable to create UDP tx socket: errno %d", errno); + close(that->m_rx_server_sock); + that->m_rx_server_sock = -1; + vTaskDelay(SLEEP_AFTER_FAIL_MS / portTICK_PERIOD_MS); + continue; + } + + int reuse = 1; + if (setsockopt(that->m_rx_server_sock, SOL_SOCKET, SO_REUSEADDR, &reuse, sizeof(reuse)) < 0) { + ESP_LOGE(TAG, "Failed to set SO_REUSEADDR. Error %d", errno); + close(that->m_rx_server_sock); + close(that->m_tx_server_sock); + that->m_rx_server_sock = -1; + that->m_tx_server_sock = -1; + vTaskDelay(SLEEP_AFTER_FAIL_MS / portTICK_PERIOD_MS); + continue; + } + + saddr.sin_family = AF_INET; + saddr.sin_port = htons(that->m_rx_port); + saddr.sin_addr.s_addr = htonl(INADDR_ANY); + int ret = bind(that->m_rx_server_sock, (struct sockaddr *)&saddr, sizeof(struct sockaddr_in)); + if (ret < 0) { + ESP_LOGE(TAG, "Failed to bind socket. Error %d", errno); + close(that->m_rx_server_sock); + close(that->m_tx_server_sock); + that->m_rx_server_sock = -1; + that->m_tx_server_sock = -1; + vTaskDelay(SLEEP_AFTER_FAIL_MS / portTICK_PERIOD_MS); + continue; + } + + struct ip_mreq imreq = {}; + imreq.imr_multiaddr.s_addr = inet_addr(RECV_MCAST); + imreq.imr_interface.s_addr = htonl(INADDR_ANY); + if(setsockopt(that->m_rx_server_sock, IPPROTO_IP, IP_ADD_MEMBERSHIP, &imreq, sizeof(struct ip_mreq)) < 0) { + ESP_LOGE(TAG, "Failed to set IP_ADD_MEMBERSHIP. Error %d", errno); + close(that->m_rx_server_sock); + close(that->m_tx_server_sock); + that->m_rx_server_sock = -1; + that->m_tx_server_sock = -1; + vTaskDelay(SLEEP_AFTER_FAIL_MS / portTICK_PERIOD_MS); + continue; + } + + uint32_t msg_size; + while(is_network_connected()) { + auto buffer = std::make_unique>(); + buffer->resize(MAX_RX_BUFFER_SIZE + 4); + + ESP_LOGI(TAG, "Before rx"); + if (int len = recvfrom(that->m_rx_server_sock, buffer->data(), MAX_RX_BUFFER_SIZE, 0, nullptr, nullptr); len < 0) { + ESP_LOGE(TAG, "Error occurred during receiving: errno %d", errno); + } else if (len < 4 || len > MAX_RX_BUFFER_SIZE) { + ESP_LOGE(TAG, "Got illegal message size"); + } else { + msg_size = *reinterpret_cast(buffer->data()); + if (msg_size > len - 4) { + ESP_LOGW(TAG, "Message size incorrect"); + continue; + } + buffer->erase(buffer->begin(), buffer->begin() + 4); // todo: copying + buffer->resize(msg_size); + that->m_rx_queue->enqueue(std::move(buffer)); + } + } + + ESP_LOGW(TAG, "Network disconnected"); + close(that->m_tx_server_sock); + that->m_tx_server_sock = -1; + vTaskDelay(SLEEP_AFTER_FAIL_MS / portTICK_PERIOD_MS); + } +} + +bool UDPServer::is_network_connected() { + esp_netif_ip_info_t ip_info; + esp_netif_t *netif = esp_netif_get_handle_from_ifkey("WIFI_STA_DEF"); + + if (netif != nullptr && esp_netif_get_ip_info(netif, &ip_info) == ESP_OK) { + return true; + } + + if (0 != ip_info.ip.addr) { + return true; + } + + netif = esp_netif_get_handle_from_ifkey("WIFI_AP_DEF"); + + if (netif != nullptr && esp_netif_get_ip_info(netif, &ip_info) == ESP_OK) { + return true; + } + + if (0 != ip_info.ip.addr) { + return true; + } + + return false; +} + +bool UDPServer::authenticate_client(int sock) { + // todo: authentication (key?) + return 0; +} + +int UDPServer::send_msg(char *buffer, const uint32_t length) const { + if (!is_network_connected() || m_tx_server_sock == -1) { + return -1; + } + + sockaddr_in mcast_dest = { + .sin_family = AF_INET, + .sin_port = htons(m_tx_port), + .sin_addr = { + .s_addr = inet_addr(SEND_MCAST) + }, + }; + + uint32_t size = length; + + iovec iov[2]; + iov[0].iov_base = &size; + iov[0].iov_len = 4; + iov[1].iov_base = buffer; + iov[1].iov_len = length; + + msghdr msg = {}; + msg.msg_iov = iov; + msg.msg_iovlen = 2; + msg.msg_name = &mcast_dest; + msg.msg_namelen = sizeof(mcast_dest); + + sendmsg(this->m_tx_server_sock, &msg, 0); + + return 0; +} diff --git a/components/rpc/wireless/WifiManager.cpp b/components/rpc/wireless/WifiManager.cpp index 97340e3..588745b 100644 --- a/components/rpc/wireless/WifiManager.cpp +++ b/components/rpc/wireless/WifiManager.cpp @@ -108,6 +108,8 @@ int WifiManager::init_connection() { esp_wifi_start(); esp_wifi_connect(); + esp_netif_set_default_netif(esp_netif_get_handle_from_ifkey("WIFI_STA_DEF")); + return 0; } @@ -199,6 +201,8 @@ int WifiManager::init_softap() { esp_wifi_start(); + esp_netif_set_default_netif(esp_netif_get_handle_from_ifkey("WIFI_STA_DEF")); + return 0; } @@ -214,12 +218,12 @@ void WifiManager::wifi_event_handler(void *event_handler_arg, esp_event_base_t e const auto that = static_cast(event_handler_arg); if (WIFI_EVENT_STA_START == event_id) { - ESP_LOGI(TAG, "Station mode started\n"); + ESP_LOGI(TAG, "Station mode started"); } else if (WIFI_EVENT_STA_CONNECTED == event_id) { - ESP_LOGI(TAG, "Connected to wifi in station mode\n"); + ESP_LOGI(TAG, "Connected to wifi in station mode"); that->update_state(wifi_state::connected); } else if (WIFI_EVENT_STA_DISCONNECTED == event_id) { - ESP_LOGI(TAG, "Station mode shutdown\n"); + ESP_LOGI(TAG, "Station mode shutdown"); xSemaphoreTake(that->m_mutex, portMAX_DELAY); if (that->m_state == wifi_state::connected) { xSemaphoreGive(that->m_mutex); @@ -228,12 +232,12 @@ void WifiManager::wifi_event_handler(void *event_handler_arg, esp_event_base_t e xSemaphoreGive(that->m_mutex); } } else if (IP_EVENT_STA_GOT_IP == event_id) { - ESP_LOGI(TAG, "Got IP as station\n"); + ESP_LOGI(TAG, "Got IP as station"); } else if (WIFI_EVENT_AP_STACONNECTED == event_id) { - ESP_LOGI(TAG, "User connected to AP\n"); + ESP_LOGI(TAG, "User connected to AP"); } else if (WIFI_EVENT_AP_STADISCONNECTED == event_id) { - ESP_LOGI(TAG, "User disconnected from AP\n"); + ESP_LOGI(TAG, "User disconnected from AP"); } else if (WIFI_EVENT_AP_START == event_id) { - ESP_LOGI(TAG, "AP started\n"); + ESP_LOGI(TAG, "AP started"); } } diff --git a/main/LoopManager.cpp b/main/LoopManager.cpp index b0cbad9..ee78369 100644 --- a/main/LoopManager.cpp +++ b/main/LoopManager.cpp @@ -14,6 +14,7 @@ #define ACTUATOR_CMD_TAG 5 #define TOPOLOGY_CMD_TAG 6 +#define METADATA_RX_TAG 7 #define METADATA_PERIOD_MS 1000 @@ -51,7 +52,18 @@ casted_orientations, that->m_messaging_interface->get_connection_type(), that->m_messaging_interface->get_leader()); - that->m_messaging_interface->send(static_cast(data), size, PC_ADDR, TOPOLOGY_CMD_TAG, true); + that->m_messaging_interface->send(static_cast(data), size, PC_ADDR, TOPOLOGY_CMD_TAG, false); vTaskDelay(METADATA_PERIOD_MS / portTICK_PERIOD_MS); } } + +[[noreturn]] void LoopManager::metadata_rx_loop(char *args) { + const auto that = reinterpret_cast(args); + const auto buffer = std::make_unique>(); + buffer->resize(512); + while (true) { + that->m_messaging_interface->recv(buffer->data(), 512, PC_ADDR, METADATA_RX_TAG); + + + } +} diff --git a/main/control/ActuatorFactory.cpp b/main/control/ActuatorFactory.cpp index ca0cc6d..d715aea 100644 --- a/main/control/ActuatorFactory.cpp +++ b/main/control/ActuatorFactory.cpp @@ -9,15 +9,16 @@ #include "control/Servo1Actuator.h" #include "flatbuffers_generated/RobotModule_generated.h" -std::unique_ptr ActuatorFactory::create_actuator(const ModuleType type) { - switch (type) { - case ModuleType_SERVO_1: - return std::make_unique(); - case ModuleType_SERVO_2: - return std::make_unique(); - case ModuleType_DC_MOTOR: - return std::make_unique(); - default: - return nullptr; - } +std::unique_ptr +ActuatorFactory::create_actuator(const ModuleType type) { + switch (type) { + case ModuleType_SERVO_1: + return std::make_unique(); + case ModuleType_SERVO_2: + return std::make_unique(); + case ModuleType_DC_MOTOR: + return std::make_unique(); + default: + return nullptr; + } } diff --git a/main/control/Servo1Actuator.cpp b/main/control/Servo1Actuator.cpp index 1a702c9..5d279bf 100644 --- a/main/control/Servo1Actuator.cpp +++ b/main/control/Servo1Actuator.cpp @@ -3,42 +3,46 @@ // #include "control/Servo1Actuator.h" -#include "util/number_utils.h" -#include "driver/ledc.h" -#include "constants/module.h" #include "AngleControlMessageBuilder.h" +#include "constants/module.h" +#include "driver/ledc.h" +#include "util/number_utils.h" #define LOW_DUTY 200 #define HIGH_DUTY 1000 #define PWM_FREQ 50 // 4khz Servo1Actuator::Servo1Actuator() { - ledc_timer_config_t ledc_timer = { - .speed_mode = LEDC_LOW_SPEED_MODE, - .duty_resolution = LEDC_TIMER_13_BIT, - .timer_num = LEDC_TIMER_0, - .freq_hz = PWM_FREQ, - .clk_cfg = LEDC_AUTO_CLK, - }; - ESP_ERROR_CHECK(ledc_timer_config(&ledc_timer)); + ledc_timer_config_t ledc_timer = { + .speed_mode = LEDC_LOW_SPEED_MODE, + .duty_resolution = LEDC_TIMER_13_BIT, + .timer_num = LEDC_TIMER_0, + .freq_hz = PWM_FREQ, + .clk_cfg = LEDC_AUTO_CLK, + }; + ESP_ERROR_CHECK(ledc_timer_config(&ledc_timer)); - ledc_channel_config_t ledc_channel = { - .gpio_num = SERVO_GPIO, - .speed_mode = LEDC_LOW_SPEED_MODE, - .channel = LEDC_CHANNEL_0, - .intr_type = LEDC_INTR_DISABLE, - .timer_sel = LEDC_TIMER_0, - .duty = (HIGH_DUTY + LOW_DUTY) / 2, // move motor to midpoint initially - .hpoint = 0, - }; + ledc_channel_config_t ledc_channel = { + .gpio_num = SERVO_GPIO, + .speed_mode = LEDC_LOW_SPEED_MODE, + .channel = LEDC_CHANNEL_0, + .intr_type = LEDC_INTR_DISABLE, + .timer_sel = LEDC_TIMER_0, + .duty = (HIGH_DUTY + LOW_DUTY) / 2, // move motor to midpoint initially + .hpoint = 0, + }; - ESP_ERROR_CHECK(ledc_channel_config(&ledc_channel)); + ESP_ERROR_CHECK(ledc_channel_config(&ledc_channel)); } void Servo1Actuator::actuate(uint8_t *cmd) { - const auto* angleControlCmd = Flatbuffers::AngleControlMessageBuilder::parse_angle_control_message(cmd); - const auto newDuty = util::mapRange(angleControlCmd->angle(), 0, 180, LOW_DUTY, HIGH_DUTY); + const auto *angleControlCmd = + Flatbuffers::AngleControlMessageBuilder::parse_angle_control_message(cmd); + const auto newDuty = util::mapRange(angleControlCmd->angle(), 0, 180, + LOW_DUTY, HIGH_DUTY); - ESP_ERROR_CHECK(ledc_set_duty(LEDC_LOW_SPEED_MODE, LEDC_CHANNEL_0, newDuty)); - ESP_ERROR_CHECK(ledc_update_duty(LEDC_LOW_SPEED_MODE, LEDC_CHANNEL_0)); + std::cout << "actuating to " << angleControlCmd->angle() << std::endl; + + ESP_ERROR_CHECK(ledc_set_duty(LEDC_LOW_SPEED_MODE, LEDC_CHANNEL_0, newDuty)); + ESP_ERROR_CHECK(ledc_update_duty(LEDC_LOW_SPEED_MODE, LEDC_CHANNEL_0)); }