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

⚡ Moves rest of events to event bus

Browse Source
This commit is contained in:
Rune Harlyk
2026-02-01 00:44:24 +01:00
parent 57e0aac2aa
commit 0c0ef0ac40
9 changed files with 110 additions and 30 deletions
Download Patch File Download Diff File Expand all files Collapse all files
esp32/include/communication/comm_base.hpp
+36 -2
View File
@@ -6,8 +6,11 @@
#include <functional>
#include <list>
#include <map>
#include <memory>
#include <vector>
#include <type_traits>
#include <communication/proto_helpers.h>
#include <event_bus/event_bus.h>
class CommAdapterBase {
public:
@@ -35,6 +38,26 @@ class CommAdapterBase {
decoder_.on<T>(handler);
}
template <typename T>
void onPublish(std::function<void(const T&, int)> handler = nullptr) {
decoder_.on<T>([this, handler](const T& data, int clientId) {
EventBus::publish(data);
if (handler) handler(data, clientId);
});
}
template <typename T>
void bridgeFromEventBus() {
eventBusHandles_.push_back(
std::make_unique<EventBusHandleStorage<T>>(EventBus::subscribe<T>([this](const T& data) { emit(data); })));
}
template <typename T>
void bridgeFromEventBus(uint32_t intervalMs) {
eventBusHandles_.push_back(std::make_unique<EventBusHandleStorage<T>>(
EventBus::subscribe<T>(intervalMs, [this](const T& data) { emit(data); })));
}
template <typename T>
void emit(const T& data, int clientId = -1) {
constexpr pb_size_t tag = MessageTraits<T>::tag;
@@ -47,8 +70,7 @@ class CommAdapterBase {
size_t out_size;
pb_get_encoded_size(&out_size, socket_message_Message_fields, &msg_);
uint8_t* buffer = pb_heap_enc_buf;
if (out_size > sizeof(pb_heap_enc_buf)) { // If the encoded size exceeds our buffer size, we needs to malloc a
// buffer of a proper size
if (out_size > sizeof(pb_heap_enc_buf)) {
buffer = (uint8_t*)malloc(out_size);
}
@@ -116,6 +138,18 @@ class CommAdapterBase {
socket_message_Message msg_ = socket_message_Message_init_zero;
uint8_t pb_heap_enc_buf[PROTO_BUFFER_SIZE];
struct EventBusHandleBase {
virtual ~EventBusHandleBase() = default;
};
template <typename T>
struct EventBusHandleStorage : EventBusHandleBase {
EventBus::Handle<T> handle;
EventBusHandleStorage(EventBus::Handle<T>&& h) : handle(std::move(h)) {}
};
std::vector<std::unique_ptr<EventBusHandleBase>> eventBusHandles_;
private:
void sendToSubscribers(int32_t tag, const uint8_t* data, size_t len) {
xSemaphoreTake(mutex_, portMAX_DELAY);
esp32/include/event_bus/event_registry.h
+4
View File
@@ -39,3 +39,7 @@ REGISTER_SETTINGS_TYPE(api_CameraSettings, EventType::CAMERA_SETTINGS)
REGISTER_EVENT_TYPE(socket_message_IMUData, EventType::IMU_DATA)
REGISTER_EVENT_TYPE(socket_message_ControllerData, EventType::MOTION_COMMAND)
REGISTER_EVENT_TYPE(socket_message_ModeData, EventType::MOTION_MODE)
REGISTER_EVENT_TYPE(socket_message_AnglesData, EventType::MOTION_ANGLES)
REGISTER_EVENT_TYPE(socket_message_WalkGaitData, EventType::MOTION_WALK_GAIT)
REGISTER_EVENT_TYPE(socket_message_ServoStateData, EventType::SERVO_STATE)
REGISTER_EVENT_TYPE(socket_message_ServoPWMData, EventType::SERVO_PWM)
esp32/include/event_bus/event_types.h
+3
View File
@@ -14,7 +14,10 @@ enum class EventType : uint16_t {
IMU_DATA = 131,
MOTION_COMMAND = 200,
MOTION_MODE = 201,
MOTION_ANGLES = 202,
MOTION_WALK_GAIT = 203,
SERVO_STATE = 141,
SERVO_PWM = 142,
SYSTEM_BOOT = 300,
STORAGE_HYDRATION_COMPLETE = 301,
esp32/include/motion.h
+14
View File
@@ -14,13 +14,18 @@
#include <motion_states/stand_state.h>
#include <motion_states/rest_state.h>
#include <message_types.h>
#include <event_bus/event_bus.h>
enum class MOTION_STATE { DEACTIVATED, IDLE, CALIBRATION, REST, STAND, WALK };
class MotionService {
public:
using ModeChangeCallback = std::function<void(bool active)>;
void begin();
void setModeChangeCallback(ModeChangeCallback callback) { modeChangeCallback_ = callback; }
void handleAngles(const socket_message_AnglesData& data);
void handleInput(const socket_message_ControllerData& data);
@@ -42,6 +47,8 @@ class MotionService {
inline bool isActive() { return state != nullptr; }
private:
void subscribeToEvents();
Kinematics kinematics;
CommandMsg command = {0, 0, 0, 0, 0, 0, 0};
@@ -62,6 +69,13 @@ class MotionService {
float dir[12] = {1, -1, -1, -1, -1, -1, 1, -1, -1, -1, -1, -1};
int64_t lastUpdate = esp_timer_get_time();
ModeChangeCallback modeChangeCallback_;
EventBus::Handle<socket_message_ControllerData> controllerHandle_;
EventBus::Handle<socket_message_ModeData> modeHandle_;
EventBus::Handle<socket_message_AnglesData> anglesHandle_;
EventBus::Handle<socket_message_WalkGaitData> walkGaitHandle_;
};
#endif
esp32/include/peripherals/servo_controller.h
+9
View File
@@ -5,6 +5,7 @@
#include <event_bus/event_bus.h>
#include <utils/math_utils.h>
#include <platform_shared/api.pb.h>
#include <platform_shared/message.pb.h>
#include <esp_http_server.h>
#ifndef FACTORY_SERVO_PWM_FREQUENCY
@@ -41,6 +42,12 @@ class ServoController {
_settingsHandle = EventBus::subscribe<api_ServoSettings>(
[this](const api_ServoSettings &settings) { onSettingsChanged(settings); });
_pwmHandle = EventBus::subscribe<socket_message_ServoPWMData>(
[this](const socket_message_ServoPWMData &data) { setServoPWM(data.servo_id, data.servo_pwm); });
_stateHandle = EventBus::subscribe<socket_message_ServoStateData>(
[this](const socket_message_ServoStateData &data) { data.active ? activate() : deactivate(); });
api_ServoSettings initialSettings;
if (EventBus::peek(initialSettings)) {
onSettingsChanged(initialSettings);
@@ -125,6 +132,8 @@ class ServoController {
api_ServoSettings _settings = ServoSettings_defaults();
EventBus::Handle<api_ServoSettings> _settingsHandle;
EventBus::Handle<socket_message_ServoPWMData> _pwmHandle;
EventBus::Handle<socket_message_ServoStateData> _stateHandle;
PCA9685Driver _pca;
esp32/src/event_bus/event_bus.cpp
+7
View File
@@ -52,3 +52,10 @@ template void EventBus::notifyGlobalListeners<socket_message_IMUData>(const sock
template void EventBus::notifyGlobalListeners<socket_message_ControllerData>(const socket_message_ControllerData&,
const char*);
template void EventBus::notifyGlobalListeners<socket_message_ModeData>(const socket_message_ModeData&, const char*);
template void EventBus::notifyGlobalListeners<socket_message_AnglesData>(const socket_message_AnglesData&, const char*);
template void EventBus::notifyGlobalListeners<socket_message_WalkGaitData>(const socket_message_WalkGaitData&,
const char*);
template void EventBus::notifyGlobalListeners<socket_message_ServoStateData>(const socket_message_ServoStateData&,
const char*);
template void EventBus::notifyGlobalListeners<socket_message_ServoPWMData>(const socket_message_ServoPWMData&,
const char*);
esp32/src/event_bus/event_types.cpp
+3
View File
@@ -13,7 +13,10 @@ const char* eventTypeName(EventType type) {
case EventType::IMU_DATA: return "IMU_DATA";
case EventType::MOTION_COMMAND: return "MOTION_COMMAND";
case EventType::MOTION_MODE: return "MOTION_MODE";
case EventType::MOTION_ANGLES: return "MOTION_ANGLES";
case EventType::MOTION_WALK_GAIT: return "MOTION_WALK_GAIT";
case EventType::SERVO_STATE: return "SERVO_STATE";
case EventType::SERVO_PWM: return "SERVO_PWM";
case EventType::SYSTEM_BOOT: return "SYSTEM_BOOT";
case EventType::STORAGE_HYDRATION_COMPLETE: return "STORAGE_HYDRATION_COMPLETE";
default: return "UNKNOWN";
esp32/src/main.cpp
+15 -27
View File
@@ -132,32 +132,18 @@ void setupEventSocket() {
[](const socket_message_FSDownloadComplete &complete, int clientId) { wsSocket.emit(complete, clientId); },
[](const socket_message_FSUploadComplete &complete, int clientId) { wsSocket.emit(complete, clientId); });
wsSocket.on<socket_message_ControllerData>(
[&](const socket_message_ControllerData &data, int clientId) { motionService.handleInput(data); });
wsSocket.on<socket_message_ModeData>([&](const socket_message_ModeData &data, int clientId) {
servoController.setMode(SERVO_CONTROL_STATE::ANGLE);
motionService.handleMode(data);
motionService.isActive() ? servoController.activate() : servoController.deactivate();
});
wsSocket.on<socket_message_WalkGaitData>(
[&](const socket_message_WalkGaitData &data, int clientId) { motionService.handleWalkGait(data); });
wsSocket.on<socket_message_AnglesData>(
[&](const socket_message_AnglesData &data, int clientId) { motionService.handleAngles(data); });
wsSocket.on<socket_message_ServoPWMData>([&](const socket_message_ServoPWMData &data, int clientId) {
servoController.setServoPWM(data.servo_id, data.servo_pwm);
});
wsSocket.on<socket_message_ServoStateData>([&](const socket_message_ServoStateData &data, int clientId) {
data.active ? servoController.activate() : servoController.deactivate();
});
wsSocket.onPublish<socket_message_ControllerData>();
wsSocket.onPublish<socket_message_ModeData>();
wsSocket.onPublish<socket_message_WalkGaitData>();
wsSocket.onPublish<socket_message_AnglesData>();
wsSocket.onPublish<socket_message_ServoPWMData>();
wsSocket.onPublish<socket_message_ServoStateData>();
wsSocket.on<socket_message_FSUploadData>(
[&](const socket_message_FSUploadData &data, int clientId) { FileSystemWS::fsHandler.handleUploadData(data); });
wsSocket.bridgeFromEventBus<socket_message_IMUData>(100);
using CorrelationHandler =
std::function<void(const socket_message_CorrelationRequest &, socket_message_CorrelationResponse &, int)>;
static std::map<pb_size_t, CorrelationHandler> correlationHandlers = {
@@ -257,6 +243,10 @@ void IRAM_ATTR SpotControlLoopEntry(void *) {
peripherals.begin();
servoController.begin();
motionService.begin();
motionService.setModeChangeCallback([](bool active) {
servoController.setMode(SERVO_CONTROL_STATE::ANGLE);
active ? servoController.activate() : servoController.deactivate();
});
#if FT_ENABLED(USE_WS2812)
ledService.begin();
#endif
@@ -310,11 +300,9 @@ void IRAM_ATTR serviceLoopEntry(void *) {
});
EXECUTE_EVERY_N_MS(100, {
if (wsSocket.hasSubscribers(socket_message_Message_imu_tag)) {
socket_message_IMUData imu = socket_message_IMUData_init_zero;
peripherals.getIMUProto(imu);
wsSocket.emit(imu);
}
socket_message_IMUData imu = socket_message_IMUData_init_zero;
peripherals.getIMUProto(imu);
EventBus::publish(imu);
if (wsSocket.hasSubscribers(socket_message_Message_rssi_tag)) {
socket_message_RSSIData rssi = {.rssi = WiFi.RSSI()};
esp32/src/motion.cpp
+19 -1
View File
@@ -1,6 +1,23 @@
#include <motion.h>
void MotionService::begin() { body_state.updateFeet(KinConfig::default_feet_positions); }
void MotionService::begin() {
body_state.updateFeet(KinConfig::default_feet_positions);
subscribeToEvents();
}
void MotionService::subscribeToEvents() {
controllerHandle_ = EventBus::subscribe<socket_message_ControllerData>(
[this](const socket_message_ControllerData& data) { handleInput(data); });
modeHandle_ =
EventBus::subscribe<socket_message_ModeData>([this](const socket_message_ModeData& data) { handleMode(data); });
anglesHandle_ = EventBus::subscribe<socket_message_AnglesData>(
[this](const socket_message_AnglesData& data) { handleAngles(data); });
walkGaitHandle_ = EventBus::subscribe<socket_message_WalkGaitData>(
[this](const socket_message_WalkGaitData& data) { handleWalkGait(data); });
}
void MotionService::handleAngles(const socket_message_AnglesData& data) {
for (int i = 0; i < 12 && i < data.angles_count; i++) {
@@ -41,6 +58,7 @@ void MotionService::handleMode(const socket_message_ModeData& data) {
case MOTION_STATE::DEACTIVATED: setState(nullptr); break;
default: setState(nullptr); break;
}
if (modeChangeCallback_) modeChangeCallback_(isActive());
}
void MotionService::handleGestures(const gesture_t ges) {