From d819d40fe1bc616fdd57719b11a41c6282cb5e80 Mon Sep 17 00:00:00 2001 From: lenn Date: Thu, 23 Oct 2025 17:21:34 +0800 Subject: [PATCH] feat: add tactile codec --- CMakeLists.txt | 7 + components/comment/cpdecoder.cc | 226 ++++++++++++++++++++ components/comment/cpdecoder.hh | 141 +++++++++++++ components/comment/tactile/tacdec.h | 56 +++++ components/comment/tactile/tecdec.cc | 297 +++++++++++++++++++++++++++ components/ffmsep/cpdecoder.cc | 215 +++++++++++++++++++ components/ffmsep/cpdecoder.hh | 133 ++++++++++++ components/ffmsep/tactile/tacdec.h | 49 +++++ components/ffmsep/tactile/tecdec.cc | 286 ++++++++++++++++++++++++++ 9 files changed, 1410 insertions(+) create mode 100644 components/comment/cpdecoder.cc create mode 100644 components/comment/cpdecoder.hh create mode 100644 components/comment/tactile/tacdec.h create mode 100644 components/comment/tactile/tecdec.cc create mode 100644 components/ffmsep/cpdecoder.cc create mode 100644 components/ffmsep/cpdecoder.hh create mode 100644 components/ffmsep/tactile/tacdec.h create mode 100644 components/ffmsep/tactile/tecdec.cc diff --git a/CMakeLists.txt b/CMakeLists.txt index 191f21c..a30b498 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -74,6 +74,12 @@ add_executable( components/charts/heatmap.cc components/charts/heatmap.hh components/charts/heatmap.impl.hh + dlog/dlog.hh + dlog/dlog.cc + components/ffmsep/cpdecoder.hh + components/ffmsep/cpdecoder.cc + components/ffmsep/tactile/tacdec.h + components/ffmsep/tactile/tecdec.cc ) qt6_add_resources(QRC_FILES resources.qrc) target_sources(${PROJECT_NAME} PRIVATE ${QRC_FILES}) @@ -84,4 +90,5 @@ target_link_libraries( modern-qt qcustomplot serial + spdlog ) diff --git a/components/comment/cpdecoder.cc b/components/comment/cpdecoder.cc new file mode 100644 index 0000000..a67405c --- /dev/null +++ b/components/comment/cpdecoder.cc @@ -0,0 +1,226 @@ +// +// FFmpeg 风格编解码器注册表与上下文管理实现（中文注释版）。 +// 注意：此实现大量使用 std::vector、std::optional 等现代 C++ 容器与 RAII 思想。 +// + +#include "cpdecoder.hh" + +#include // 提供 std::find_if 等算法，用于查找与遍历 +#include // std::mutex + std::lock_guard，保证注册表线程安全 + +namespace ffmsep { + +namespace { // 匿名命名空间：限制辅助函数和静态变量作用域在当前编译单元 + +std::vector& codec_registry() { + static std::vector registry; // 使用静态局部变量 + vector 保留所有注册的编解码器 + return registry; +} + +std::mutex& registry_mutex() { + static std::mutex m; // 构建一个全局互斥量，用于守护注册表操作 + return m; +} + +void attach_codec(CPCodecContext* ctx, const CPCodec* codec) { + if (!ctx) { + return; + } + + ctx->codec = codec; + if (!codec) { + ctx->codec_type = CPMediaType::Unknown; + ctx->priv_data = nullptr; + ctx->release_priv_storage(); + return; + } + + ctx->codec_type = codec->type; + ctx->priv_data = ctx->ensure_priv_storage(codec->priv_data_size); +} + +bool codec_name_equals(const CPCodec* codec, std::string_view name) { + if (!codec || !codec->name) { + return false; + } + return std::string_view(codec->name) == name; // std::string_view 避免额外拷贝 +} + +} // namespace + +void* CPCodecContext::ensure_priv_storage(std::size_t size) { + // 将 vector 当作动态缓冲区，确保大小满足编解码器私有数据需求 + if (size == 0U) { + priv_storage.clear(); + priv_data = nullptr; + return nullptr; + } + if (priv_storage.size() != size) { + priv_storage.assign(size, static_cast(0)); + } + priv_data = priv_storage.data(); + return priv_data; +} + +void CPCodecContext::release_priv_storage() noexcept { + // 与 ensure 配套，释放时直接清空 vector + priv_storage.clear(); + priv_data = nullptr; +} + +// 将单个编解码器指针注册到全局列表，重复注册时作安全检查。 +void cpcodec_register(const CPCodec* codec) { + if (!codec || !codec->name) { + return; + } + + std::lock_guard lock(registry_mutex()); + auto& reg = codec_registry(); + auto already = std::find(reg.begin(), reg.end(), codec); + if (already != reg.end()) { + return; + } + + auto same_id = std::find_if(reg.begin(), reg.end(), [codec](const CPCodec* entry) { + return entry && codec && entry->id == codec->id && codec->id != CPCodecID::Unknown; + }); + if (same_id != reg.end()) { + *same_id = codec; // 如果 ID 已存在，则替换为新实现，以最新注册者为准 + return; + } + + reg.push_back(codec); +} + +// 批量注册函数：利用 C++11 std::initializer_list 便捷传入多个指针。 +void cpcodec_register_many(std::initializer_list codecs) { + for (const CPCodec* codec : codecs) { + cpcodec_register(codec); + } +} + +const CPCodec* cpcodec_find_decoder(CPCodecID id) { + std::lock_guard lock(registry_mutex()); + const auto& reg = codec_registry(); + auto it = std::find_if(reg.begin(), reg.end(), [id](const CPCodec* codec) { + return codec && codec->id == id; + }); + return it == reg.end() ? nullptr : *it; // 使用可空返回值：找不到时返回 nullptr +} + +const CPCodec* cpcodec_find_decoder_by_name(std::string_view name) { + std::lock_guard lock(registry_mutex()); + const auto& reg = codec_registry(); + auto it = std::find_if(reg.begin(), reg.end(), [name](const CPCodec* codec) { + return codec_name_equals(codec, name); + }); + return it == reg.end() ? nullptr : *it; +} + +std::vector cpcodec_list_codecs() { + std::lock_guard lock(registry_mutex()); + return codec_registry(); // 返回当前注册表的浅拷贝 +} + +// 分配一个新的上下文，并可选绑定已有编解码器。 +CPCodecContext* cpcodec_alloc_context3(const CPCodec* codec) { + auto* ctx = new CPCodecContext(); + if (codec) { + attach_codec(ctx, codec); + } + return ctx; +} + +// 打开上下文：允许调用方在此时指定（或替换）具体编解码器。 +int cpcodec_open2(CPCodecContext* ctx, const CPCodec* codec) { + if (!ctx) { + return CP_ERROR_INVALID_ARGUMENT; + } + + if (ctx->is_open) { + return CP_ERROR_INVALID_STATE; + } + + if (codec) { + attach_codec(ctx, codec); + } + + if (!ctx->codec) { + return CP_ERROR_INVALID_ARGUMENT; + } + + ctx->is_open = true; + if (ctx->codec->init) { + int rc = ctx->codec->init(ctx); + if (rc < 0) { + ctx->is_open = false; + if (ctx->codec->close) { + ctx->codec->close(ctx); // 初始化失败时调用 close 进行善后 + } + return rc; + } + } + return CP_SUCCESS; +} + +// 关闭上下文：调用编解码器收尾逻辑并释放私有状态。 +int cpcodec_close(CPCodecContext* ctx) { + if (!ctx) { + return CP_ERROR_INVALID_ARGUMENT; + } + + if (!ctx->is_open) { + return CP_SUCCESS; + } + + if (ctx->codec && ctx->codec->close) { + ctx->codec->close(ctx); + } + + ctx->is_open = false; + ctx->release_priv_storage(); + ctx->codec_type = CPMediaType::Unknown; + ctx->codec = nullptr; + ctx->priv_data = nullptr; + return CP_SUCCESS; +} + +// 释放上下文指针，防止悬挂引用。 +void cpcodec_free_context(CPCodecContext** ctx) { + if (!ctx || !*ctx) { + return; + } + cpcodec_close(*ctx); + delete *ctx; + *ctx = nullptr; +} + +// 输入侧 API：推送一帧串口数据，内部队列通常会缓冲。 +int cpcodec_send_packet(CPCodecContext* ctx, const CPPacket* packet) { + if (!ctx || !packet) { + return CP_ERROR_INVALID_ARGUMENT; + } + if (!ctx->is_open || !ctx->codec) { + return CP_ERROR_NOT_OPEN; + } + if (!ctx->codec->send_packet) { + return CP_ERROR_INVALID_STATE; + } + return ctx->codec->send_packet(ctx, *packet); +} + +// 输出侧 API：尝试从编解码器读取一帧解码结果。 +int cpcodec_receive_frame(CPCodecContext* ctx, CPFrame* frame) { + if (!ctx || !frame) { + return CP_ERROR_INVALID_ARGUMENT; + } + if (!ctx->is_open || !ctx->codec) { + return CP_ERROR_NOT_OPEN; + } + if (!ctx->codec->receive_frame) { + return CP_ERROR_INVALID_STATE; + } + return ctx->codec->receive_frame(ctx, *frame); +} + +} // namespace ffmsep diff --git a/components/comment/cpdecoder.hh b/components/comment/cpdecoder.hh new file mode 100644 index 0000000..411e35a --- /dev/null +++ b/components/comment/cpdecoder.hh @@ -0,0 +1,141 @@ +// +// FFmpeg 风格的串口解码工具核心定义（带详细中文注释）。 +// 说明：本文件使用了 C++17 引入的 std::optional（可选值容器）与 [[nodiscard]] 属性， +// 以及 C++11 的 std::initializer_list、基于 enum class 的强类型枚举，用于提高类型安全。 +// + +#pragma once + +#include // 固定宽度整数类型，便于协议字段与位宽一致 +#include +#include // 互斥锁，用于注册表线程安全 +#include // C++17 可选类型，用来表示可能为空的查找结果 +#include +#include // C++17 字符串视图，避免不必要的拷贝 +#include +#include // C++11 可初始化列表，支持批量注册编解码器 + +namespace ffmsep { // 命名空间隔离所有与串口编解码相关的类型 + +// 错误码定义：参考 FFmpeg 的返回值约定，所有负数表示异常。 +inline constexpr int CP_SUCCESS = 0; // 成功 +inline constexpr int CP_ERROR_EOF = -1; // 流结束（End Of File） +inline constexpr int CP_ERROR_EAGAIN = -2; // 数据暂不可用，稍后重试 +inline constexpr int CP_ERROR_NOT_OPEN = -3; // 编解码上下文尚未打开 +inline constexpr int CP_ERROR_INVALID_STATE = -4; // 当前状态不允许该操作 +inline constexpr int CP_ERROR_INVALID_ARGUMENT = -5;// 传入参数不合法 + +// enum class 使用 C++11 强类型枚举，避免隐式转换导致的错误。 +enum class CPMediaType : std::uint8_t { + Unknown = 0, // 未知类型：默认值 + Data, // 数据流（例如串口数据） + Audio, // 音频流（预留扩展） + Video // 视频流（预留扩展） +}; + +// 编解码器 ID，用于在注册表中快速定位具体实现。 +enum class CPCodecID : std::uint32_t { + Unknown = 0, // 未知或未设置 + Tactile = 0x54514354u // 'T','Q','C','T'：触觉传感器协议标识 +}; + +// CPPacket 表示输入的“帧包”，与 FFmpeg 中 AVPacket 的思想类似。 +struct CPPacket { + std::vector payload; // 有效载荷：串口原始数据 + std::int64_t pts = 0; // 显示时间戳（presentation timestamp） + std::int64_t dts = 0; // 解码时间戳（decode timestamp） + bool end_of_stream = false; // 标记此包是否为流结尾 + bool flush = false; // 是否请求刷新内部状态（例如重置缓冲） + + CPPacket() = default; + CPPacket(std::vector data, std::int64_t pts_value = 0, std::int64_t dts_value = 0) noexcept + : payload(std::move(data)), pts(pts_value), dts(dts_value) {} + + [[nodiscard]] bool empty() const noexcept { return payload.empty(); } // [[nodiscard]] 防止忽略返回值 +}; + +// CPFrame 表示解码后的数据帧，对应业务层可消费的实体。 +struct CPFrame { + std::vector data; // 解码后的数据内容 + std::int64_t pts = 0; // 与输入包对应的时间戳 + bool key_frame = false; // 是否为关键帧（例如起始帧） + bool valid = false; // 是否包含有效数据 + + void reset() noexcept { + data.clear(); + pts = 0; + key_frame = false; + valid = false; + } +}; + +struct CPCodecContext; + +// CPCodec 用函数指针描述具体编解码器的行为，等价于 FFmpeg 中的 AVCodec。 +struct CPCodec { + using InitFn = int (*)(CPCodecContext*); // 初始化回调 + using CloseFn = void (*)(CPCodecContext*); // 关闭回调 + using SendPacketFn = int (*)(CPCodecContext*, const CPPacket&); // 发送输入包 + using ReceiveFrameFn = int (*)(CPCodecContext*, CPFrame&); // 拉取输出帧 + + const char* name = nullptr; + const char* long_name = nullptr; + CPMediaType type = CPMediaType::Unknown; + CPCodecID id = CPCodecID::Unknown; + std::size_t priv_data_size = 0; + InitFn init = nullptr; + CloseFn close = nullptr; + SendPacketFn send_packet = nullptr; + ReceiveFrameFn receive_frame = nullptr; +}; + +struct CPCodecContext { + const CPCodec* codec = nullptr; // 指向当前使用的编解码器描述 + void* priv_data = nullptr; // 指向编解码器私有状态（大小由 priv_data_size 控制） + CPMediaType codec_type = CPMediaType::Unknown; // 保存媒体类型，便于外部查询 + bool is_open = false; // 是否已经成功调用 open + + void clear() noexcept { + codec = nullptr; + priv_data = nullptr; + codec_type = CPMediaType::Unknown; + is_open = false; + priv_storage.clear(); + } + + void* ensure_priv_storage(std::size_t size); // 确保私有存储空间足够，不足时重新分配 + void release_priv_storage() noexcept; // 释放私有存储 + + template + [[nodiscard]] T* priv_as() noexcept { + return static_cast(priv_data); + } + + template + [[nodiscard]] const T* priv_as() const noexcept { + return static_cast(priv_data); + } + +private: + std::vector priv_storage; // 私有缓冲区，使用 std::vector 管理生命周期 + + friend CPCodecContext* cpcodec_alloc_context3(const CPCodec*); + friend int cpcodec_open2(CPCodecContext*, const CPCodec*); + friend int cpcodec_close(CPCodecContext*); +}; + +// 注册接口：允许外部模块将编解码器加入全局列表。 +void cpcodec_register(const CPCodec* codec); +void cpcodec_register_many(std::initializer_list codecs); +const CPCodec* cpcodec_find_decoder(CPCodecID id); +const CPCodec* cpcodec_find_decoder_by_name(std::string_view name); +std::vector cpcodec_list_codecs(); + +CPCodecContext* cpcodec_alloc_context3(const CPCodec* codec); // 分配上下文，关联指定编解码器 +int cpcodec_open2(CPCodecContext* ctx, const CPCodec* codec = nullptr); // 打开上下文，可在此处指定或替换编解码器 +int cpcodec_close(CPCodecContext* ctx); // 关闭上下文并释放资源 +void cpcodec_free_context(CPCodecContext** ctx); // 释放上下文指针 +int cpcodec_send_packet(CPCodecContext* ctx, const CPPacket* packet); // 推送一份待解码的数据包 +int cpcodec_receive_frame(CPCodecContext* ctx, CPFrame* frame); // 拉取解码出来的帧 + +} // namespace ffmsep diff --git a/components/comment/tactile/tacdec.h b/components/comment/tactile/tacdec.h new file mode 100644 index 0000000..b4e6bc1 --- /dev/null +++ b/components/comment/tactile/tacdec.h @@ -0,0 +1,56 @@ +// +// 触觉传感器串口协议高层解析工具（中文注释版）。 +// 注意：文件使用了 C++17 的 std::optional 与 inline constexpr， +// 可提供编译期常量与安全的可空返回值。 +// + +#pragma once + +#include "../cpdecoder.hh" + +#include // 协议字段均以固定宽度字节表示 +#include // std::optional：可能解析失败时返回空 +#include + +namespace ffmsep::tactile { + +inline constexpr std::uint8_t kStartByte = 0x3A; // 帧起始符（冒号） +inline constexpr std::uint8_t kEndByteFirst = 0x0D; // 帧结束符（回车） +inline constexpr std::uint8_t kEndByteSecond = 0x0A; // 帧结束符（换行） + +// 功能码枚举：使用 enum class 提升类型安全，避免与其他数值混用。 +enum class FunctionCode : std::uint8_t { + Unknown = 0x00, // 未知功能，解析失败或未初始化 + ReadMatrix = 0x01, // 读取整块矩阵 AD 值 + ReadSingle = 0x02, // 读取单点 AD 值 + ReadTemperature = 0x03,// 读取温度数据 + SetDeviceId = 0x51, // 修改设备编号 + SetMatrixSize = 0x52, // 修改矩阵尺寸（长边/短边） + CalibrationMode = 0x53 // 进入校准模式 +}; + +struct MatrixSize { + std::uint8_t long_edge = 0; // 矩阵长边尺寸 + std::uint8_t short_edge = 0; // 矩阵短边尺寸 +}; + +struct TactileFrame { + std::uint8_t device_address = 0; // 设备地址（1~255） + FunctionCode function = FunctionCode::Unknown; // 功能码 + std::uint8_t data_length = 0; // 数据域长度 + std::vector payload; // 数据域内容（按协议解析） +}; + +// 将底层 CPFrame 解析为结构化的 TactileFrame。 +std::optional parse_frame(const CPFrame& frame); +// 将数据域按小端 16 位压力值数组解析。 +std::vector parse_pressure_values(const TactileFrame& frame); +// 解析矩阵尺寸（用于读/写矩阵尺寸的命令）。 +std::optional parse_matrix_size_payload(const TactileFrame& frame); +// 解析矩阵坐标（复用尺寸结构，分别表示长边/短边索引）。 +std::optional parse_matrix_coordinate_payload(const TactileFrame& frame); + +const CPCodec* tactile_codec(); +void register_tactile_codec(); + +} // namespace ffmsep::tactile diff --git a/components/comment/tactile/tecdec.cc b/components/comment/tactile/tecdec.cc new file mode 100644 index 0000000..6a3ec67 --- /dev/null +++ b/components/comment/tactile/tecdec.cc @@ -0,0 +1,297 @@ +// +// 触觉传感器串口协议解码器实现（中文注释版）。 +// 使用要点： +// 1. 通过循环缓冲累积串口字节，按起始/结束符重组完整帧； +// 2. 使用 C++17 std::vector/std::optional 管理内存与返回值； +// 3. placement new（ 头文件）在已有内存上构造上下文对象。 +// + +#include "tacdec.h" + +#include // std::find 等算法，用于搜索起始符 +#include +#include +#include // placement new：在已分配内存上构造对象 + +namespace ffmsep::tactile { // 与头文件保持一致的命名空间层次 + +namespace { // 匿名命名空间，用于封装文件内部的工具常量与函数 + +constexpr std::size_t kMinimumFrameSize = 1 // start 起始符 + + 1 // address 设备地址 + + 1 // function 功能码 + + 1 // length 数据长度 + + 0 // payload 最短为 0 + + 2 // CRC 校验 + + 2; // end markers 结束符 + +constexpr std::uint16_t kCrcInitial = 0xFFFF; // CRC 初始值 +constexpr std::uint16_t kCrcPolynomial = 0xA001; // CRC-16/MODBUS 多项式 (LSB-first) + +// 解码器私有上下文，跟随 CPCodecContext 的 priv_data 生命周期。 +struct TactileDecoderContext { + std::vector fifo; // 环形缓冲，存储尚未解析的原始字节 + bool end_of_stream = false; // 标记是否收到流结尾 + std::int64_t next_pts = 0; // 输出帧的自增 pts +}; + +std::uint16_t crc16_modbus(const std::uint8_t* data, std::size_t length) { + std::uint16_t crc = kCrcInitial; + for (std::size_t i = 0; i < length; ++i) { + crc ^= static_cast(data[i]); + for (int bit = 0; bit < 8; ++bit) { + if ((crc & 0x0001U) != 0U) { + crc = static_cast((crc >> 1U) ^ kCrcPolynomial); // LSB 为 1 时异或多项式 + } else { + crc = static_cast(crc >> 1U); // 否则右移 + } + } + } + return crc; +} + +TactileDecoderContext* get_priv(CPCodecContext* ctx) { + return ctx ? ctx->priv_as() : nullptr; // 使用模板封装的安全转换 +} + +int tactile_init(CPCodecContext* ctx) { + if (!ctx) { + return CP_ERROR_INVALID_ARGUMENT; + } + if (!ctx->priv_data) { + ctx->ensure_priv_storage(sizeof(TactileDecoderContext)); + } + auto* storage = static_cast(ctx->priv_data); + new (storage) TactileDecoderContext(); // placement new：在已分配的缓冲区中原地构造对象 + return CP_SUCCESS; +} + +void tactile_close(CPCodecContext* ctx) { + if (!ctx || !ctx->priv_data) { + return; + } + if (auto* priv = get_priv(ctx); priv != nullptr) { + priv->~TactileDecoderContext(); // 显式调用析构函数，释放内部 std::vector + } +} + +int tactile_send_packet(CPCodecContext* ctx, const CPPacket& packet) { + auto* priv = get_priv(ctx); + if (!priv) { + return CP_ERROR_INVALID_STATE; + } + + if (packet.flush) { + priv->fifo.clear(); // flush: 清除所有缓存，回到初始状态 + priv->end_of_stream = false; + priv->next_pts = 0; + } + + if (!packet.payload.empty()) { + priv->fifo.insert(priv->fifo.end(), packet.payload.begin(), packet.payload.end()); // 拼接新字节 + } + + if (packet.end_of_stream) { + priv->end_of_stream = true; // 标记输入源已结束 + } + + return CP_SUCCESS; +} + +std::size_t frame_length_from_payload(std::uint8_t payload_length) { + return 1U + 1U + 1U + 1U + payload_length + 2U + 2U; // 计算完整帧总长度 +} + +const std::uint8_t* buffer_data(const std::vector& buf) { + return buf.empty() ? nullptr : buf.data(); +} + +int tactile_receive_frame(CPCodecContext* ctx, CPFrame& frame) { + auto* priv = get_priv(ctx); + if (!priv) { + return CP_ERROR_INVALID_STATE; + } + + auto& buf = priv->fifo; + + while (true) { + if (buf.empty()) { + if (priv->end_of_stream) { + priv->end_of_stream = false; + return CP_ERROR_EOF; // 没有数据且流结束 + } + return CP_ERROR_EAGAIN; // 告诉调用者需要更多数据 + } + + // Discard bytes until start byte is found. + auto start_it = std::find(buf.begin(), buf.end(), kStartByte); + if (start_it == buf.end()) { + buf.clear(); + if (priv->end_of_stream) { + priv->end_of_stream = false; + return CP_ERROR_EOF; + } + return CP_ERROR_EAGAIN; + } + if (start_it != buf.begin()) { + buf.erase(buf.begin(), start_it); // 丢掉起始符前的噪声 + } + + if (buf.size() < kMinimumFrameSize) { + if (priv->end_of_stream) { + // Incomplete frame at end of stream: drop it and report EOF. + buf.clear(); + priv->end_of_stream = false; + return CP_ERROR_EOF; + } + return CP_ERROR_EAGAIN; // 帧尚未完整，继续等待 + } + + const std::uint8_t* data = buffer_data(buf); + const std::uint8_t address = data[1]; + const std::uint8_t function = data[2]; + const std::uint8_t payload_length = data[3]; + + const std::size_t total_frame_length = frame_length_from_payload(payload_length); // 根据长度字段推算完整帧尺寸 + if (buf.size() < total_frame_length) { + if (priv->end_of_stream) { + // Not enough data before stream end: treat as EOF and drop buffer. + buf.clear(); + priv->end_of_stream = false; + return CP_ERROR_EOF; + } + return CP_ERROR_EAGAIN; + } + + const std::size_t payload_offset = 4U; + const std::size_t crc_offset = payload_offset + payload_length; + const std::size_t end_offset = crc_offset + 2U; // CRC 后紧接 0x0D 0x0A + + const std::uint8_t crc_lo = data[crc_offset]; + const std::uint8_t crc_hi = data[crc_offset + 1U]; + const std::uint16_t crc_value = static_cast(crc_lo) | + static_cast(crc_hi << 8U); + + const std::uint8_t end_first = data[end_offset]; + const std::uint8_t end_second = data[end_offset + 1U]; + + if (end_first != kEndByteFirst || end_second != kEndByteSecond) { + // Invalid end marker, drop start byte and retry. + buf.erase(buf.begin()); + continue; + } + + const std::size_t crc_region_length = 3U + payload_length; // address + function + length + payload + const std::uint16_t computed_crc = crc16_modbus(data + 1U, crc_region_length); + if (computed_crc != crc_value) { + buf.erase(buf.begin()); // CRC 校验失败，丢弃该起始符并重新同步 + continue; + } + + (void)address; // 当前实现仅检查 CRC，不直接使用地址/功能码 + (void)function; + + frame.data.assign(buf.begin(), buf.begin() + static_cast(total_frame_length)); + frame.pts = priv->next_pts++; + frame.key_frame = true; + frame.valid = true; + + buf.erase(buf.begin(), buf.begin() + static_cast(total_frame_length)); // 移除已消费的内容 + + return CP_SUCCESS; + } +} + +const CPCodec kTactileCodec { + "tactile_serial", + "Framed tactile sensor serial protocol decoder", + CPMediaType::Data, + CPCodecID::Tactile, + sizeof(TactileDecoderContext), + &tactile_init, + &tactile_close, + &tactile_send_packet, + &tactile_receive_frame +}; + +} // namespace + +// 将底层 CPFrame 转换为协议专用的 TactileFrame，失败时返回 std::nullopt。 +std::optional parse_frame(const CPFrame& frame) { + if (!frame.valid || frame.data.size() < kMinimumFrameSize) { + return std::nullopt; + } + const auto* bytes = frame.data.data(); + const std::size_t size = frame.data.size(); + + if (bytes[0] != kStartByte) { + return std::nullopt; + } + if (bytes[size - 2] != kEndByteFirst || bytes[size - 1] != kEndByteSecond) { + return std::nullopt; + } + + if (size < 4U) { + return std::nullopt; + } + + const std::uint8_t length = bytes[3]; + if (frame_length_from_payload(length) != size) { + return std::nullopt; + } + + const std::uint8_t address = bytes[1]; + const FunctionCode function = static_cast(bytes[2]); + const std::size_t payload_offset = 4U; + + TactileFrame parsed{}; + parsed.device_address = address; + parsed.function = function; + parsed.data_length = length; + parsed.payload.assign(bytes + payload_offset, bytes + payload_offset + length); + return parsed; +} + +// 将数据域按照小端排列的 16 位压力值序列解析为整数数组。 +std::vector parse_pressure_values(const TactileFrame& frame) { + if (frame.payload.empty() || (frame.payload.size() % 2U != 0U)) { + return {}; + } + std::vector values; + values.reserve(frame.payload.size() / 2U); + for (std::size_t idx = 0; idx + 1U < frame.payload.size(); idx += 2U) { + const std::uint16_t value = static_cast( + static_cast(frame.payload[idx]) | + static_cast(frame.payload[idx + 1U] << 8U)); + values.push_back(value); + } + return values; +} + +// 解析矩阵尺寸载荷：两个字节分别为长边/短边。 +std::optional parse_matrix_size_payload(const TactileFrame& frame) { + if (frame.payload.size() != 2U) { + return std::nullopt; + } + MatrixSize size{}; + size.long_edge = frame.payload[0]; + size.short_edge = frame.payload[1]; + return size; +} + +// 解析矩阵坐标：协议格式与尺寸一致，直接复用逻辑。 +std::optional parse_matrix_coordinate_payload(const TactileFrame& frame) { + return parse_matrix_size_payload(frame); +} + +// 提供对外查询：返回触觉协议对应的 CPCodec 描述。 +const CPCodec* tactile_codec() { + return &kTactileCodec; +} + +// 将触觉协议编解码器注册到全局列表，供 cpcodec_find_decoder 使用。 +void register_tactile_codec() { + cpcodec_register(&kTactileCodec); +} + +} // namespace ffmsep::tactile diff --git a/components/ffmsep/cpdecoder.cc b/components/ffmsep/cpdecoder.cc new file mode 100644 index 0000000..e59381d --- /dev/null +++ b/components/ffmsep/cpdecoder.cc @@ -0,0 +1,215 @@ +// +// Core FFmpeg-style codec registry and decoding helpers. +// + +#include "cpdecoder.hh" + +#include +#include + +namespace ffmsep { + +namespace { + +std::vector& codec_registry() { + static std::vector registry; + return registry; +} + +std::mutex& registry_mutex() { + static std::mutex m; + return m; +} + +void attach_codec(CPCodecContext* ctx, const CPCodec* codec) { + if (!ctx) { + return; + } + + ctx->codec = codec; + if (!codec) { + ctx->codec_type = CPMediaType::Unknown; + ctx->priv_data = nullptr; + ctx->release_priv_storage(); + return; + } + + ctx->codec_type = codec->type; + ctx->priv_data = ctx->ensure_priv_storage(codec->priv_data_size); +} + +bool codec_name_equals(const CPCodec* codec, std::string_view name) { + if (!codec || !codec->name) { + return false; + } + return std::string_view(codec->name) == name; +} + +} // namespace + +void* CPCodecContext::ensure_priv_storage(std::size_t size) { + if (size == 0U) { + priv_storage.clear(); + priv_data = nullptr; + return nullptr; + } + if (priv_storage.size() != size) { + priv_storage.assign(size, static_cast(0)); + } + priv_data = priv_storage.data(); + return priv_data; +} + +void CPCodecContext::release_priv_storage() noexcept { + priv_storage.clear(); + priv_data = nullptr; +} + +void cpcodec_register(const CPCodec* codec) { + if (!codec || !codec->name) { + return; + } + + std::lock_guard lock(registry_mutex()); + auto& reg = codec_registry(); + auto already = std::find(reg.begin(), reg.end(), codec); + if (already != reg.end()) { + return; + } + + auto same_id = std::find_if(reg.begin(), reg.end(), [codec](const CPCodec* entry) { + return entry && codec && entry->id == codec->id && codec->id != CPCodecID::Unknown; + }); + if (same_id != reg.end()) { + *same_id = codec; + return; + } + + reg.push_back(codec); +} + +void cpcodec_register_many(std::initializer_list codecs) { + for (const CPCodec* codec : codecs) { + cpcodec_register(codec); + } +} + +const CPCodec* cpcodec_find_decoder(CPCodecID id) { + std::lock_guard lock(registry_mutex()); + const auto& reg = codec_registry(); + auto it = std::find_if(reg.begin(), reg.end(), [id](const CPCodec* codec) { + return codec && codec->id == id; + }); + return it == reg.end() ? nullptr : *it; +} + +const CPCodec* cpcodec_find_decoder_by_name(std::string_view name) { + std::lock_guard lock(registry_mutex()); + const auto& reg = codec_registry(); + auto it = std::find_if(reg.begin(), reg.end(), [name](const CPCodec* codec) { + return codec_name_equals(codec, name); + }); + return it == reg.end() ? nullptr : *it; +} + +std::vector cpcodec_list_codecs() { + std::lock_guard lock(registry_mutex()); + return codec_registry(); +} + +CPCodecContext* cpcodec_alloc_context3(const CPCodec* codec) { + auto* ctx = new CPCodecContext(); + if (codec) { + attach_codec(ctx, codec); + } + return ctx; +} + +int cpcodec_open2(CPCodecContext* ctx, const CPCodec* codec) { + if (!ctx) { + return CP_ERROR_INVALID_ARGUMENT; + } + + if (ctx->is_open) { + return CP_ERROR_INVALID_STATE; + } + + if (codec) { + attach_codec(ctx, codec); + } + + if (!ctx->codec) { + return CP_ERROR_INVALID_ARGUMENT; + } + + ctx->is_open = true; + if (ctx->codec->init) { + int rc = ctx->codec->init(ctx); + if (rc < 0) { + ctx->is_open = false; + if (ctx->codec->close) { + ctx->codec->close(ctx); + } + return rc; + } + } + return CP_SUCCESS; +} + +int cpcodec_close(CPCodecContext* ctx) { + if (!ctx) { + return CP_ERROR_INVALID_ARGUMENT; + } + + if (!ctx->is_open) { + return CP_SUCCESS; + } + + if (ctx->codec && ctx->codec->close) { + ctx->codec->close(ctx); + } + + ctx->is_open = false; + ctx->release_priv_storage(); + ctx->codec_type = CPMediaType::Unknown; + ctx->codec = nullptr; + ctx->priv_data = nullptr; + return CP_SUCCESS; +} + +void cpcodec_free_context(CPCodecContext** ctx) { + if (!ctx || !*ctx) { + return; + } + cpcodec_close(*ctx); + delete *ctx; + *ctx = nullptr; +} + +int cpcodec_send_packet(CPCodecContext* ctx, const CPPacket* packet) { + if (!ctx || !packet) { + return CP_ERROR_INVALID_ARGUMENT; + } + if (!ctx->is_open || !ctx->codec) { + return CP_ERROR_NOT_OPEN; + } + if (!ctx->codec->send_packet) { + return CP_ERROR_INVALID_STATE; + } + return ctx->codec->send_packet(ctx, *packet); +} + +int cpcodec_receive_frame(CPCodecContext* ctx, CPFrame* frame) { + if (!ctx || !frame) { + return CP_ERROR_INVALID_ARGUMENT; + } + if (!ctx->is_open || !ctx->codec) { + return CP_ERROR_NOT_OPEN; + } + if (!ctx->codec->receive_frame) { + return CP_ERROR_INVALID_STATE; + } + return ctx->codec->receive_frame(ctx, *frame); +} + +} // namespace ffmsep diff --git a/components/ffmsep/cpdecoder.hh b/components/ffmsep/cpdecoder.hh new file mode 100644 index 0000000..0a570c0 --- /dev/null +++ b/components/ffmsep/cpdecoder.hh @@ -0,0 +1,133 @@ +// +// Simple FFmpeg-inspired serial decoding toolkit. +// + +#pragma once + +#include +#include +#include +#include +#include +#include +#include +#include + +namespace ffmsep { + +// Error codes loosely mirroring FFmpeg semantics. +inline constexpr int CP_SUCCESS = 0; +inline constexpr int CP_ERROR_EOF = -1; +inline constexpr int CP_ERROR_EAGAIN = -2; +inline constexpr int CP_ERROR_NOT_OPEN = -3; +inline constexpr int CP_ERROR_INVALID_STATE = -4; +inline constexpr int CP_ERROR_INVALID_ARGUMENT = -5; + +enum class CPMediaType : std::uint8_t { + Unknown = 0, + Data, + Audio, + Video +}; + +enum class CPCodecID : std::uint32_t { + Unknown = 0, + Tactile = 0x54514354u // 'T','Q','C','T' marker for tactile quick codec. +}; + +struct CPPacket { + std::vector payload; + std::int64_t pts = 0; + std::int64_t dts = 0; + bool end_of_stream = false; + bool flush = false; + + CPPacket() = default; + CPPacket(std::vector data, std::int64_t pts_value = 0, std::int64_t dts_value = 0) noexcept + : payload(std::move(data)), pts(pts_value), dts(dts_value) {} + + [[nodiscard]] bool empty() const noexcept { return payload.empty(); } +}; + +struct CPFrame { + std::vector data; + std::int64_t pts = 0; + bool key_frame = false; + bool valid = false; + + void reset() noexcept { + data.clear(); + pts = 0; + key_frame = false; + valid = false; + } +}; + +struct CPCodecContext; + +struct CPCodec { + using InitFn = int (*)(CPCodecContext*); + using CloseFn = void (*)(CPCodecContext*); + using SendPacketFn = int (*)(CPCodecContext*, const CPPacket&); + using ReceiveFrameFn = int (*)(CPCodecContext*, CPFrame&); + + const char* name = nullptr; + const char* long_name = nullptr; + CPMediaType type = CPMediaType::Unknown; + CPCodecID id = CPCodecID::Unknown; + std::size_t priv_data_size = 0; + InitFn init = nullptr; + CloseFn close = nullptr; + SendPacketFn send_packet = nullptr; + ReceiveFrameFn receive_frame = nullptr; +}; + +struct CPCodecContext { + const CPCodec* codec = nullptr; + void* priv_data = nullptr; + CPMediaType codec_type = CPMediaType::Unknown; + bool is_open = false; + + void clear() noexcept { + codec = nullptr; + priv_data = nullptr; + codec_type = CPMediaType::Unknown; + is_open = false; + priv_storage.clear(); + } + + void* ensure_priv_storage(std::size_t size); + void release_priv_storage() noexcept; + + template + [[nodiscard]] T* priv_as() noexcept { + return static_cast(priv_data); + } + + template + [[nodiscard]] const T* priv_as() const noexcept { + return static_cast(priv_data); + } + +private: + std::vector priv_storage; + + friend CPCodecContext* cpcodec_alloc_context3(const CPCodec*); + friend int cpcodec_open2(CPCodecContext*, const CPCodec*); + friend int cpcodec_close(CPCodecContext*); +}; + +void cpcodec_register(const CPCodec* codec); +void cpcodec_register_many(std::initializer_list codecs); +const CPCodec* cpcodec_find_decoder(CPCodecID id); +const CPCodec* cpcodec_find_decoder_by_name(std::string_view name); +std::vector cpcodec_list_codecs(); + +CPCodecContext* cpcodec_alloc_context3(const CPCodec* codec); +int cpcodec_open2(CPCodecContext* ctx, const CPCodec* codec = nullptr); +int cpcodec_close(CPCodecContext* ctx); +void cpcodec_free_context(CPCodecContext** ctx); +int cpcodec_send_packet(CPCodecContext* ctx, const CPPacket* packet); +int cpcodec_receive_frame(CPCodecContext* ctx, CPFrame* frame); + +} // namespace ffmsep diff --git a/components/ffmsep/tactile/tacdec.h b/components/ffmsep/tactile/tacdec.h new file mode 100644 index 0000000..c6f01f4 --- /dev/null +++ b/components/ffmsep/tactile/tacdec.h @@ -0,0 +1,49 @@ +// +// High level helpers for the tactile sensor binary protocol. +// + +#pragma once + +#include "../cpdecoder.hh" + +#include +#include +#include + +namespace ffmsep::tactile { + +inline constexpr std::uint8_t kStartByte = 0x3A; +inline constexpr std::uint8_t kEndByteFirst = 0x0D; +inline constexpr std::uint8_t kEndByteSecond = 0x0A; + +enum class FunctionCode : std::uint8_t { + Unknown = 0x00, + ReadMatrix = 0x01, + ReadSingle = 0x02, + ReadTemperature = 0x03, + SetDeviceId = 0x51, + SetMatrixSize = 0x52, + CalibrationMode = 0x53 +}; + +struct MatrixSize { + std::uint8_t long_edge = 0; + std::uint8_t short_edge = 0; +}; + +struct TactileFrame { + std::uint8_t device_address = 0; + FunctionCode function = FunctionCode::Unknown; + std::uint8_t data_length = 0; + std::vector payload; +}; + +std::optional parse_frame(const CPFrame& frame); +std::vector parse_pressure_values(const TactileFrame& frame); +std::optional parse_matrix_size_payload(const TactileFrame& frame); +std::optional parse_matrix_coordinate_payload(const TactileFrame& frame); + +const CPCodec* tactile_codec(); +void register_tactile_codec(); + +} // namespace ffmsep::tactile diff --git a/components/ffmsep/tactile/tecdec.cc b/components/ffmsep/tactile/tecdec.cc new file mode 100644 index 0000000..fa97e67 --- /dev/null +++ b/components/ffmsep/tactile/tecdec.cc @@ -0,0 +1,286 @@ +// +// Decoder for the tactile sensor framed binary protocol. +// + +#include "tacdec.h" + +#include +#include +#include +#include + +namespace ffmsep::tactile { + +namespace { + +constexpr std::size_t kMinimumFrameSize = 1 // start + + 1 // address + + 1 // function + + 1 // length + + 0 // payload + + 2 // CRC + + 2; // end markers + +constexpr std::uint16_t kCrcInitial = 0xFFFF; +constexpr std::uint16_t kCrcPolynomial = 0xA001; // CRC-16/MODBUS (LSB first) + +struct TactileDecoderContext { + std::vector fifo; + bool end_of_stream = false; + std::int64_t next_pts = 0; +}; + +std::uint16_t crc16_modbus(const std::uint8_t* data, std::size_t length) { + std::uint16_t crc = kCrcInitial; + for (std::size_t i = 0; i < length; ++i) { + crc ^= static_cast(data[i]); + for (int bit = 0; bit < 8; ++bit) { + if ((crc & 0x0001U) != 0U) { + crc = static_cast((crc >> 1U) ^ kCrcPolynomial); + } else { + crc = static_cast(crc >> 1U); + } + } + } + return crc; +} + +TactileDecoderContext* get_priv(CPCodecContext* ctx) { + return ctx ? ctx->priv_as() : nullptr; +} + +int tactile_init(CPCodecContext* ctx) { + if (!ctx) { + return CP_ERROR_INVALID_ARGUMENT; + } + if (!ctx->priv_data) { + ctx->ensure_priv_storage(sizeof(TactileDecoderContext)); + } + auto* storage = static_cast(ctx->priv_data); + new (storage) TactileDecoderContext(); + return CP_SUCCESS; +} + +void tactile_close(CPCodecContext* ctx) { + if (!ctx || !ctx->priv_data) { + return; + } + if (auto* priv = get_priv(ctx); priv != nullptr) { + priv->~TactileDecoderContext(); + } +} + +int tactile_send_packet(CPCodecContext* ctx, const CPPacket& packet) { + auto* priv = get_priv(ctx); + if (!priv) { + return CP_ERROR_INVALID_STATE; + } + + if (packet.flush) { + priv->fifo.clear(); + priv->end_of_stream = false; + priv->next_pts = 0; + } + + if (!packet.payload.empty()) { + priv->fifo.insert(priv->fifo.end(), packet.payload.begin(), packet.payload.end()); + } + + if (packet.end_of_stream) { + priv->end_of_stream = true; + } + + return CP_SUCCESS; +} + +std::size_t frame_length_from_payload(std::uint8_t payload_length) { + return 1U + 1U + 1U + 1U + payload_length + 2U + 2U; +} + +const std::uint8_t* buffer_data(const std::vector& buf) { + return buf.empty() ? nullptr : buf.data(); +} + +int tactile_receive_frame(CPCodecContext* ctx, CPFrame& frame) { + auto* priv = get_priv(ctx); + if (!priv) { + return CP_ERROR_INVALID_STATE; + } + + auto& buf = priv->fifo; + + while (true) { + if (buf.empty()) { + if (priv->end_of_stream) { + priv->end_of_stream = false; + return CP_ERROR_EOF; + } + return CP_ERROR_EAGAIN; + } + + // Discard bytes until start byte is found. + auto start_it = std::find(buf.begin(), buf.end(), kStartByte); + if (start_it == buf.end()) { + buf.clear(); + if (priv->end_of_stream) { + priv->end_of_stream = false; + return CP_ERROR_EOF; + } + return CP_ERROR_EAGAIN; + } + if (start_it != buf.begin()) { + buf.erase(buf.begin(), start_it); + } + + if (buf.size() < kMinimumFrameSize) { + if (priv->end_of_stream) { + // Incomplete frame at end of stream: drop it and report EOF. + buf.clear(); + priv->end_of_stream = false; + return CP_ERROR_EOF; + } + return CP_ERROR_EAGAIN; + } + + const std::uint8_t* data = buffer_data(buf); + const std::uint8_t address = data[1]; + const std::uint8_t function = data[2]; + const std::uint8_t payload_length = data[3]; + + const std::size_t total_frame_length = frame_length_from_payload(payload_length); + if (buf.size() < total_frame_length) { + if (priv->end_of_stream) { + // Not enough data before stream end: treat as EOF and drop buffer. + buf.clear(); + priv->end_of_stream = false; + return CP_ERROR_EOF; + } + return CP_ERROR_EAGAIN; + } + + const std::size_t payload_offset = 4U; + const std::size_t crc_offset = payload_offset + payload_length; + const std::size_t end_offset = crc_offset + 2U; + + const std::uint8_t crc_lo = data[crc_offset]; + const std::uint8_t crc_hi = data[crc_offset + 1U]; + const std::uint16_t crc_value = static_cast(crc_lo) | + static_cast(crc_hi << 8U); + + const std::uint8_t end_first = data[end_offset]; + const std::uint8_t end_second = data[end_offset + 1U]; + + if (end_first != kEndByteFirst || end_second != kEndByteSecond) { + // Invalid end marker, drop start byte and retry. + buf.erase(buf.begin()); + continue; + } + + const std::size_t crc_region_length = 3U + payload_length; // address + function + length + payload + const std::uint16_t computed_crc = crc16_modbus(data + 1U, crc_region_length); + if (computed_crc != crc_value) { + buf.erase(buf.begin()); + continue; + } + + (void)address; + (void)function; + + frame.data.assign(buf.begin(), buf.begin() + static_cast(total_frame_length)); + frame.pts = priv->next_pts++; + frame.key_frame = true; + frame.valid = true; + + buf.erase(buf.begin(), buf.begin() + static_cast(total_frame_length)); + + return CP_SUCCESS; + } +} + +const CPCodec kTactileCodec { + "tactile_serial", + "Framed tactile sensor serial protocol decoder", + CPMediaType::Data, + CPCodecID::Tactile, + sizeof(TactileDecoderContext), + &tactile_init, + &tactile_close, + &tactile_send_packet, + &tactile_receive_frame +}; + +} // namespace + +std::optional parse_frame(const CPFrame& frame) { + if (!frame.valid || frame.data.size() < kMinimumFrameSize) { + return std::nullopt; + } + const auto* bytes = frame.data.data(); + const std::size_t size = frame.data.size(); + + if (bytes[0] != kStartByte) { + return std::nullopt; + } + if (bytes[size - 2] != kEndByteFirst || bytes[size - 1] != kEndByteSecond) { + return std::nullopt; + } + + if (size < 4U) { + return std::nullopt; + } + + const std::uint8_t length = bytes[3]; + if (frame_length_from_payload(length) != size) { + return std::nullopt; + } + + const std::uint8_t address = bytes[1]; + const FunctionCode function = static_cast(bytes[2]); + const std::size_t payload_offset = 4U; + + TactileFrame parsed{}; + parsed.device_address = address; + parsed.function = function; + parsed.data_length = length; + parsed.payload.assign(bytes + payload_offset, bytes + payload_offset + length); + return parsed; +} + +std::vector parse_pressure_values(const TactileFrame& frame) { + if (frame.payload.empty() || (frame.payload.size() % 2U != 0U)) { + return {}; + } + std::vector values; + values.reserve(frame.payload.size() / 2U); + for (std::size_t idx = 0; idx + 1U < frame.payload.size(); idx += 2U) { + const std::uint16_t value = static_cast( + static_cast(frame.payload[idx]) | + static_cast(frame.payload[idx + 1U] << 8U)); + values.push_back(value); + } + return values; +} + +std::optional parse_matrix_size_payload(const TactileFrame& frame) { + if (frame.payload.size() != 2U) { + return std::nullopt; + } + MatrixSize size{}; + size.long_edge = frame.payload[0]; + size.short_edge = frame.payload[1]; + return size; +} + +std::optional parse_matrix_coordinate_payload(const TactileFrame& frame) { + return parse_matrix_size_payload(frame); +} + +const CPCodec* tactile_codec() { + return &kTactileCodec; +} + +void register_tactile_codec() { + cpcodec_register(&kTactileCodec); +} + +} // namespace ffmsep::tactile