diff --git a/esp32/build_settings.ini b/esp32/build_settings.ini index 65f1cb0..5d54b3e 100644 --- a/esp32/build_settings.ini +++ b/esp32/build_settings.ini @@ -5,4 +5,6 @@ build_flags = -D EMBED_WWW -D SERVE_CONFIG_FILES -D CORS_ORIGIN=\"*\" - -D ENABLE_CORS \ No newline at end of file + -D ENABLE_CORS + -D USE_MSGPACK=1 ; Use either msgpack or json + -D USE_JSON=0 ; Use either msgpack or json \ No newline at end of file diff --git a/esp32/include/adapters/bluetooth.hpp b/esp32/include/adapters/bluetooth.hpp new file mode 100644 index 0000000..57e61c6 --- /dev/null +++ b/esp32/include/adapters/bluetooth.hpp @@ -0,0 +1,60 @@ +#pragma once +#include +#include +#include +#include + +#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 + using EventBusHandle = typename EventBus::Handle; + + template + EventBusHandle& getHandle(Topic topic) { + return *static_cast*>(subscriptionHandle[static_cast(topic)]); + } + + template + void setHandle(Topic topic, EventBusHandle&& handle) { + subscriptionHandle[static_cast(topic)] = new EventBusHandle(std::move(handle)); + } + + std::array(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(Topic::COUNT); ++i) svc->unsubscribe(static_cast(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() {} +}; \ No newline at end of file diff --git a/esp32/include/adapters/comm_base.hpp b/esp32/include/adapters/comm_base.hpp new file mode 100644 index 0000000..5f04028 --- /dev/null +++ b/esp32/include/adapters/comm_base.hpp @@ -0,0 +1,63 @@ +#pragma once +#include +#include +#include +#include +#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 (Topic::COUNT)> +class CommBase { + using Bits = std::bitset; + std::array subs_; + portMUX_TYPE mux_ portMUX_INITIALIZER_UNLOCKED; + + static constexpr size_t idx(Topic t) { return static_cast(t); } + + template + void encode(JsonDocument& d, const typename TopicTraits::Msg& m) { + auto a = d.to(); + a.add(static_cast(MsgKind::Event)); + a.add(static_cast(T)); + toJson(a.add(), 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 + void emit(const typename TopicTraits::Msg& m) { + if (!has(T)) return; + JsonDocument doc; + encode(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()); + } +}; \ No newline at end of file diff --git a/esp32/include/event_bus.hpp b/esp32/include/event_bus.hpp new file mode 100644 index 0000000..2de7f4c --- /dev/null +++ b/esp32/include/event_bus.hpp @@ -0,0 +1,203 @@ +#pragma once +#include +#include +#include +#include + +#include +#include +#include + +enum class EmitMode { Latest, Batch }; + +template +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 cb; + TickType_t interval; + TickType_t last; + EmitMode mode; + std::array 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(q_storage), &q_buf); + + inline static std::array, 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 + 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(fn), + .interval = pdMS_TO_TICKS(ms), + .last = xTaskGetTickCount(), + .mode = mode, + .cnt = 0}); + return Handle(i); + } + return Handle(MaxSubs); + } + + template + static Handle subscribe(C&& fn) { + using F = std::decay_t; + if constexpr (std::is_invocable_v) + return subscribe(0, EmitMode::Latest, [f = std::forward(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(fn)); + } + + template + static Handle subscribe(uint32_t ms, C&& fn) { + using F = std::decay_t; + if constexpr (std::is_invocable_v) + return subscribe(ms, EmitMode::Batch, [f = std::forward(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(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; + } +}; \ No newline at end of file diff --git a/esp32/include/msgs/command_msg.hpp b/esp32/include/msgs/command_msg.hpp new file mode 100644 index 0000000..05393b2 --- /dev/null +++ b/esp32/include/msgs/command_msg.hpp @@ -0,0 +1,17 @@ +#pragma once +#include + +struct CommandMsg { + float x, y; + friend void toJson(JsonVariant v, CommandMsg const &c) { + JsonArray arr = v.to(); + arr.add(c.x); + arr.add(c.y); + } + + void fromJson(JsonVariantConst o) { + JsonArrayConst arr = o.as(); + x = arr[0].as(); + y = arr[1].as(); + } +}; \ No newline at end of file diff --git a/esp32/include/msgs/imu_msg.hpp b/esp32/include/msgs/imu_msg.hpp new file mode 100644 index 0000000..4bc7342 --- /dev/null +++ b/esp32/include/msgs/imu_msg.hpp @@ -0,0 +1,18 @@ +#pragma once +#include + +struct ImuMsg { + float ypr[3]; + friend void toJson(JsonVariant v, ImuMsg const &a) { + JsonArray arr = v.to(); + arr.add(a.ypr[0]); + arr.add(a.ypr[1]); + arr.add(a.ypr[2]); + } + void fromJson(JsonVariantConst o) { + JsonArrayConst arr = o.as(); + ypr[0] = arr[0].as(); + ypr[1] = arr[1].as(); + ypr[2] = arr[2].as(); + } +}; \ No newline at end of file diff --git a/esp32/include/msgs/mode_msg.hpp b/esp32/include/msgs/mode_msg.hpp new file mode 100644 index 0000000..4e93ad4 --- /dev/null +++ b/esp32/include/msgs/mode_msg.hpp @@ -0,0 +1,10 @@ +#pragma once +#include + +enum class MotionState { ON, OFF }; + +struct ModeMsg { + MotionState mode; + friend void toJson(JsonVariant v, ModeMsg const &m) { v.set(static_cast(m.mode)); } + void fromJson(JsonVariantConst o) { mode = (MotionState)o.as(); } +}; \ No newline at end of file diff --git a/esp32/include/msgs/topics.def b/esp32/include/msgs/topics.def new file mode 100644 index 0000000..a89c1f5 --- /dev/null +++ b/esp32/include/msgs/topics.def @@ -0,0 +1,4 @@ +#define TOPIC_LIST \ + X(Imu, ImuMsg) \ + X(Mode, ModeMsg) \ + X(Command, CommandMsg) diff --git a/esp32/include/topic.hpp b/esp32/include/topic.hpp new file mode 100644 index 0000000..27366bf --- /dev/null +++ b/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 +struct TopicTraits; + +#define X(e, t) \ + template <> \ + struct TopicTraits { \ + using Msg = t; \ + }; +TOPIC_LIST +#undef X + +template +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 +// struct TopicTraits; + +// template <> +// struct TopicTraits { +// using Msg = ImuMsg; +// static constexpr Topic id = Topic::Imu; +// }; +// template <> +// struct TopicTraits { +// using Msg = CommandMsg; +// static constexpr Topic id = Topic::Command; +// }; +// template <> +// struct TopicTraits { +// using Msg = ModeMsg; +// static constexpr Topic id = Topic::Mode; +// }; \ No newline at end of file diff --git a/esp32/lib/ESP32-sveltekit/features.h b/esp32/lib/ESP32-sveltekit/features.h index a98df9f..b77cbde 100644 --- a/esp32/lib/ESP32-sveltekit/features.h +++ b/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(); diff --git a/esp32/src/adapters/bluetooth.cpp b/esp32/src/adapters/bluetooth.cpp new file mode 100644 index 0000000..ae2f505 --- /dev/null +++ b/esp32/src/adapters/bluetooth.cpp @@ -0,0 +1,81 @@ +#include +#include +#include + +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(Topic::e, EventBus::subscribe([this](const t* d, size_t n) { \ + if (connected && n) emit(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(); + + MsgKind msgKind = static_cast(payload[0].as()); + switch (msgKind) { + case MsgKind::Connect: + for (size_t i = 1; i < payload.size(); ++i) subscribe(static_cast(payload[i].as()), 0); + break; + + case MsgKind::Disconnect: + for (size_t i = 1; i < payload.size(); ++i) { + Topic t = static_cast(payload[i].as()); + unsubscribe(t, 0); +#define X(e, m) \ + if (t == Topic::e) { \ + auto* h = &getHandle(Topic::e); \ + if (h->valid()) h->unsubscribe(); \ + } + TOPIC_LIST +#undef X + } + break; + + case MsgKind::Event: + if (payload.size() < 3) break; + switch (static_cast(payload[1].as())) { +#define X(e, m) \ + case Topic::e: EventBus::publishAsync(parse(payload[2]), getHandle(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(); +} \ No newline at end of file diff --git a/esp32/src/main.cpp b/esp32/src/main.cpp index c02ce3e..5ccae26 100644 --- a/esp32/src/main.cpp +++ b/esp32/src/main.cpp @@ -1,6 +1,9 @@ #include +#include +#include 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();