Niklas Jensen
126 lines
5.1 KiB
C++
126 lines
5.1 KiB
C++
#pragma once
|
|
|
|
#include <pb_encode.h>
|
|
#include <pb_decode.h>
|
|
#include <platform_shared/message.pb.h>
|
|
#include <esp_log.h>
|
|
#include <functional>
|
|
#include <map>
|
|
|
|
#define PROTO_BUFFER_SIZE 2048
|
|
|
|
template <typename T>
|
|
struct MessageTraits;
|
|
|
|
#define DEFINE_MESSAGE_TRAITS(DataType, field) \
|
|
template <> \
|
|
struct MessageTraits<socket_message_##DataType> { \
|
|
static constexpr pb_size_t tag = socket_message_Message_##field##_tag; \
|
|
static void assign(socket_message_Message& msg, const socket_message_##DataType& data) { \
|
|
msg.message.field = data; \
|
|
} \
|
|
static const socket_message_##DataType& access(const socket_message_Message& msg) { \
|
|
return msg.message.field; \
|
|
} \
|
|
};
|
|
|
|
DEFINE_MESSAGE_TRAITS(IMUData, imu)
|
|
DEFINE_MESSAGE_TRAITS(ModeData, mode)
|
|
DEFINE_MESSAGE_TRAITS(AnalyticsData, analytics)
|
|
DEFINE_MESSAGE_TRAITS(AnglesData, angles)
|
|
DEFINE_MESSAGE_TRAITS(RSSIData, rssi)
|
|
DEFINE_MESSAGE_TRAITS(KinematicData, kinematic_data)
|
|
DEFINE_MESSAGE_TRAITS(IMUCalibrateData, imu_calibrate)
|
|
DEFINE_MESSAGE_TRAITS(I2CScanData, i2c_scan)
|
|
DEFINE_MESSAGE_TRAITS(PeripheralSettingsData, peripheral_settings)
|
|
DEFINE_MESSAGE_TRAITS(ControllerData, controller_data)
|
|
DEFINE_MESSAGE_TRAITS(WalkGaitData, walk_gait)
|
|
DEFINE_MESSAGE_TRAITS(IMUCalibrateExecute, imu_calibrate_execute)
|
|
DEFINE_MESSAGE_TRAITS(I2CScanDataRequest, i2c_scan_data_request)
|
|
DEFINE_MESSAGE_TRAITS(PeripheralSettingsDataRequest, peripheral_settings_data_request)
|
|
DEFINE_MESSAGE_TRAITS(ServoPWMData, servo_pwm)
|
|
DEFINE_MESSAGE_TRAITS(ServoStateData, servo_state)
|
|
DEFINE_MESSAGE_TRAITS(CorrelationRequest, correlation_request)
|
|
DEFINE_MESSAGE_TRAITS(CorrelationResponse, correlation_response)
|
|
|
|
// Streaming file transfer messages
|
|
DEFINE_MESSAGE_TRAITS(FSDownloadMetadata, fs_download_metadata)
|
|
DEFINE_MESSAGE_TRAITS(FSDownloadData, fs_download_data)
|
|
DEFINE_MESSAGE_TRAITS(FSDownloadComplete, fs_download_complete)
|
|
DEFINE_MESSAGE_TRAITS(FSUploadData, fs_upload_data)
|
|
DEFINE_MESSAGE_TRAITS(FSUploadComplete, fs_upload_complete)
|
|
|
|
#undef DEFINE_MESSAGE_TRAITS
|
|
|
|
class ProtoDecoder {
|
|
public:
|
|
using SubscribeHandler = std::function<void(int32_t tag, int clientId)>;
|
|
using UnsubscribeHandler = std::function<void(int32_t tag, int clientId)>;
|
|
using PingHandler = std::function<void(int clientId)>;
|
|
|
|
void onSubscribe(SubscribeHandler handler) { subscribeHandler_ = handler; }
|
|
void onUnsubscribe(UnsubscribeHandler handler) { unsubscribeHandler_ = handler; }
|
|
void onPing(PingHandler handler) { pingHandler_ = handler; }
|
|
|
|
template <typename T>
|
|
void on(std::function<void(const T&, int)> handler) {
|
|
handlers_[MessageTraits<T>::tag] = [handler, this](int clientId) {
|
|
handler(MessageTraits<T>::access(msg_), clientId);
|
|
};
|
|
}
|
|
|
|
bool decode(const uint8_t* data, size_t len, int clientId) {
|
|
pb_istream_t stream = pb_istream_from_buffer(data, len);
|
|
|
|
// Reset message before decoding (nanopb will malloc FT_POINTER fields)
|
|
msg_ = socket_message_Message_init_zero;
|
|
|
|
bool success = pb_decode(&stream, socket_message_Message_fields, &msg_);
|
|
|
|
if (!success) {
|
|
ESP_LOGE("ProtoHelpers", "Decode failed: %s (len=%u)", PB_GET_ERROR(&stream), len);
|
|
pb_release(socket_message_Message_fields, &msg_);
|
|
return false;
|
|
}
|
|
|
|
bool handled = false;
|
|
switch (msg_.which_message) {
|
|
case socket_message_Message_sub_notif_tag:
|
|
if (subscribeHandler_) subscribeHandler_(msg_.message.sub_notif.tag, clientId);
|
|
handled = true;
|
|
break;
|
|
|
|
case socket_message_Message_unsub_notif_tag:
|
|
if (unsubscribeHandler_) unsubscribeHandler_(msg_.message.unsub_notif.tag, clientId);
|
|
handled = true;
|
|
break;
|
|
|
|
case socket_message_Message_pingmsg_tag:
|
|
if (pingHandler_) pingHandler_(clientId);
|
|
handled = true;
|
|
break;
|
|
|
|
default: {
|
|
auto it = handlers_.find(msg_.which_message);
|
|
if (it != handlers_.end()) {
|
|
it->second(clientId);
|
|
handled = true;
|
|
}
|
|
break;
|
|
}
|
|
}
|
|
|
|
// Free any malloc'd FT_POINTER fields
|
|
pb_release(socket_message_Message_fields, &msg_);
|
|
|
|
return handled;
|
|
}
|
|
|
|
private:
|
|
socket_message_Message msg_ = socket_message_Message_init_zero;
|
|
SubscribeHandler subscribeHandler_;
|
|
UnsubscribeHandler unsubscribeHandler_;
|
|
PingHandler pingHandler_;
|
|
std::map<pb_size_t, std::function<void(int)>> handlers_;
|
|
};
|