Darkages/SpotMicroESP32-Leika
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

⚡ Heap event bus

Browse Source
This commit is contained in:
Rune Harlyk
2026-02-01 01:09:38 +01:00
parent 0c0ef0ac40
commit 18ec2e618e
3 changed files with 168 additions and 96 deletions
Download Patch File Download Diff File Expand all files Collapse all files
esp32/include/communication/comm_base.hpp
+8 -4
View File
@@ -64,6 +64,8 @@ class CommAdapterBase {
if (clientId < 0 && !hasSubscribers(tag)) return; if (clientId < 0 && !hasSubscribers(tag)) return;
xSemaphoreTake(mutex_, portMAX_DELAY);
msg_.which_message = tag; msg_.which_message = tag;
MessageTraits<T>::assign(msg_, data); MessageTraits<T>::assign(msg_, data);
@@ -77,18 +79,22 @@ class CommAdapterBase {
pb_ostream_t stream = pb_ostream_from_buffer(buffer, out_size); pb_ostream_t stream = pb_ostream_from_buffer(buffer, out_size);
if (!pb_encode(&stream, socket_message_Message_fields, &msg_)) { if (!pb_encode(&stream, socket_message_Message_fields, &msg_)) {
ESP_LOGE("ProtoComm", "Failed to encode message (tag %d), buffer too small?", (int)tag); ESP_LOGE("ProtoComm", "Failed to encode message (tag %d), buffer too small?", (int)tag);
xSemaphoreGive(mutex_);
if (pb_heap_enc_buf != buffer) free(buffer);
return; return;
} }
if (clientId >= 0) { if (clientId >= 0) {
send(buffer, stream.bytes_written, clientId); send(buffer, stream.bytes_written, clientId);
} else { } else {
sendToSubscribers(tag, buffer, stream.bytes_written); sendToSubscribersLocked(tag, buffer, stream.bytes_written);
} }
if (pb_heap_enc_buf != buffer) { if (pb_heap_enc_buf != buffer) {
free(buffer); free(buffer);
} }
xSemaphoreGive(mutex_);
} }
protected: protected:
@@ -151,11 +157,9 @@ class CommAdapterBase {
std::vector<std::unique_ptr<EventBusHandleBase>> eventBusHandles_; std::vector<std::unique_ptr<EventBusHandleBase>> eventBusHandles_;
private: private:
void sendToSubscribers(int32_t tag, const uint8_t* data, size_t len) { void sendToSubscribersLocked(int32_t tag, const uint8_t* data, size_t len) {
xSemaphoreTake(mutex_, portMAX_DELAY);
for (int cid : client_subscriptions_[tag]) { for (int cid : client_subscriptions_[tag]) {
send(data, len, cid); send(data, len, cid);
} }
xSemaphoreGive(mutex_);
} }
}; };
esp32/include/event_bus/event_registry.h
+25 -7
View File
@@ -36,10 +36,28 @@ REGISTER_SETTINGS_TYPE(api_PeripheralSettings, EventType::PERIPHERAL_SETTINGS)
REGISTER_SETTINGS_TYPE(api_ServoSettings, EventType::SERVO_SETTINGS) REGISTER_SETTINGS_TYPE(api_ServoSettings, EventType::SERVO_SETTINGS)
REGISTER_SETTINGS_TYPE(api_CameraSettings, EventType::CAMERA_SETTINGS) REGISTER_SETTINGS_TYPE(api_CameraSettings, EventType::CAMERA_SETTINGS)
REGISTER_EVENT_TYPE(socket_message_IMUData, EventType::IMU_DATA) #define REGISTER_COMMAND_TYPE(MsgType, EventTypeValue) \
REGISTER_EVENT_TYPE(socket_message_ControllerData, EventType::MOTION_COMMAND) REGISTER_EVENT_TYPE(MsgType, EventTypeValue) \
REGISTER_EVENT_TYPE(socket_message_ModeData, EventType::MOTION_MODE) template <> \
REGISTER_EVENT_TYPE(socket_message_AnglesData, EventType::MOTION_ANGLES) struct EventBusConfig<MsgType> { \
REGISTER_EVENT_TYPE(socket_message_WalkGaitData, EventType::MOTION_WALK_GAIT) static constexpr size_t QueueDepth = 1; \
REGISTER_EVENT_TYPE(socket_message_ServoStateData, EventType::SERVO_STATE) static constexpr size_t MaxSubs = 3; \
REGISTER_EVENT_TYPE(socket_message_ServoPWMData, EventType::SERVO_PWM) static constexpr size_t BatchSize = 1; \
};
#define REGISTER_STREAM_TYPE(MsgType, EventTypeValue) \
REGISTER_EVENT_TYPE(MsgType, EventTypeValue) \
template <> \
struct EventBusConfig<MsgType> { \
static constexpr size_t QueueDepth = 4; \
static constexpr size_t MaxSubs = 3; \
static constexpr size_t BatchSize = 4; \
};
REGISTER_STREAM_TYPE(socket_message_IMUData, EventType::IMU_DATA)
REGISTER_COMMAND_TYPE(socket_message_ControllerData, EventType::MOTION_COMMAND)
REGISTER_COMMAND_TYPE(socket_message_ModeData, EventType::MOTION_MODE)
REGISTER_COMMAND_TYPE(socket_message_AnglesData, EventType::MOTION_ANGLES)
REGISTER_COMMAND_TYPE(socket_message_WalkGaitData, EventType::MOTION_WALK_GAIT)
REGISTER_COMMAND_TYPE(socket_message_ServoStateData, EventType::SERVO_STATE)
REGISTER_COMMAND_TYPE(socket_message_ServoPWMData, EventType::SERVO_PWM)
esp32/include/event_bus/typed_event_bus.h
+135 -85
View File
@@ -10,6 +10,7 @@
#include <freertos/FreeRTOS.h> #include <freertos/FreeRTOS.h>
#include <freertos/task.h> #include <freertos/task.h>
#include <freertos/queue.h> #include <freertos/queue.h>
#include <esp_heap_caps.h>
template <typename Sig, size_t MaxSize> template <typename Sig, size_t MaxSize>
class FixedFn; class FixedFn;
@@ -65,82 +66,121 @@ class TypedEventBus {
TickType_t interval; TickType_t interval;
TickType_t last; TickType_t last;
EmitMode mode; EmitMode mode;
std::array<Msg, BatchSize> buf; Msg* buf;
size_t cnt; size_t cnt;
std::atomic<bool> enabled; std::atomic<bool> enabled;
std::atomic<uint32_t> running; std::atomic<uint32_t> running;
Sub() : buf(nullptr), cnt(0), enabled(false), running(0) {
buf = static_cast<Msg*>(heap_caps_malloc(BatchSize * sizeof(Msg), MALLOC_CAP_SPIRAM | MALLOC_CAP_8BIT));
if (!buf) buf = static_cast<Msg*>(malloc(BatchSize * sizeof(Msg)));
}
~Sub() {
if (buf) free(buf);
}
Sub(const Sub&) = delete;
Sub& operator=(const Sub&) = delete;
}; };
inline static StaticQueue_t qbuf;
inline static Item qStorage[QueueDepth]; struct BusState {
inline static QueueHandle_t queue = QueueHandle_t queue;
xQueueCreateStatic(QueueDepth, sizeof(Item), reinterpret_cast<uint8_t*>(qStorage), &qbuf); Sub* subs[MaxSubs];
inline static std::array<std::optional<Sub>, MaxSubs> subs {}; portMUX_TYPE mux;
inline static portMUX_TYPE mux = portMUX_INITIALIZER_UNLOCKED; Msg latest;
inline static Msg latest {}; std::atomic<bool> hasLatest;
inline static std::atomic<bool> hasLatest {false}; std::atomic<size_t> subCount;
inline static std::atomic<size_t> subCount {0}; std::atomic<bool> taskStarted;
inline static std::atomic<bool> taskStarted {false};
BusState()
: queue(nullptr),
mux(portMUX_INITIALIZER_UNLOCKED),
latest {},
hasLatest(false),
subCount(0),
taskStarted(false) {
for (size_t i = 0; i < MaxSubs; ++i) subs[i] = nullptr;
}
};
static BusState& state() {
static BusState s;
return s;
}
static void ensureQueue() {
auto& s = state();
if (s.queue) return;
portENTER_CRITICAL(&s.mux);
if (!s.queue) {
s.queue = xQueueCreate(QueueDepth, sizeof(Item));
}
portEXIT_CRITICAL(&s.mux);
}
static void storeISR(const Msg& m) { static void storeISR(const Msg& m) {
UBaseType_t s = portSET_INTERRUPT_MASK_FROM_ISR(); auto& s = state();
latest = m; UBaseType_t saved = portSET_INTERRUPT_MASK_FROM_ISR();
hasLatest.store(true, std::memory_order_release); s.latest = m;
portCLEAR_INTERRUPT_MASK_FROM_ISR(s); s.hasLatest.store(true, std::memory_order_release);
portCLEAR_INTERRUPT_MASK_FROM_ISR(saved);
} }
static void dispatch(const Msg& m, size_t ex) { static void dispatch(const Msg& m, size_t ex) {
auto& s = state();
TickType_t now = xTaskGetTickCount(); TickType_t now = xTaskGetTickCount();
Sub* ready[MaxSubs]; Sub* ready[MaxSubs];
size_t readyCnt = 0; size_t readyCnt = 0;
portENTER_CRITICAL(&mux); portENTER_CRITICAL(&s.mux);
for (size_t i = 0; i < MaxSubs; ++i) { for (size_t i = 0; i < MaxSubs; ++i) {
auto& opt = subs[i]; Sub* sub = s.subs[i];
if (!opt || i == ex) continue; if (!sub || i == ex) continue;
Sub& s = *opt; if (!sub->enabled.load(std::memory_order_acquire)) continue;
if (!s.enabled.load(std::memory_order_acquire)) continue;
TickType_t dt = now - s.last; TickType_t dt = now - sub->last;
if (s.interval && dt < s.interval) { if (sub->interval && dt < sub->interval) {
if (s.mode == EmitMode::Batch) { if (sub->mode == EmitMode::Batch) {
if (s.cnt < BatchSize) if (sub->cnt < BatchSize)
s.buf[s.cnt++] = m; sub->buf[sub->cnt++] = m;
else else
s.buf[BatchSize - 1] = m; sub->buf[BatchSize - 1] = m;
} else { } else {
s.buf[0] = m; sub->buf[0] = m;
s.cnt = 1; sub->cnt = 1;
} }
} else { } else {
if (s.cnt < BatchSize) if (sub->cnt < BatchSize)
s.buf[s.cnt++] = m; sub->buf[sub->cnt++] = m;
else else
s.buf[BatchSize - 1] = m; sub->buf[BatchSize - 1] = m;
s.last = now; sub->last = now;
s.running.fetch_add(1, std::memory_order_acq_rel); sub->running.fetch_add(1, std::memory_order_acq_rel);
ready[readyCnt++] = &s; ready[readyCnt++] = sub;
} }
} }
portEXIT_CRITICAL(&mux); portEXIT_CRITICAL(&s.mux);
for (size_t i = 0; i < readyCnt; ++i) { for (size_t i = 0; i < readyCnt; ++i) {
Sub* s = ready[i]; Sub* sub = ready[i];
s->cb(s->buf.data(), s->cnt); sub->cb(sub->buf, sub->cnt);
s->cnt = 0; sub->cnt = 0;
s->running.fetch_sub(1, std::memory_order_acq_rel); sub->running.fetch_sub(1, std::memory_order_acq_rel);
} }
} }
static void worker(void*) { static void worker(void*) {
auto& s = state();
Item it; Item it;
while (xQueueReceive(queue, &it, portMAX_DELAY) == pdTRUE) dispatch(it.payload, it.exclude); while (xQueueReceive(s.queue, &it, portMAX_DELAY) == pdTRUE) dispatch(it.payload, it.exclude);
} }
static void ensureTask() { static void ensureTask() {
if (!taskStarted.load(std::memory_order_acquire)) { auto& s = state();
if (!s.taskStarted.load(std::memory_order_acquire)) {
bool expected = false; bool expected = false;
if (taskStarted.compare_exchange_strong(expected, true, std::memory_order_acq_rel)) { if (s.taskStarted.compare_exchange_strong(expected, true, std::memory_order_acq_rel)) {
ensureQueue();
xTaskCreatePinnedToCore(worker, "evtbus", 4096, nullptr, 6, nullptr, 1); xTaskCreatePinnedToCore(worker, "evtbus", 4096, nullptr, 6, nullptr, 1);
} }
} }
@@ -148,8 +188,9 @@ class TypedEventBus {
static bool push(const Msg& m, size_t ex = NO_EX, TickType_t to = 0) { static bool push(const Msg& m, size_t ex = NO_EX, TickType_t to = 0) {
ensureTask(); ensureTask();
auto& s = state();
Item it {m, ex}; Item it {m, ex};
return xQueueSend(queue, &it, to) == pdTRUE; return xQueueSend(s.queue, &it, to) == pdTRUE;
} }
public: public:
@@ -174,19 +215,21 @@ class TypedEventBus {
~Handle() { unsubscribe(); } ~Handle() { unsubscribe(); }
void unsubscribe() { void unsubscribe() {
if (idx < MaxSubs) { if (idx < MaxSubs) {
Sub* s = nullptr; auto& s = state();
portENTER_CRITICAL(&mux); Sub* sub = nullptr;
if (subs[idx]) { portENTER_CRITICAL(&s.mux);
s = &*subs[idx]; sub = s.subs[idx];
s->enabled.store(false, std::memory_order_release); if (sub) {
sub->enabled.store(false, std::memory_order_release);
} }
portEXIT_CRITICAL(&mux); portEXIT_CRITICAL(&s.mux);
if (s) { if (sub) {
while (s->running.load(std::memory_order_acquire) != 0) taskYIELD(); while (sub->running.load(std::memory_order_acquire) != 0) taskYIELD();
portENTER_CRITICAL(&mux); portENTER_CRITICAL(&s.mux);
subs[idx].reset(); s.subs[idx] = nullptr;
portEXIT_CRITICAL(&mux); portEXIT_CRITICAL(&s.mux);
subCount.fetch_sub(1, std::memory_order_acq_rel); delete sub;
s.subCount.fetch_sub(1, std::memory_order_acq_rel);
} }
idx = NO_EX; idx = NO_EX;
} }
@@ -195,10 +238,11 @@ class TypedEventBus {
}; };
static void store(const Msg& m) { static void store(const Msg& m) {
portENTER_CRITICAL(&mux); auto& s = state();
latest = m; portENTER_CRITICAL(&s.mux);
hasLatest.store(true, std::memory_order_release); s.latest = m;
portEXIT_CRITICAL(&mux); s.hasLatest.store(true, std::memory_order_release);
portEXIT_CRITICAL(&s.mux);
} }
template <typename C> template <typename C>
@@ -210,23 +254,25 @@ class TypedEventBus {
template <typename C> template <typename C>
static Handle subscribe(uint32_t ms, EmitMode mode, C fn) { static Handle subscribe(uint32_t ms, EmitMode mode, C fn) {
ensureTask(); ensureTask();
portENTER_CRITICAL(&mux); auto& s = state();
for (size_t i = 0; i < MaxSubs; ++i) portENTER_CRITICAL(&s.mux);
if (!subs[i]) { for (size_t i = 0; i < MaxSubs; ++i) {
subs[i].emplace(); if (!s.subs[i]) {
Sub& s = *subs[i]; Sub* sub = new Sub();
s.cb.set(std::move(fn)); sub->cb.set(std::move(fn));
s.interval = pdMS_TO_TICKS(ms); sub->interval = pdMS_TO_TICKS(ms);
s.last = xTaskGetTickCount() - s.interval; sub->last = xTaskGetTickCount() - sub->interval;
s.mode = mode; sub->mode = mode;
s.cnt = 0; sub->cnt = 0;
s.enabled.store(true, std::memory_order_release); sub->enabled.store(true, std::memory_order_release);
s.running.store(0, std::memory_order_release); sub->running.store(0, std::memory_order_release);
subCount.fetch_add(1, std::memory_order_acq_rel); s.subs[i] = sub;
portEXIT_CRITICAL(&mux); s.subCount.fetch_add(1, std::memory_order_acq_rel);
portEXIT_CRITICAL(&s.mux);
return Handle(i); return Handle(i);
} }
portEXIT_CRITICAL(&mux); }
portEXIT_CRITICAL(&s.mux);
return Handle(NO_EX); return Handle(NO_EX);
} }
@@ -261,27 +307,31 @@ class TypedEventBus {
} }
static void publishISR(const Msg& m, BaseType_t* hpw = nullptr) { static void publishISR(const Msg& m, BaseType_t* hpw = nullptr) {
auto& s = state();
storeISR(m); storeISR(m);
ensureQueue();
Item it {m, NO_EX}; Item it {m, NO_EX};
xQueueSendFromISR(queue, &it, hpw); xQueueSendFromISR(s.queue, &it, hpw);
} }
static bool peek(Msg& out) { static bool peek(Msg& out) {
if (!hasLatest.load(std::memory_order_acquire)) return false; auto& s = state();
portENTER_CRITICAL(&mux); if (!s.hasLatest.load(std::memory_order_acquire)) return false;
out = latest; portENTER_CRITICAL(&s.mux);
portEXIT_CRITICAL(&mux); out = s.latest;
portEXIT_CRITICAL(&s.mux);
return true; return true;
} }
static bool take(Msg& out) { static bool take(Msg& out) {
if (!hasLatest.load(std::memory_order_acquire)) return false; auto& s = state();
portENTER_CRITICAL(&mux); if (!s.hasLatest.load(std::memory_order_acquire)) return false;
out = latest; portENTER_CRITICAL(&s.mux);
hasLatest.store(false, std::memory_order_release); out = s.latest;
portEXIT_CRITICAL(&mux); s.hasLatest.store(false, std::memory_order_release);
portEXIT_CRITICAL(&s.mux);
return true; return true;
} }
static bool hasSubscribers() { return subCount.load(std::memory_order_acquire) > 0; } static bool hasSubscribers() { return state().subCount.load(std::memory_order_acquire) > 0; }
}; };