Config manager fixes

components/config/ConfigManager.cpp

@@ -2,101 +2,182 @@
 // Created by Johnathon Slightham on 2025-06-12.
 //
 
+
+#include <mutex>
+#include <esp_log.h>
+
 #include "nvs_flash.h"
 #include "constants/config.h"
-
 #include "ConfigManager.h"
 
+static auto TAG = "ConfigManager";
+
 void ConfigManager::init_config() {
-    nvs_flash_init();
+    std::unique_lock lock(rw_lock);
+    if (this->initialized)
+        return;
+    this->initialized = true;
+    lock.unlock();
 
-    nvs_handle config_handle;
-    nvs_open(NVS_FLASH_NAMESPACE, NVS_READWRITE, &config_handle);
-
-    uint16_t id;
-    if (ESP_ERR_NVS_NOT_FOUND == nvs_get_u16(config_handle, MODULE_ID_KEY, &id)) {
-        nvs_set_u16(config_handle, MODULE_ID_KEY, DEFAULT_MODULE_ID);
+    esp_err_t ret = nvs_flash_init();
+    if (ret == ESP_ERR_NVS_NO_FREE_PAGES || ret == ESP_ERR_NVS_NEW_VERSION_FOUND) {
+        // NVS partition was truncated and needs to be erased, retry nvs_flash_init after erasing
+        ESP_ERROR_CHECK(nvs_flash_erase());
+        ret = nvs_flash_init();
     }
-
-    uint16_t type;
-    if (ESP_ERR_NVS_NOT_FOUND == nvs_get_u16(config_handle, MODULE_TYPE_KEY, &type)) {
-        nvs_set_u16(config_handle, MODULE_TYPE_KEY, DEFAULT_MODULE_TYPE);
-    }
-
-    nvs_close(config_handle);
+    ESP_ERROR_CHECK(ret);
 }
 
-// todo: we should probably cache some of these things
-uint16_t ConfigManager::get_module_id() {
-    nvs_handle config_handle;
-    nvs_open(NVS_FLASH_NAMESPACE, NVS_READONLY, &config_handle);
-    uint16_t id;
-    nvs_get_u16(config_handle, MODULE_ID_KEY, &id);
-    nvs_close(config_handle);
-    return id;
+uint16_t ConfigManager::get_module_id() const {
+    uint16_t i = DEFAULT_MODULE_ID;
+    get_uint16(MODULE_ID_KEY, &i);
+    return i;
 }
 
-ModuleType ConfigManager::get_module_type() {
-    nvs_handle config_handle;
-    nvs_open(NVS_FLASH_NAMESPACE, NVS_READONLY, &config_handle);
-    uint16_t type;
-    nvs_get_u16(config_handle, MODULE_TYPE_KEY, &type);
-    nvs_close(config_handle);
+ModuleType ConfigManager::get_module_type() const {
+    uint16_t type = DEFAULT_MODULE_TYPE;
+    get_uint16(MODULE_TYPE_KEY, &type);
     return static_cast<ModuleType>(type);
 }
 
-std::string ConfigManager::get_wifi_ssid() {
-    return get_string(WIFI_SSID_KEY);
-}
-
-std::string ConfigManager::get_wifi_password() {
-    return get_string(WIFI_PASSWORD_KEY);
-}
-
-std::string ConfigManager::get_string(const char *key) {
-    nvs_handle config_handle;
-    nvs_open(NVS_FLASH_NAMESPACE, NVS_READONLY, &config_handle);
-
-    size_t required_size; // get size of the string
-    if (ESP_ERR_NVS_NOT_FOUND == nvs_get_str(config_handle, key, nullptr, &required_size)) {
-        return "";
-    }
-
+std::string ConfigManager::get_wifi_ssid() const {
     std::string str;
-    str.resize(required_size);
-    if (ESP_ERR_NVS_NOT_FOUND == nvs_get_str(config_handle, key, str.data(), &required_size)) {
-        return "";
-    }
-
-    nvs_close(config_handle);
+    get_string(WIFI_SSID_KEY, str);
     return str;
 }
 
-void ConfigManager::set_module_id(const uint16_t id) {
+std::string ConfigManager::get_wifi_password() const {
+    std::string str;
+    get_string(WIFI_PASSWORD_KEY, str);
+    return str;
+}
+
+esp_err_t ConfigManager::get_string(const char *key, std::string& out) const {
+    std::shared_lock lock(rw_lock);
+    esp_err_t ret = ESP_OK;
+    if (const auto str = get_config_from_cache<std::string>(key)) {
+        out = *str;
+        return ret;
+    }
+
+    ESP_LOGD(TAG, "get_string cache miss");
+
     nvs_handle config_handle;
-    nvs_open(NVS_FLASH_NAMESPACE, NVS_READWRITE, &config_handle);
-    nvs_set_u16(config_handle, MODULE_ID_KEY, id);
+    if (ret = nvs_open(NVS_FLASH_NAMESPACE, NVS_READONLY, &config_handle); ret != ESP_OK) {
+        ESP_LOGE(TAG, "get_string Failed to open NVS");
+        return ret;
+    }
+
+    size_t required_size; // get size of the string
+    if (ret = nvs_get_str(config_handle, key, nullptr, &required_size); ret != ESP_OK) {
+        ESP_LOGE(TAG, "get_string failed to get string size");
+        return ret;
+    }
+
+    out.resize(required_size);
+    if (ret = nvs_get_str(config_handle, key, out.data(), &required_size); ret != ESP_OK) {
+        ESP_LOGE(TAG, "get_string failed to get string");
+        return ret;
+    }
+
     nvs_close(config_handle);
+    return ret;
+}
+
+esp_err_t ConfigManager::get_uint16(const char *key, uint16_t *out) const {
+    esp_err_t ret = ESP_OK;
+    std::shared_lock lock(rw_lock);
+    if (const auto i = get_config_from_cache<uint16_t>(key)) {
+        *out = i.value();
+        return ret;
+    }
+
+    ESP_LOGD(TAG, "get_uint16 cache miss");
+
+    nvs_handle config_handle;
+    if (ret = nvs_open(NVS_FLASH_NAMESPACE, NVS_READONLY, &config_handle); ret != ESP_OK) {
+        return ret;
+    }
+
+    if (ret = nvs_get_u16(config_handle, key, out); ret != ESP_OK) {
+        return ret;
+    }
+
+    nvs_close(config_handle);
+    return ret;
+}
+
+template<typename T>
+std::optional<T> ConfigManager::get_config_from_cache(const std::string& key) const {
+    // Not thread safe
+    if (const auto it = cache.find(key); it != cache.end()) {
+        if (auto val = std::get_if<T>(&it->second))
+            return *val;
+    }
+    return std::nullopt;
+}
+
+void ConfigManager::set_module_id(const uint16_t id) {
+    set<nvs_set_u16, uint16_t>(MODULE_ID_KEY, id);
 }
 
 void ConfigManager::set_module_type(const ModuleType type) {
-    nvs_handle config_handle;
-    nvs_open(NVS_FLASH_NAMESPACE, NVS_READWRITE, &config_handle);
-    nvs_set_u16(config_handle, MODULE_TYPE_KEY, type);
-    nvs_close(config_handle);
+    set<nvs_set_u16, uint16_t>(MODULE_TYPE_KEY, type);
 }
 
 void ConfigManager::set_wifi_ssid(const char* ssid) {
-    nvs_handle config_handle;
-    nvs_open(NVS_FLASH_NAMESPACE, NVS_READWRITE, &config_handle);
-    nvs_set_str(config_handle, WIFI_SSID_KEY, ssid);
-    nvs_close(config_handle);
+    set<nvs_set_cpp_str, std::string>(WIFI_SSID_KEY, ssid);
 }
 
 void ConfigManager::set_wifi_password(const char* password) {
-    nvs_handle config_handle;
-    nvs_open(NVS_FLASH_NAMESPACE, NVS_READWRITE, &config_handle);
-    nvs_set_str(config_handle, WIFI_PASSWORD_KEY, password);
-    nvs_close(config_handle);
+    set<nvs_set_cpp_str, std::string>(WIFI_PASSWORD_KEY, password);
 }
 
+// Func - the esp write function to call (ie. nvs_set_u16)
+// T - the type of the value in the key value pair
+template<auto Func, typename T>
+esp_err_t ConfigManager::set(const char* key, const T value) {
+    std::unique_lock lock(rw_lock);
+
+    if (write_to_cache(key, value)) {
+        return ESP_OK;
+    }
+
+    esp_err_t ret = ESP_OK;
+    nvs_handle config_handle;
+    ret = nvs_open(NVS_FLASH_NAMESPACE, NVS_READWRITE, &config_handle);
+    if (ESP_OK != ret) {
+        ESP_LOGE(TAG, "set error opening NVS handle");
+        return ret;
+    }
+
+    ret = Func(config_handle, key, value);
+    if (ESP_OK != ret) {
+        ESP_LOGE(TAG, "set error writing to NVS key %s", key);
+    }
+    nvs_close(config_handle);
+
+    return ret;
+}
+
+// Returns true when we don't need to write through the cache
+template<typename T>
+bool ConfigManager::write_to_cache(const std::string& key, const T value) {
+    const auto it = cache.find(key);
+    if (it != cache.end()) {
+        if (const auto existing = std::get_if<T>(&it->second)) {
+            if (*existing == value)
+                return true;
+        }
+    }
+    cache[key] = value;
+    return false;
+}
+
+esp_err_t ConfigManager::nvs_set_cpp_str(const nvs_handle_t handle, const std::string& key, const std::string& str) {
+    return nvs_set_str(handle, key.c_str(), str.c_str());
+}
+
+esp_err_t ConfigManager::nvs_get_cpp_str(const nvs_handle_t handle, const std::string& key, std::string& out_str, size_t *length) {
+    return nvs_get_str(handle, key.c_str(), out_str.data(), length);
+}

components/config/include/ConfigManager.h

@@ -5,26 +5,60 @@
 #ifndef CONFIGMANAGER_H
 #define CONFIGMANAGER_H
 
-#include "constants/config.h"
+#include <variant>
+#include <unordered_map>
+#include <shared_mutex>
+#include <esp_check.h>
+#include <nvs.h>
+
 #include "flatbuffers_generated/RobotModule_generated.h"
 
+// Singleton to r/w config from the ESP32 nvs (thread safe and cached)
+
 class ConfigManager {
 public:
-    static void init_config();
+    ConfigManager(const ConfigManager&) = delete;
+    void operator=(ConfigManager &) = delete;
 
-    static uint16_t get_module_id();
-    static ModuleType get_module_type();
-    static std::string get_wifi_ssid();
-    static std::string get_wifi_password();
+    static ConfigManager& get_instance() { // Thread safe as of C++11
+        static ConfigManager instance;
+        instance.init_config();
+        return instance;
+    }
 
-    static void set_module_id(uint16_t id);
-    static void set_module_type(ModuleType type);
-    static void set_wifi_ssid(const char* ssid);
-    static void set_wifi_password(const char* password);
+    uint16_t get_module_id() const;
+    ModuleType get_module_type() const;
+    std::string get_wifi_ssid() const;
+    std::string get_wifi_password() const;
+
+    void set_module_id(uint16_t id);
+    void set_module_type(ModuleType type);
+    void set_wifi_ssid(const char* ssid);
+    void set_wifi_password(const char* password);
 
 private:
-    static std::string get_string(const char* key);
+    ConfigManager() = default;
+    esp_err_t get_string(const char *key, std::string &out) const;
+    esp_err_t get_uint16(const char *key, uint16_t *out) const;
 
+
+    static esp_err_t nvs_set_cpp_str(nvs_handle_t handle, const std::string& key, const std::string& str);
+    static esp_err_t nvs_get_cpp_str(nvs_handle_t handle, const std::string& key, std::string& out_str, size_t *length);
+
+    template<typename T>
+    std::optional<T> get_config_from_cache(const std::string& key) const;
+
+    template<auto Func, typename T>
+    esp_err_t set(const char* key, T value);
+
+    template<typename T>
+    bool write_to_cache(const std::string& key, T value);
+
+    void init_config();
+
+    std::unordered_map<std::string, std::variant<uint16_t, int8_t, std::string>> cache;
+    mutable std::shared_mutex rw_lock;
+    bool initialized = false;
 };
 
 #endif //CONFIGMANAGER_H

components/flatbuffers/include/flatbuffers_generated/RobotModule_generated.h

@@ -24,33 +24,36 @@ enum ModuleType : int8_t {
   ModuleType_SERVO_1 = 1,
   ModuleType_DC_MOTOR = 2,
   ModuleType_BATTERY = 3,
+  ModuleType_SERVO_2 = 4,
   ModuleType_MIN = ModuleType_SPLITTER,
-  ModuleType_MAX = ModuleType_BATTERY
+  ModuleType_MAX = ModuleType_SERVO_2
 };
 
-inline const ModuleType (&EnumValuesModuleType())[4] {
+inline const ModuleType (&EnumValuesModuleType())[5] {
   static const ModuleType values[] = {
     ModuleType_SPLITTER,
     ModuleType_SERVO_1,
     ModuleType_DC_MOTOR,
-    ModuleType_BATTERY
+    ModuleType_BATTERY,
+    ModuleType_SERVO_2
   };
   return values;
 }
 
 inline const char * const *EnumNamesModuleType() {
-  static const char * const names[5] = {
+  static const char * const names[6] = {
     "SPLITTER",
     "SERVO_1",
     "DC_MOTOR",
     "BATTERY",
+    "SERVO_2",
     nullptr
   };
   return names;
 }
 
 inline const char *EnumNameModuleType(ModuleType e) {
-  if (::flatbuffers::IsOutRange(e, ModuleType_SPLITTER, ModuleType_BATTERY)) return "";
+  if (::flatbuffers::IsOutRange(e, ModuleType_SPLITTER, ModuleType_SERVO_2)) return "";
   const size_t index = static_cast<size_t>(e);
   return EnumNamesModuleType()[index];
 }

components/ptrQueue/include/PtrQueue.h

@@ -13,7 +13,7 @@
 template <typename T>
 class PtrQueue {
 public:
-    explicit PtrQueue(UBaseType_t queueLength)
+    explicit PtrQueue(const UBaseType_t queueLength)
         : queue(xQueueCreate(queueLength, sizeof(T*))) {}
 
     ~PtrQueue() {

components/rpc/CMakeLists.txt

@@ -1,4 +1,6 @@
 idf_component_register(SRCS "WifiManager.cpp" "mDNSDiscoveryService.cpp" "TCPServer.cpp" "CommunicationRouter.cpp" "MessagingInterface.cpp" "OrientationDetection.cpp"
-                        PRIV_REQUIRES driver esp_event nvs_flash esp_netif esp_wifi espressif__mdns constants config flatbuffers dataLink rmt
-                        REQUIRES ptrQueue
+                        PRIV_REQUIRES driver esp_event nvs_flash esp_netif espressif__mdns constants config flatbuffers dataLink rmt
+                        REQUIRES ptrQueue esp_wifi
                         INCLUDE_DIRS "include")
+
+target_compile_options(${COMPONENT_LIB} PRIVATE -fexceptions)

components/rpc/CommunicationRouter.cpp

@@ -1,16 +1,17 @@
-#include "CommunicationRouter.h"
-
-#include <AngleControlMessageBuilder.h>
 #include <iostream>
+
+#include "CommunicationRouter.h"
+#include "AngleControlMessageBuilder.h"
 #include "mDNSDiscoveryService.h"
 #include "MPIMessageBuilder.h"
 #include "WifiManager.h"
 #include "freertos/FreeRTOS.h"
-#include "freertos/queue.h"
 #include "Tables.h"
 #include "PtrQueue.h"
 #include "OrientationDetection.h"
 
+#define TAG "CommunicationRouter"
+
 CommunicationRouter::~CommunicationRouter() {
     vTaskDelete(m_router_thread);
 }
@@ -24,7 +25,7 @@ CommunicationRouter::~CommunicationRouter() {
 
     while (true) {
         const auto buffer = that->m_tcp_rx_queue->dequeue();
-        std::cout << "routing from tcp" << std::endl;
+        ESP_LOGD(TAG, "Got message from TCP");
         that->route(buffer->data(), buffer->size());
     }
 }
@@ -35,10 +36,10 @@ CommunicationRouter::~CommunicationRouter() {
     const auto channel = params->channel;
     delete params;
 
-    char buffer[512];
     size_t bytes_received = 0;
     that->m_data_link_manager->start_receive_frames(channel);
     while (true) {
+        char buffer[512];
         // todo: very c style function calls
         const auto err = that->m_data_link_manager->receive(reinterpret_cast<uint8_t *>(buffer), 512, &bytes_received, channel);
         that->m_data_link_manager->start_receive_frames(channel);
@@ -46,7 +47,7 @@ CommunicationRouter::~CommunicationRouter() {
         // todo: do we only want one thread ever doing this?
         if (std::chrono::system_clock::now() - that->m_last_leader_updated > std::chrono::seconds(15)) {
             that->m_last_leader_updated = std::chrono::system_clock::now();
-            std::cout << "Updating leader" << std::endl;
+            ESP_LOGI(TAG, "Updating leader");
             that->update_leader();
         }
 
@@ -54,12 +55,13 @@ CommunicationRouter::~CommunicationRouter() {
             continue;
         }
 
-        std::cout << "routing message from rmt" << std::endl;
+        ESP_LOGD(TAG, "Got message from RMT");
         that->route(reinterpret_cast<uint8_t *>(buffer), bytes_received);
     }
 }
 
 int CommunicationRouter::send_msg(char* buffer, const size_t length) const {
+    ESP_LOGD(TAG, "Got message from application");
     route(reinterpret_cast<uint8_t *>(buffer), length);
     return 0;
 }
@@ -101,17 +103,16 @@ void CommunicationRouter::route(uint8_t* buffer, const size_t length) const {
     const auto& mpi_message = Flatbuffers::MPIMessageBuilder::parse_mpi_message(buffer);
 
     if (mpi_message->destination() == m_module_id) {
-        std::cout << "Routing to this module [dest:" << static_cast<int>(mpi_message->destination()) << ", length: " << length << "]" << std::endl;
-
+        ESP_LOGD(TAG, "Routing to this module [dest: %d, length: %d]", static_cast<int>(mpi_message->destination()), length);
         this->m_rx_callback(reinterpret_cast<char *>(buffer), 512);
     } else if (mpi_message->destination() == PC_ADDR && this->m_leader == m_module_id) {
-        std::cout << "Routing to wifi [dest:" << static_cast<int>(mpi_message->destination()) << ", length: " << length << "]" << std::endl;
+        ESP_LOGD(TAG, "Routing to wifi [dest: %d, length: %d]", static_cast<int>(mpi_message->destination()), length);
         this->m_tcp_server->send_msg(reinterpret_cast<char *>(buffer), 512);
     } else if (mpi_message->destination() == PC_ADDR) {
-        std::cout << "Routing to wireline for wifi [dest:" << static_cast<int>(mpi_message->destination()) << ", length: " << length << "]" << std::endl;
+        ESP_LOGD(TAG, "Routing to wireline for wifi [dest: %d, length: %d]", static_cast<int>(mpi_message->destination()), length);
         this->m_data_link_manager->send(this->m_leader, buffer, length, FrameType::MOTOR_TYPE, 0);
     }else {
-        std::cout << "Routing to wireline [dest:" << static_cast<int>(mpi_message->destination()) << ", length: " << length << "]" << std::endl;
+        ESP_LOGD(TAG, "Routing to wireline [dest: %d, length: %d]", static_cast<int>(mpi_message->destination()), length);
         this->m_data_link_manager->send(mpi_message->destination(), buffer, length, FrameType::MOTOR_TYPE, 0);
     }
 }

components/rpc/MessagingInterface.cpp

@@ -15,14 +15,14 @@ MessagingInterface::~MessagingInterface() {
     vQueueDelete(m_mpi_rx_queue);
     vSemaphoreDelete(m_map_semaphore);
 
-    for (const auto [_tag, queue] : m_tag_to_queue) {
+    for (const auto queue: m_tag_to_queue | std::views::values) {
         vQueueDelete(queue);
     }
 }
 
-int MessagingInterface::send(char* buffer, int size, int destination, int tag, bool durable) {
+int MessagingInterface::send(char* buffer, const int size, const int destination, const int tag, const bool durable) {
     Flatbuffers::MPIMessageBuilder builder;
-    const auto [mpi_buffer, mpi_size] = builder.build_mpi_message(Messaging::MessageType_PTP, ConfigManager::get_module_id(), destination, sequence_number++, durable, tag, std::vector<uint8_t>(buffer, buffer + size));
+    const auto [mpi_buffer, mpi_size] = builder.build_mpi_message(Messaging::MessageType_PTP, m_config_manager.get_module_id(), destination, m_sequence_number++, durable, tag, std::vector<uint8_t>(buffer, buffer + size));
 
     m_router->send_msg(static_cast<char *>(mpi_buffer), mpi_size);
     return 0;

components/rpc/OrientationDetection.cpp

@@ -2,15 +2,16 @@
 // Created by Johnathon Slightham on 2025-07-26.
 //
 
-#include <constants/module.h>
+#include "constants/module.h"
 #include "OrientationDetection.h"
+#include "ConfigManager.h"
+#include "iostream"
 
-#include <ConfigManager.h>
-#include <iostream>
-#include <bits/ostream.tcc>
+#define TAG "OrientationDetection"
 
 void OrientationDetection::init() {
-    for (int i = 0; i < MODULE_TO_NUM_CHANNELS_MAP[ConfigManager::get_module_type()]; i++) {
+    const auto& config_manager = ConfigManager::get_instance();
+    for (int i = 0; i < MODULE_TO_NUM_CHANNELS_MAP[config_manager.get_module_type()]; i++) {
         setup_gpio(static_cast<gpio_num_t>(CHANNEL_TO_0_DEG_MAP[i]));
         setup_gpio(static_cast<gpio_num_t>(CHANNEL_TO_90_DEG_MAP[i]));
         setup_gpio(static_cast<gpio_num_t>(CHANNEL_TO_180_DEG_MAP[i]));
@@ -20,16 +21,16 @@ void OrientationDetection::init() {
 
 Orientation OrientationDetection::get_orientation(const uint8_t channel) {
     if (gpio_get_level(static_cast<gpio_num_t>(CHANNEL_TO_90_DEG_MAP[channel]))) {
-        std::cout << "90deg" << std::endl;
+        ESP_LOGD(TAG, "90deg");
         return Orientation_Deg90;
     } else if (gpio_get_level(static_cast<gpio_num_t>(CHANNEL_TO_180_DEG_MAP[channel]))) {
-        std::cout << "180deg" << std::endl;
+        ESP_LOGD(TAG, "180deg");
         return Orientation_Deg180;
     } else if (gpio_get_level(static_cast<gpio_num_t>(CHANNEL_TO_270_DEG_MAP[channel]))) {
-        std::cout << "270deg" << std::endl;
+        ESP_LOGD(TAG, "270deg");
         return Orientation_Deg270;
     } else {
-        std::cout << "No orientation detected" << std::endl;
+        ESP_LOGD(TAG, "No orientation detected");
         return Orientation_Deg0;
     }
 }

components/rpc/TCPServer.cpp

@@ -1,24 +1,23 @@
-#include <string.h>
+#include <iostream>
+#include <memory>
+#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 <iostream>
-
+#include "lwip/netdb.h"
 #include "TCPServer.h"
-
-#include <memory>
-#include <bits/shared_ptr_base.h>
-#include <constants/app_comms.h>
-
+#include "bits/shared_ptr_base.h"
+#include "constants/app_comms.h"
 #include "constants/tcp.h"
 
+#define TAG "TCPServer"
+
 // todo: - add message routing to correct client
 //       - authenticate (don't just return true from the auth function)
 //       - tx from board
@@ -51,18 +50,18 @@ TCPServer::~TCPServer() {
     const auto that = static_cast<TCPServer*>(args);
 
     while (true) {
-        printf("Attempting to start TCP Server on port %d", that->m_port);
+        ESP_LOGI(TAG, "Attempting to start TCP Server on port %d", that->m_port);
 
         that->m_server_sock = socket(AF_INET, SOCK_STREAM, IPPROTO_IP);
         if (that->m_server_sock < 0) {
-            printf("Unable to create TCP socket: errno %d\n", errno);
+            ESP_LOGE(TAG, "Unable to create TCP socket: errno %d\n", errno);
             vTaskDelay(SLEEP_AFTER_FAIL_MS / portTICK_PERIOD_MS);
             continue;
         }
 
         constexpr int opt = 1;
         setsockopt(that->m_server_sock, SOL_SOCKET, SO_REUSEADDR, &opt, sizeof(opt));
-        printf("Socket created\n");
+        ESP_LOGI(TAG, "Socket created\n");
 
         sockaddr_in server_addr = {
             .sin_family = AF_INET,
@@ -74,18 +73,18 @@ TCPServer::~TCPServer() {
 
         int err = bind(that->m_server_sock, reinterpret_cast<struct sockaddr *>(&server_addr), sizeof(server_addr));
         if (0 != err) {
-            printf("Socket unable to bind: errno %d\n", errno);
+            ESP_LOGE(TAG, "Socket unable to bind: errno %d\n", errno);
             close(that->m_server_sock);
             that->m_server_sock = -1;
             vTaskDelay(SLEEP_AFTER_FAIL_MS / portTICK_PERIOD_MS);
             continue;
         }
 
-        printf("Socket bound to port %d\n", that->m_port);
+        ESP_LOGI(TAG, "Socket bound to port %d\n", that->m_port);
 
         err = listen(that->m_server_sock, TCP_DEFAULT_LISTEN_BACKLOG);
         if (0 != err) {
-            printf("Error occurred during TCP listen: errno %d\n", errno);
+            ESP_LOGE(TAG, "Error occurred during TCP listen: errno %d\n", errno);
             close(that->m_server_sock);
             that->m_server_sock = -1;
             vTaskDelay(SLEEP_AFTER_FAIL_MS / portTICK_PERIOD_MS);
@@ -97,13 +96,13 @@ TCPServer::~TCPServer() {
             socklen_t addr_len = sizeof(client_addr);
             int client_sock = accept(that->m_server_sock, reinterpret_cast<struct sockaddr *>(&client_addr), &addr_len);
             if (client_sock < 0) {
-                printf("Unable to accept TCP connection: errno %d\n", errno);
+                ESP_LOGE(TAG, "Unable to accept TCP connection: errno %d\n", errno);
                 continue;
             }
 
             err = that->authenticate_client(client_sock);
             if (0 != err) {
-                printf("Client failed authentication\n");
+                ESP_LOGE(TAG, "Client failed authentication\n");
             }
 
             setsockopt(client_sock, SOL_SOCKET, SO_KEEPALIVE, &keepAlive, sizeof(int));
@@ -154,18 +153,17 @@ TCPServer::~TCPServer() {
                         continue;
                     }
 
-                    printf("Message size: %ld\n", msg_size);
+                    ESP_LOGD(TAG, "Message size: %ld\n", msg_size);
 
-                    int len = recv(sock, buffer->data(), msg_size, MSG_WAITALL);
-                    if (len < 0) {
-                        printf("Error occurred during receiving: errno %d\n", errno);
+                    if (int len = recv(sock, buffer->data(), msg_size, MSG_WAITALL); len < 0) {
+                        ESP_LOGE(TAG, "Error occurred during receiving: errno %d\n", errno);
                         to_remove.emplace_back(sock);
                     } else if (0 == len) {
-                        printf("Connection closed\n");
+                        ESP_LOGW(TAG, "Connection closed\n");
                         close(sock);
                         to_remove.emplace_back(sock);
                     } else {
-                        printf("TCP Server Received %d bytes\n", len);
+                        ESP_LOGD(TAG, "TCP Server Received %d bytes\n", len);
                         buffer->resize(len);
                         that->m_rx_queue->enqueue(std::move(buffer));
                     }
@@ -219,7 +217,6 @@ int TCPServer::send_msg(char *buffer, uint32_t length) const {
     }
 
     for (const auto client_sock : m_clients) {
-        std::cout << "sending tcp" << std::endl;
         send(client_sock, &length, 4, 0);
         send(client_sock, buffer, length, 0);
     }

components/rpc/WifiManager.cpp

@@ -1,30 +1,11 @@
-#include "WifiManager.h"
-
-#include <ConfigManager.h>
-#include <esp_netif.h>
-#include <esp_event.h>
-#include <freertos/semphr.h>
 #include <cstring>
-#include <mDNSDiscoveryService.h>
 
-#include "freertos/FreeRTOS.h"
-#include "freertos/semphr.h"
-#include "esp_wifi.h"
+#include "WifiManager.h"
+#include "ConfigManager.h"
 #include "constants/wifi.h"
+#include "mDNSDiscoveryService.h"
 
-WifiManager::WifiManager() {
-    esp_netif_init();
-    esp_wifi_set_storage(WIFI_STORAGE_RAM);
-    esp_event_loop_create_default();
-
-    this->m_mutex = xSemaphoreCreateMutex();
-    this->m_state = wifi_state::disconnected;
-    this->m_attempts = 0;
-    this->m_task = nullptr;
-    this->m_netif = nullptr;
-
-    xTaskCreate(reinterpret_cast<TaskFunction_t>(s_manage), "wifi_task", 3096, this, 5, &m_task);
-}
+#define TAG "WifiManager"
 
 WifiManager::~WifiManager() {
     this->handle_disconnect();
@@ -53,21 +34,21 @@ int WifiManager::disconnect() {
 
         switch (state) {
             case wifi_state::connect:
-                printf("Attempting to connect to wifi in station mode\n");
+                ESP_LOGI(TAG, "Attempting to connect to wifi in station mode\n");
                 init_connection();
                 update_state(wifi_state::connecting);
                 break;
             case wifi_state::connecting:
-                printf("connecting...\n");
+                ESP_LOGI(TAG, "connecting...\n");
                 handle_connecting();
                 break;
             case wifi_state::broadcast:
-                printf("Attempting to broadcast in softap mode\n");
+                ESP_LOGI(TAG, "Attempting to broadcast in softap mode\n");
                 init_softap();
                 update_state(wifi_state::broadcasting);
                 break;
             case wifi_state::disconnect:
-                printf("Shutting down wifi\n");
+                ESP_LOGI(TAG, "Shutting down wifi\n");
                 handle_disconnect();
                 update_state(wifi_state::disconnected);
                 break;
@@ -103,8 +84,8 @@ int WifiManager::init_connection() {
         }
     };
 
-    std::string ssid = ConfigManager::get_wifi_ssid();
-    std::string pass = ConfigManager::get_wifi_password();
+    std::string ssid = m_config_manager.get_wifi_ssid();
+    std::string pass = m_config_manager.get_wifi_password();
     std::strncpy(reinterpret_cast<char *>(wifi_configuration.sta.ssid), ssid.c_str(), 32);
     std::strncpy(reinterpret_cast<char *>(wifi_configuration.sta.password), pass.c_str(), 64);
     wifi_configuration.sta.ssid[31] = '\0';
@@ -190,13 +171,13 @@ void WifiManager::wifi_event_handler(void *event_handler_arg, esp_event_base_t e
     const auto that = static_cast<WifiManager *>(event_handler_arg);
 
     if (WIFI_EVENT_STA_START == event_id) {
-        printf("Station mode started\n");
+        ESP_LOGI(TAG, "Station mode started\n");
     } else if (WIFI_EVENT_STA_CONNECTED == event_id) {
-        printf("Connected to wifi in station mode\n");
+        ESP_LOGI(TAG, "Connected to wifi in station mode\n");
         that->update_state(wifi_state::connected);
         mDNSDiscoveryService::setup();
     } else if (WIFI_EVENT_STA_DISCONNECTED == event_id) {
-        printf("Station mode shutdown\n");
+        ESP_LOGI(TAG, "Station mode shutdown\n");
         xSemaphoreTake(that->m_mutex, portMAX_DELAY);
         if (that->m_state == wifi_state::connected) {
             xSemaphoreGive(that->m_mutex);
@@ -206,13 +187,13 @@ 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) {
-        printf("Got IP as station\n");
+        ESP_LOGI(TAG, "Got IP as station\n");
     } else if (WIFI_EVENT_AP_STACONNECTED == event_id) {
-        printf("User connected to AP\n");
+        ESP_LOGI(TAG, "User connected to AP\n");
     } else if (WIFI_EVENT_AP_STADISCONNECTED == event_id) {
-        printf("User disconnected from AP\n");
+        ESP_LOGI(TAG, "User disconnected from AP\n");
     } else if (WIFI_EVENT_AP_START == event_id) {
         mDNSDiscoveryService::setup();
-        printf("AP started\n");
+        ESP_LOGI(TAG, "AP started\n");
     }
 }

components/rpc/include/CommunicationRouter.h

@@ -36,17 +36,18 @@ public:
     CommunicationRouter(const std::function<void(char*, int)> &rx_callback, std::unique_ptr<WifiManager>&& pc_connection)
         : m_tcp_rx_queue(std::make_shared<PtrQueue<std::vector<uint8_t>>>(10)),
             m_rx_callback(rx_callback),
+            m_config_manager(ConfigManager::get_instance()),
             m_tcp_server(std::make_unique<TCPServer>(TCP_PORT, m_tcp_rx_queue)),
-            m_data_link_manager(std::make_unique<DataLinkManager>(ConfigManager::get_module_id(), MODULE_TO_NUM_CHANNELS_MAP[ConfigManager::get_module_type()])),
+            m_data_link_manager(std::make_unique<DataLinkManager>(m_config_manager.get_module_id(), MODULE_TO_NUM_CHANNELS_MAP[m_config_manager.get_module_type()])),
             m_pc_connection(std::move(pc_connection)),
-            m_module_id(ConfigManager::get_module_id()),
+            m_module_id(m_config_manager.get_module_id()),
             m_last_leader_updated(std::chrono::system_clock::now()){
         OrientationDetection::init();
         update_leader();
 
         xTaskCreate(router_thread, "communication_router", 4096, this, 3, &this->m_router_thread);
 
-        const auto num_channels = MODULE_TO_NUM_CHANNELS_MAP[ConfigManager::get_module_type()];
+        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);
@@ -73,6 +74,7 @@ public:
     std::function<void(char*, int)> m_rx_callback;
 private:
     TaskHandle_t m_router_thread = nullptr;
+    ConfigManager &m_config_manager;
     std::vector<TaskHandle_t> m_link_layer_threads;
     std::unique_ptr<TCPServer> m_tcp_server; // todo: dependency injection
     std::unique_ptr<DataLinkManager> m_data_link_manager;

components/rpc/include/MessagingInterface.h

@@ -14,8 +14,9 @@
 class MessagingInterface {
 public:
     explicit MessagingInterface(std::unique_ptr<WifiManager>&& pc_connection)
-        : m_mpi_rx_queue(xQueueCreate(MAX_RX_BUFFER_SIZE, RX_QUEUE_SIZE)),
-            m_router(std::make_unique<CommunicationRouter>([this](char* buffer, const int size) { handleRecv(buffer, size); }, std::move(pc_connection))),
+        : m_config_manager(ConfigManager::get_instance()),
+            m_mpi_rx_queue(xQueueCreate(MAX_RX_BUFFER_SIZE, RX_QUEUE_SIZE)),
+            m_router(std::make_unique<CommunicationRouter>([this](const char* buffer, const int size) { handleRecv(buffer, size); }, std::move(pc_connection))),
             m_map_semaphore(xSemaphoreCreateMutex()) {};
 
     ~MessagingInterface();
@@ -31,7 +32,8 @@ private:
 
     void checkOrInsertTag(uint8_t tag);
 
-    uint16_t sequence_number = 0;
+    ConfigManager& m_config_manager;
+    uint16_t m_sequence_number = 0;
     QueueHandle_t m_mpi_rx_queue; // todo: maybe move this down classes more
     std::unique_ptr<CommunicationRouter> m_router;
     SemaphoreHandle_t m_map_semaphore;

components/rpc/include/WifiManager.h

@@ -2,17 +2,27 @@
 #define NETWORKMANAGER_H
 
 #include <esp_netif_types.h>
+#include <esp_event.h>
+#include <esp_netif.h>
+#include <esp_wifi.h>
+#include <freertos/FreeRTOS.h>
+#include <freertos/semphr.h>
 
-#include "esp_event.h"
-
-#include "freertos/FreeRTOS.h"
-#include "freertos/semphr.h"
-
+#include "ConfigManager.h"
 #include "IWifiManager.h"
 
 class WifiManager final : IWifiManager {
 public:
-  WifiManager();
+  WifiManager() : m_config_manager(ConfigManager::get_instance()),
+                  m_mutex(xSemaphoreCreateMutex()),
+                  m_state(wifi_state::disconnected) {
+    esp_netif_init();
+    esp_wifi_set_storage(WIFI_STORAGE_RAM);
+    esp_event_loop_create_default();
+    // todo: move all task metadata to a constants/config file
+    xTaskCreate(reinterpret_cast<TaskFunction_t>(s_manage), "wifi_task", 3096, this, 5, &m_task);
+  }
+
   ~WifiManager() override;
   int connect() override;
   int disconnect() override;
@@ -31,11 +41,12 @@ private:
 
   static void wifi_event_handler(void *event_handler_arg, esp_event_base_t event_base, int32_t event_id, void *event_data);
 
+  ConfigManager& m_config_manager;
   SemaphoreHandle_t m_mutex;
   wifi_state m_state;
-  TaskHandle_t m_task;
-  unsigned int m_attempts;
-  esp_netif_t *m_netif;
+  TaskHandle_t m_task = nullptr;
+  unsigned int m_attempts = 0;
+  esp_netif_t *m_netif = nullptr;
 
 };
 

components/rpc/mDNSDiscoveryService.cpp

@@ -18,15 +18,18 @@
 // todo: clean this up (strange to be a constructor) also need to add more details, need to add to routing table
 void mDNSDiscoveryService::setup() {
     mdns_init();
-    mdns_hostname_set(std::format("botchain-{}", ConfigManager::get_module_id()).c_str());
-    mdns_instance_name_set(std::format("BotChain Robot Module {}", ConfigManager::get_module_id()).c_str());
+
+    const auto& config_manager = ConfigManager::get_instance();
+
+    mdns_hostname_set(std::format("botchain-{}", config_manager.get_module_id()).c_str());
+    mdns_instance_name_set(std::format("BotChain Robot Module {}", config_manager.get_module_id()).c_str());
 
     mdns_service_add(nullptr, "_robotcontrol", "_tcp", TCP_PORT, nullptr, 0);
     mdns_service_instance_name_set("_robotcontrol", "_tcp", "Robot Control TCP Server");
 
     mdns_txt_item_t service_txt_data[3] = {
-        {"module_id",std::to_string(ConfigManager::get_module_id()).c_str()},
-        {"module_type",std::to_string(ConfigManager::get_module_type()).c_str()},
+        {"module_id",std::to_string(config_manager.get_module_id()).c_str()},
+        {"module_type",std::to_string(config_manager.get_module_type()).c_str()},
            {"connected_modules",""},
     };
 
@@ -36,6 +39,8 @@ void mDNSDiscoveryService::setup() {
 void mDNSDiscoveryService::set_connected_boards(const std::vector<int>& boards) {
     std::stringstream ss;
 
+    const auto& config_manager = ConfigManager::get_instance();
+
     for (int i = 0; i < boards.size(); i++) {
         ss << std::to_string(boards[i]);
         if (i < boards.size() - 1) {
@@ -44,8 +49,8 @@ void mDNSDiscoveryService::set_connected_boards(const std::vector<int>& boards)
     }
 
     mdns_txt_item_t service_txt_data[3] = {
-        {"module_id",std::to_string(ConfigManager::get_module_id()).c_str()},
-        {"module_type",std::to_string(ConfigManager::get_module_type()).c_str()},
+        {"module_id",std::to_string(config_manager.get_module_id()).c_str()},
+        {"module_type",std::to_string(config_manager.get_module_type()).c_str()},
            {"connected_modules",ss.str().c_str()},
     };
     mdns_service_txt_set("_robotcontrol", "_tcp", service_txt_data, 3);

main/LoopManager.cpp

@@ -17,7 +17,7 @@
 #define METADATA_PERIOD_MS 1000
 
 [[noreturn]] void LoopManager::control_loop() const {
-    const auto actuator = ActuatorFactory::create_actuator(ConfigManager::get_module_type());
+    const auto actuator = ActuatorFactory::create_actuator(m_config_manager.get_module_type());
 
     uint8_t buffer[512];
     while (true) {
@@ -38,7 +38,7 @@
             casted_orientations.emplace_back(orientation);
         }
 
-        const auto [data, size] = topology_message_builder->build_topology_message(ConfigManager::get_module_id(), ConfigManager::get_module_type(), module_ids, casted_orientations);
+        const auto [data, size] = topology_message_builder->build_topology_message(that->m_config_manager.get_module_id(), that->m_config_manager.get_module_type(), module_ids, casted_orientations);
         that->m_messaging_interface->send(static_cast<char *>(data), size, PC_ADDR, TOPOLOGY_CMD_TAG, true);
         vTaskDelay(METADATA_PERIOD_MS / portTICK_PERIOD_MS);
     }

main/include/LoopManager.h

@@ -14,11 +14,14 @@
 
 class LoopManager {
 public:
-    LoopManager() : m_messaging_interface(std::make_unique<MessagingInterface>(std::make_unique<WifiManager>())) {}
+    LoopManager() : m_config_manager(ConfigManager::get_instance()),
+        m_messaging_interface(std::make_unique<MessagingInterface>(std::make_unique<WifiManager>())) {}
     [[noreturn]] void control_loop() const;
     [[noreturn]] static void metadata_tx_loop(char * args);
+    [[noreturn]] static void metadata_rx_loop(char * args);
 
 private:
+    ConfigManager& m_config_manager;
     std::unique_ptr<MessagingInterface> m_messaging_interface;
 
 private:

main/main.cpp

@@ -1,25 +1,25 @@
 #include <cstdio>
 #include <memory>
 
-#include "sdkconfig.h"
 #include "freertos/FreeRTOS.h"
+#include "sdkconfig.h"
 
-#include "WifiManager.h"
-#include "mDNSDiscoveryService.h"
-#include "TCPServer.h"
 #include "ConfigManager.h"
 #include "LoopManager.h"
+#include "TCPServer.h"
+#include "WifiManager.h"
 #include "esp_log.h"
+#include "mDNSDiscoveryService.h"
 
 extern "C" [[noreturn]] void app_main(void) {
-    ESP_LOGI("MEM", "Free internal RAM: %d", heap_caps_get_free_size(MALLOC_CAP_8BIT));
+    ESP_LOGI("MEM", "Free internal RAM: %d",
+        heap_caps_get_free_size(MALLOC_CAP_8BIT));
     ESP_LOGI("MEM", "Free PSRAM: %d", heap_caps_get_free_size(MALLOC_CAP_SPIRAM));
 
-    ConfigManager::init_config();
-
-
+    auto& config_manager = ConfigManager::get_instance(); // NOLINT - here for easily adding temporary config
 
     const auto loop_manager = std::make_unique<LoopManager>();
-    xTaskCreate(reinterpret_cast<TaskFunction_t>(LoopManager::metadata_tx_loop), "metadata_tx", 3096, loop_manager.get(), 3, nullptr);
+    xTaskCreate(reinterpret_cast<TaskFunction_t>(LoopManager::metadata_tx_loop),
+        "metadata_tx", 3096, loop_manager.get(), 3, nullptr);
     loop_manager->control_loop();
 }