🚌 Adds eventbus with bluetooth adapter

esp32/build_settings.ini

@@ -5,4 +5,6 @@ build_flags =
     -D EMBED_WWW
     -D SERVE_CONFIG_FILES
     -D CORS_ORIGIN=\"*\"
-    -D ENABLE_CORS
+    -D ENABLE_CORS
+    -D USE_MSGPACK=1 ; Use either msgpack or json
+    -D USE_JSON=0    ; Use either msgpack or json

esp32/include/adapters/bluetooth.hpp

@@ -0,0 +1,60 @@
+#pragma once
+#include <BLEDevice.h>
+#include <BLEServer.h>
+#include <BLEUtils.h>
+#include <BLE2902.h>
+
+#include "comm_base.hpp"
+#include "event_bus.hpp"
+#include "topic.hpp"
+
+class BluetoothService : public CommBase<> {
+    BLEServer* bleServer {nullptr};
+    BLECharacteristic* txCharacteristic {nullptr};
+    BLECharacteristic* rxCharacteristic {nullptr};
+    bool connected {false};
+
+  protected:
+    template <typename Msg>
+    using EventBusHandle = typename EventBus<Msg>::Handle;
+
+    template <typename Msg>
+    EventBusHandle<Msg>& getHandle(Topic topic) {
+        return *static_cast<EventBusHandle<Msg>*>(subscriptionHandle[static_cast<size_t>(topic)]);
+    }
+
+    template <typename Msg>
+    void setHandle(Topic topic, EventBusHandle<Msg>&& handle) {
+        subscriptionHandle[static_cast<size_t>(topic)] = new EventBusHandle<Msg>(std::move(handle));
+    }
+
+    std::array<void*, static_cast<size_t>(Topic::COUNT)> subscriptionHandle {};
+
+  private:
+    void handleReceive(const std::string& data);
+    void send(size_t clientId, const char* data, size_t len) override;
+
+    struct ServerCb : BLEServerCallbacks {
+        BluetoothService* svc;
+        ServerCb(BluetoothService* s) : svc(s) {}
+        void onConnect(BLEServer*) override { svc->connected = true; }
+        void onDisconnect(BLEServer* s) override {
+            svc->connected = false;
+            for (size_t i = 0; i < static_cast<size_t>(Topic::COUNT); ++i) svc->unsubscribe(static_cast<Topic>(i), 0);
+            svc->bleServer->startAdvertising();
+        }
+    };
+
+    struct RxCb : BLECharacteristicCallbacks {
+        BluetoothService* svc;
+        RxCb(BluetoothService* s) : svc(s) {}
+        void onWrite(BLECharacteristic* c) override {
+            auto v = c->getValue();
+            if (!v.empty()) svc->handleReceive(v);
+        }
+    };
+
+  public:
+    void begin(const char* name);
+    void loop() {}
+};

esp32/include/adapters/comm_base.hpp

@@ -0,0 +1,63 @@
+#pragma once
+#include <ArduinoJson.h>
+#include <array>
+#include <bitset>
+#include <freertos/FreeRTOS.h>
+#include "topic.hpp"
+
+#ifndef MAX_CID
+#define MAX_CID 4
+#endif
+
+enum class MsgKind : uint8_t { Connect = 0, Disconnect = 1, Event = 2, Ping = 3, Pong = 4 };
+
+template <size_t MaxCid = MAX_CID, size_t NTopics = static_cast<size_t>(Topic::COUNT)>
+class CommBase {
+    using Bits = std::bitset<MaxCid>;
+    std::array<Bits, NTopics> subs_;
+    portMUX_TYPE mux_ portMUX_INITIALIZER_UNLOCKED;
+
+    static constexpr size_t idx(Topic t) { return static_cast<size_t>(t); }
+
+    template <Topic T>
+    void encode(JsonDocument& d, const typename TopicTraits<T>::Msg& m) {
+        auto a = d.to<JsonArray>();
+        a.add(static_cast<uint8_t>(MsgKind::Event));
+        a.add(static_cast<uint8_t>(T));
+        toJson(a.add<JsonVariant>(), m);
+    }
+
+  protected:
+    virtual void send(size_t cid, const char* data, size_t len) = 0;
+
+  public:
+    void subscribe(Topic t, size_t cid) {
+        portENTER_CRITICAL(&mux_);
+        subs_[idx(t)].set(cid);
+        portEXIT_CRITICAL(&mux_);
+    }
+
+    void unsubscribe(Topic t, size_t cid) {
+        portENTER_CRITICAL(&mux_);
+        subs_[idx(t)].reset(cid);
+        portEXIT_CRITICAL(&mux_);
+    }
+
+    bool has(Topic t) const { return subs_[idx(t)].any(); }
+
+    template <Topic T>
+    void emit(const typename TopicTraits<T>::Msg& m) {
+        if (!has(T)) return;
+        JsonDocument doc;
+        encode<T>(doc, m);
+        String out;
+#if USE_MSGPACK
+        serializeMsgPack(doc, out);
+#else
+        serializeJson(doc, out);
+#endif
+        auto& b = subs_[idx(T)];
+        for (size_t cid = 0; cid < MaxCid; ++cid)
+            if (b.test(cid)) send(cid, out.c_str(), out.length());
+    }
+};

esp32/include/event_bus.hpp

@@ -0,0 +1,203 @@
+#pragma once
+#include <array>
+#include <bitset>
+#include <functional>
+#include <optional>
+
+#include <freertos/FreeRTOS.h>
+#include <freertos/task.h>
+#include <freertos/queue.h>
+
+enum class EmitMode { Latest, Batch };
+
+template <typename Msg, size_t QueueDepth = 64, size_t MaxSubs = 8, size_t BatchSize = 16>
+class EventBus {
+    static_assert(BatchSize > 0);
+
+    struct Item {
+        Msg payload;
+        uint8_t exclude; // 0-MaxSubs-1 or 0xFF for “none”
+    };
+
+    using ExIdx = uint8_t;
+
+    static constexpr ExIdx NO_EX = 0xFF;
+
+    struct Sub {
+        std::function<void(const Msg*, size_t)> cb;
+        TickType_t interval;
+        TickType_t last;
+        EmitMode mode;
+        std::array<Msg, BatchSize> buf;
+        size_t cnt;
+    };
+
+    inline static StaticQueue_t q_buf {};
+    inline static Item q_storage[QueueDepth];
+    inline static QueueHandle_t q_handle =
+        xQueueCreateStatic(QueueDepth, sizeof(Item), reinterpret_cast<uint8_t*>(q_storage), &q_buf);
+
+    inline static std::array<std::optional<Sub>, MaxSubs> subs {};
+    inline static portMUX_TYPE mux = portMUX_INITIALIZER_UNLOCKED;
+    inline static Msg latest {};
+    inline static volatile bool has_latest = false;
+
+    static void store(const Msg& m) {
+        portENTER_CRITICAL(&mux);
+        latest = m;
+        has_latest = true;
+        portEXIT_CRITICAL(&mux);
+    }
+
+    static void storeISR(const Msg& m) {
+        UBaseType_t s = portSET_INTERRUPT_MASK_FROM_ISR();
+        latest = m;
+        has_latest = true;
+        portCLEAR_INTERRUPT_MASK_FROM_ISR(s);
+    }
+
+    static void dispatch(const Msg& m, Sub* ex) {
+        store(m);
+        TickType_t now = xTaskGetTickCount();
+        for (auto& sref : subs) {
+            if (!sref) continue;
+            auto& sub = *sref;
+            if (&sub == ex) continue;
+            TickType_t dt = now - sub.last;
+            if (sub.interval && dt < sub.interval) {
+                if (sub.mode == EmitMode::Batch && sub.cnt < BatchSize)
+                    sub.buf[sub.cnt++] = m;
+                else if (sub.mode == EmitMode::Latest)
+                    sub.buf[0] = m, sub.cnt = 1;
+            } else {
+                if (sub.cnt == 0)
+                    sub.cb(&m, 1);
+                else {
+                    sub.cb(sub.buf.data(), sub.cnt);
+                    sub.cnt = 0;
+                }
+                sub.last = now;
+            }
+        }
+    }
+
+    static void worker(void*) {
+        Item it;
+        for (;;)
+            if (xQueueReceive(q_handle, &it, portMAX_DELAY) == pdTRUE)
+                dispatch(it.payload, it.exclude == NO_EX ? nullptr : &*subs[it.exclude]);
+    }
+
+    static void ensureTask() {
+        static bool once =
+            (xTaskCreatePinnedToCore(worker, "eventbus", 4096, nullptr, 6, nullptr, tskNO_AFFINITY), true);
+        (void)once;
+    }
+
+    static void push(const Msg& m, uint8_t ex = NO_EX) {
+        Item it {m, ex};
+        xQueueSend(q_handle, &it, portMAX_DELAY);
+    }
+
+  public:
+    class Handle {
+        size_t index;
+        friend class EventBus;
+        explicit Handle(size_t i) : index(i) {}
+
+      public:
+        Handle(const Handle&) = delete;
+        Handle& operator=(const Handle&) = delete;
+        Handle(Handle&& h) noexcept : index(h.index) { h.index = MaxSubs; }
+        Handle& operator=(Handle&& h) noexcept {
+            if (this != &h) {
+                unsubscribe();
+                index = h.index;
+                h.index = MaxSubs;
+            }
+            return *this;
+        }
+        ~Handle() = default;
+        void unsubscribe() {
+            if (index < MaxSubs) subs[index].reset(), index = MaxSubs;
+        }
+        bool valid() const { return index < MaxSubs; }
+    };
+
+    template <typename C>
+    static Handle subscribe(uint32_t ms, EmitMode mode, C&& fn) {
+        ensureTask();
+        for (size_t i = 0; i < MaxSubs; ++i)
+            if (!subs[i]) {
+                subs[i].emplace(Sub {.cb = std::forward<C>(fn),
+                                     .interval = pdMS_TO_TICKS(ms),
+                                     .last = xTaskGetTickCount(),
+                                     .mode = mode,
+                                     .cnt = 0});
+                return Handle(i);
+            }
+        return Handle(MaxSubs);
+    }
+
+    template <typename C>
+    static Handle subscribe(C&& fn) {
+        using F = std::decay_t<C>;
+        if constexpr (std::is_invocable_v<F, const Msg&>)
+            return subscribe(0, EmitMode::Latest, [f = std::forward<C>(fn)](const Msg* p, size_t n) {
+                for (size_t i = 0; i < n; ++i) f(p[i]);
+            });
+        else
+            return subscribe(0, EmitMode::Latest, std::forward<C>(fn));
+    }
+
+    template <typename C>
+    static Handle subscribe(uint32_t ms, C&& fn) {
+        using F = std::decay_t<C>;
+        if constexpr (std::is_invocable_v<F, const Msg&>)
+            return subscribe(ms, EmitMode::Batch, [f = std::forward<C>(fn)](const Msg* p, size_t n) {
+                for (size_t i = 0; i < n; ++i) f(p[i]);
+            });
+        else
+            return subscribe(ms, EmitMode::Batch, std::forward<C>(fn));
+    }
+
+    static void publish(const Msg& m) {
+        store(m);
+        push(m);
+    }
+
+    static void publishAsync(const Msg& m, const Handle& ex) {
+        store(m);
+        push(m, ex.valid() ? ex.index : NO_EX);
+    }
+
+    static void publish(const Msg& m, const Handle& ex) {
+        if (ex.valid())
+            dispatch(m, &*subs[ex.index]);
+        else
+            publish(m);
+    }
+
+    static void publishISR(const Msg& m, BaseType_t* hpw = nullptr) {
+        storeISR(m);
+        Item it {m, NO_EX};
+        xQueueSendFromISR(q_handle, &it, hpw);
+    }
+
+    static bool peek(Msg& out) {
+        if (!has_latest) return false;
+        portENTER_CRITICAL(&mux);
+        out = latest;
+        portEXIT_CRITICAL(&mux);
+        return true;
+    }
+
+    static bool take(Msg& out) {
+        if (!has_latest) return false;
+        portENTER_CRITICAL(&mux);
+        out = latest;
+        has_latest = false;
+        portEXIT_CRITICAL(&mux);
+        return true;
+    }
+};

esp32/include/msgs/command_msg.hpp

@@ -0,0 +1,17 @@
+#pragma once
+#include <ArduinoJson.h>
+
+struct CommandMsg {
+    float x, y;
+    friend void toJson(JsonVariant v, CommandMsg const &c) {
+        JsonArray arr = v.to<JsonArray>();
+        arr.add(c.x);
+        arr.add(c.y);
+    }
+
+    void fromJson(JsonVariantConst o) {
+        JsonArrayConst arr = o.as<JsonArrayConst>();
+        x = arr[0].as<float>();
+        y = arr[1].as<float>();
+    }
+};

esp32/include/msgs/imu_msg.hpp

@@ -0,0 +1,18 @@
+#pragma once
+#include <ArduinoJson.h>
+
+struct ImuMsg {
+    float ypr[3];
+    friend void toJson(JsonVariant v, ImuMsg const &a) {
+        JsonArray arr = v.to<JsonArray>();
+        arr.add(a.ypr[0]);
+        arr.add(a.ypr[1]);
+        arr.add(a.ypr[2]);
+    }
+    void fromJson(JsonVariantConst o) {
+        JsonArrayConst arr = o.as<JsonArrayConst>();
+        ypr[0] = arr[0].as<float>();
+        ypr[1] = arr[1].as<float>();
+        ypr[2] = arr[2].as<float>();
+    }
+};

esp32/include/msgs/mode_msg.hpp

@@ -0,0 +1,10 @@
+#pragma once
+#include <ArduinoJson.h>
+
+enum class MotionState { ON, OFF };
+
+struct ModeMsg {
+    MotionState mode;
+    friend void toJson(JsonVariant v, ModeMsg const &m) { v.set(static_cast<int>(m.mode)); }
+    void fromJson(JsonVariantConst o) { mode = (MotionState)o.as<int>(); }
+};

esp32/include/msgs/topics.def

@@ -0,0 +1,4 @@
+#define TOPIC_LIST   \
+    X(Imu, ImuMsg)   \
+    X(Mode, ModeMsg) \
+    X(Command, CommandMsg)

esp32/include/topic.hpp

@@ -0,0 +1,57 @@
+#pragma once
+#include "msgs/imu_msg.hpp"
+#include "msgs/command_msg.hpp"
+#include "msgs/mode_msg.hpp"
+
+#include "msgs/topics.def"
+
+enum class Topic : uint8_t {
+#define X(e, t) e,
+    TOPIC_LIST
+#undef X
+        COUNT
+};
+
+template <Topic>
+struct TopicTraits;
+
+#define X(e, t)                    \
+    template <>                    \
+    struct TopicTraits<Topic::e> { \
+        using Msg = t;             \
+    };
+TOPIC_LIST
+#undef X
+
+template <typename Msg>
+static Msg parse(JsonVariantConst v) {
+    Msg msg;
+    msg.fromJson(v);
+    return msg;
+}
+
+// enum class Topic : uint8_t {
+//     Imu = 0,
+//     Command = 1,
+//     Mode = 2,
+//     COUNT // Should always be the last
+// };
+
+// template <Topic>
+// struct TopicTraits;
+
+// template <>
+// struct TopicTraits<Topic::Imu> {
+//     using Msg = ImuMsg;
+//     static constexpr Topic id = Topic::Imu;
+// };
+// template <>
+// struct TopicTraits<Topic::Command> {
+//     using Msg = CommandMsg;
+//     static constexpr Topic id = Topic::Command;
+// };
+// template <>
+// struct TopicTraits<Topic::Mode> {
+//     using Msg = ModeMsg;
+//     static constexpr Topic id = Topic::Mode;
+// };

esp32/lib/ESP32-sveltekit/features.h

@@ -67,6 +67,18 @@
 #define USE_MDNS 1
 #endif
 
+// ESP32 MSGPACK on by default
+#ifndef USE_MSGPACK
+#define USE_MSGPACK 1
+#endif
+
+// ESP32 JSON off by default
+#ifndef USE_JSON
+#define USE_JSON 0
+#endif
+
+static_assert(!(USE_JSON == 1 && USE_MSGPACK == 1), "Cannot set both USE_JSON and USE_MSGPACK to 1 simultaneously");
+
 namespace feature_service {
 
 void printFeatureConfiguration();

esp32/src/adapters/bluetooth.cpp

@@ -0,0 +1,81 @@
+#include <adapters/bluetooth.hpp>
+#include <topic.hpp>
+#include <ArduinoJson.h>
+
+static constexpr auto SERVICE_UUID = "6e400001-b5a3-f393-e0a9-e50e24dcca9e";
+static constexpr auto TX_UUID = "6e400003-b5a3-f393-e0a9-e50e24dcca9e";
+static constexpr auto RX_UUID = "6e400002-b5a3-f393-e0a9-e50e24dcca9e";
+
+void BluetoothService::begin(const char* name) {
+#define X(e, t)                                                                  \
+    setHandle<t>(Topic::e, EventBus<t>::subscribe([this](const t* d, size_t n) { \
+                     if (connected && n) emit<Topic::e>(d[0]);                   \
+                 }));
+    TOPIC_LIST
+#undef X
+
+    BLEDevice::init(name);
+    bleServer = BLEDevice::createServer();
+    bleServer->setCallbacks(new ServerCb(this));
+    auto* svc = bleServer->createService(SERVICE_UUID);
+    txCharacteristic = svc->createCharacteristic(TX_UUID, BLECharacteristic::PROPERTY_NOTIFY);
+    txCharacteristic->addDescriptor(new BLE2902());
+    rxCharacteristic = svc->createCharacteristic(RX_UUID, BLECharacteristic::PROPERTY_WRITE);
+    rxCharacteristic->setCallbacks(new RxCb(this));
+    svc->start();
+    bleServer->getAdvertising()->start();
+}
+
+void BluetoothService::handleReceive(const std::string& data) {
+    JsonDocument doc;
+#if USE_MSGPACK
+    if (deserializeMsgPack(doc, data)) return;
+#else
+    if (deserializeJson(doc, data)) return;
+#endif
+
+    serializeJson(doc, Serial);
+    Serial.println();
+
+    auto payload = doc.as<JsonArrayConst>();
+
+    MsgKind msgKind = static_cast<MsgKind>(payload[0].as<uint8_t>());
+    switch (msgKind) {
+        case MsgKind::Connect:
+            for (size_t i = 1; i < payload.size(); ++i) subscribe(static_cast<Topic>(payload[i].as<uint8_t>()), 0);
+            break;
+
+        case MsgKind::Disconnect:
+            for (size_t i = 1; i < payload.size(); ++i) {
+                Topic t = static_cast<Topic>(payload[i].as<uint8_t>());
+                unsubscribe(t, 0);
+#define X(e, m)                            \
+    if (t == Topic::e) {                   \
+        auto* h = &getHandle<m>(Topic::e); \
+        if (h->valid()) h->unsubscribe();  \
+    }
+                TOPIC_LIST
+#undef X
+            }
+            break;
+
+        case MsgKind::Event:
+            if (payload.size() < 3) break;
+            switch (static_cast<Topic>(payload[1].as<uint8_t>())) {
+#define X(e, m) \
+    case Topic::e: EventBus<m>::publishAsync(parse<m>(payload[2]), getHandle<m>(Topic::e)); break;
+                TOPIC_LIST
+#undef X
+                default: break;
+            }
+            break;
+
+        default: break;
+    }
+}
+
+void BluetoothService::send(size_t, const char* data, size_t len) {
+    if (!connected) return;
+    txCharacteristic->setValue((uint8_t*)data, len);
+    txCharacteristic->notify();
+}

esp32/src/main.cpp

@@ -1,6 +1,9 @@
 #include <spot.h>
+#include <event_bus.hpp>
+#include <adapters/bluetooth.hpp>
 
 DRAM_ATTR Spot spot;
+BluetoothService bluetooth;
 
 void IRAM_ATTR SpotControlLoopEntry(void*) {
     ESP_LOGI("main", "Setup complete now runing tsk");
@@ -17,6 +20,7 @@ void IRAM_ATTR SpotControlLoopEntry(void*) {
 
 void setup() {
     Serial.begin(115200);
+    bluetooth.begin("Hexapod");
 
     spot.initialize();