feat: add tactile codec
This commit is contained in:
226
components/comment/cpdecoder.cc
Normal file
226
components/comment/cpdecoder.cc
Normal file
@@ -0,0 +1,226 @@
|
||||
//
|
||||
// FFmpeg 风格编解码器注册表与上下文管理实现(中文注释版)。
|
||||
// 注意:此实现大量使用 std::vector、std::optional 等现代 C++ 容器与 RAII 思想。
|
||||
//
|
||||
|
||||
#include "cpdecoder.hh"
|
||||
|
||||
#include <algorithm> // 提供 std::find_if 等算法,用于查找与遍历
|
||||
#include <mutex> // std::mutex + std::lock_guard,保证注册表线程安全
|
||||
|
||||
namespace ffmsep {
|
||||
|
||||
namespace { // 匿名命名空间:限制辅助函数和静态变量作用域在当前编译单元
|
||||
|
||||
std::vector<const CPCodec*>& codec_registry() {
|
||||
static std::vector<const CPCodec*> 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<std::uint8_t>(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<std::mutex> 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<const CPCodec*> codecs) {
|
||||
for (const CPCodec* codec : codecs) {
|
||||
cpcodec_register(codec);
|
||||
}
|
||||
}
|
||||
|
||||
const CPCodec* cpcodec_find_decoder(CPCodecID id) {
|
||||
std::lock_guard<std::mutex> 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<std::mutex> 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<const CPCodec*> cpcodec_list_codecs() {
|
||||
std::lock_guard<std::mutex> 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
|
||||
141
components/comment/cpdecoder.hh
Normal file
141
components/comment/cpdecoder.hh
Normal file
@@ -0,0 +1,141 @@
|
||||
//
|
||||
// FFmpeg 风格的串口解码工具核心定义(带详细中文注释)。
|
||||
// 说明:本文件使用了 C++17 引入的 std::optional(可选值容器)与 [[nodiscard]] 属性,
|
||||
// 以及 C++11 的 std::initializer_list、基于 enum class 的强类型枚举,用于提高类型安全。
|
||||
//
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <cstdint> // 固定宽度整数类型,便于协议字段与位宽一致
|
||||
#include <cstddef>
|
||||
#include <mutex> // 互斥锁,用于注册表线程安全
|
||||
#include <optional> // C++17 可选类型,用来表示可能为空的查找结果
|
||||
#include <string>
|
||||
#include <string_view> // C++17 字符串视图,避免不必要的拷贝
|
||||
#include <vector>
|
||||
#include <initializer_list> // 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<std::uint8_t> 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<std::uint8_t> 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<std::uint8_t> 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 <typename T>
|
||||
[[nodiscard]] T* priv_as() noexcept {
|
||||
return static_cast<T*>(priv_data);
|
||||
}
|
||||
|
||||
template <typename T>
|
||||
[[nodiscard]] const T* priv_as() const noexcept {
|
||||
return static_cast<const T*>(priv_data);
|
||||
}
|
||||
|
||||
private:
|
||||
std::vector<std::uint8_t> 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<const CPCodec*> codecs);
|
||||
const CPCodec* cpcodec_find_decoder(CPCodecID id);
|
||||
const CPCodec* cpcodec_find_decoder_by_name(std::string_view name);
|
||||
std::vector<const CPCodec*> 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
|
||||
56
components/comment/tactile/tacdec.h
Normal file
56
components/comment/tactile/tacdec.h
Normal file
@@ -0,0 +1,56 @@
|
||||
//
|
||||
// 触觉传感器串口协议高层解析工具(中文注释版)。
|
||||
// 注意:文件使用了 C++17 的 std::optional 与 inline constexpr,
|
||||
// 可提供编译期常量与安全的可空返回值。
|
||||
//
|
||||
|
||||
#pragma once
|
||||
|
||||
#include "../cpdecoder.hh"
|
||||
|
||||
#include <cstdint> // 协议字段均以固定宽度字节表示
|
||||
#include <optional> // std::optional:可能解析失败时返回空
|
||||
#include <vector>
|
||||
|
||||
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<std::uint8_t> payload; // 数据域内容(按协议解析)
|
||||
};
|
||||
|
||||
// 将底层 CPFrame 解析为结构化的 TactileFrame。
|
||||
std::optional<TactileFrame> parse_frame(const CPFrame& frame);
|
||||
// 将数据域按小端 16 位压力值数组解析。
|
||||
std::vector<std::uint16_t> parse_pressure_values(const TactileFrame& frame);
|
||||
// 解析矩阵尺寸(用于读/写矩阵尺寸的命令)。
|
||||
std::optional<MatrixSize> parse_matrix_size_payload(const TactileFrame& frame);
|
||||
// 解析矩阵坐标(复用尺寸结构,分别表示长边/短边索引)。
|
||||
std::optional<MatrixSize> parse_matrix_coordinate_payload(const TactileFrame& frame);
|
||||
|
||||
const CPCodec* tactile_codec();
|
||||
void register_tactile_codec();
|
||||
|
||||
} // namespace ffmsep::tactile
|
||||
297
components/comment/tactile/tecdec.cc
Normal file
297
components/comment/tactile/tecdec.cc
Normal file
@@ -0,0 +1,297 @@
|
||||
//
|
||||
// 触觉传感器串口协议解码器实现(中文注释版)。
|
||||
// 使用要点:
|
||||
// 1. 通过循环缓冲累积串口字节,按起始/结束符重组完整帧;
|
||||
// 2. 使用 C++17 std::vector/std::optional 管理内存与返回值;
|
||||
// 3. placement new(<new> 头文件)在已有内存上构造上下文对象。
|
||||
//
|
||||
|
||||
#include "tacdec.h"
|
||||
|
||||
#include <algorithm> // std::find 等算法,用于搜索起始符
|
||||
#include <cstddef>
|
||||
#include <cstdint>
|
||||
#include <new> // 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<std::uint8_t> 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<std::uint16_t>(data[i]);
|
||||
for (int bit = 0; bit < 8; ++bit) {
|
||||
if ((crc & 0x0001U) != 0U) {
|
||||
crc = static_cast<std::uint16_t>((crc >> 1U) ^ kCrcPolynomial); // LSB 为 1 时异或多项式
|
||||
} else {
|
||||
crc = static_cast<std::uint16_t>(crc >> 1U); // 否则右移
|
||||
}
|
||||
}
|
||||
}
|
||||
return crc;
|
||||
}
|
||||
|
||||
TactileDecoderContext* get_priv(CPCodecContext* ctx) {
|
||||
return ctx ? ctx->priv_as<TactileDecoderContext>() : 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<TactileDecoderContext*>(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<std::uint8_t>& 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<std::uint16_t>(crc_lo) |
|
||||
static_cast<std::uint16_t>(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<std::ptrdiff_t>(total_frame_length));
|
||||
frame.pts = priv->next_pts++;
|
||||
frame.key_frame = true;
|
||||
frame.valid = true;
|
||||
|
||||
buf.erase(buf.begin(), buf.begin() + static_cast<std::ptrdiff_t>(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<TactileFrame> 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<FunctionCode>(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<std::uint16_t> parse_pressure_values(const TactileFrame& frame) {
|
||||
if (frame.payload.empty() || (frame.payload.size() % 2U != 0U)) {
|
||||
return {};
|
||||
}
|
||||
std::vector<std::uint16_t> 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<std::uint16_t>(
|
||||
static_cast<std::uint16_t>(frame.payload[idx]) |
|
||||
static_cast<std::uint16_t>(frame.payload[idx + 1U] << 8U));
|
||||
values.push_back(value);
|
||||
}
|
||||
return values;
|
||||
}
|
||||
|
||||
// 解析矩阵尺寸载荷:两个字节分别为长边/短边。
|
||||
std::optional<MatrixSize> 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<MatrixSize> 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
|
||||
215
components/ffmsep/cpdecoder.cc
Normal file
215
components/ffmsep/cpdecoder.cc
Normal file
@@ -0,0 +1,215 @@
|
||||
//
|
||||
// Core FFmpeg-style codec registry and decoding helpers.
|
||||
//
|
||||
|
||||
#include "cpdecoder.hh"
|
||||
|
||||
#include <algorithm>
|
||||
#include <mutex>
|
||||
|
||||
namespace ffmsep {
|
||||
|
||||
namespace {
|
||||
|
||||
std::vector<const CPCodec*>& codec_registry() {
|
||||
static std::vector<const CPCodec*> 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<std::uint8_t>(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<std::mutex> 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<const CPCodec*> codecs) {
|
||||
for (const CPCodec* codec : codecs) {
|
||||
cpcodec_register(codec);
|
||||
}
|
||||
}
|
||||
|
||||
const CPCodec* cpcodec_find_decoder(CPCodecID id) {
|
||||
std::lock_guard<std::mutex> 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<std::mutex> 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<const CPCodec*> cpcodec_list_codecs() {
|
||||
std::lock_guard<std::mutex> 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
|
||||
133
components/ffmsep/cpdecoder.hh
Normal file
133
components/ffmsep/cpdecoder.hh
Normal file
@@ -0,0 +1,133 @@
|
||||
//
|
||||
// Simple FFmpeg-inspired serial decoding toolkit.
|
||||
//
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <cstdint>
|
||||
#include <cstddef>
|
||||
#include <mutex>
|
||||
#include <optional>
|
||||
#include <string>
|
||||
#include <string_view>
|
||||
#include <vector>
|
||||
#include <initializer_list>
|
||||
|
||||
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<std::uint8_t> payload;
|
||||
std::int64_t pts = 0;
|
||||
std::int64_t dts = 0;
|
||||
bool end_of_stream = false;
|
||||
bool flush = false;
|
||||
|
||||
CPPacket() = default;
|
||||
CPPacket(std::vector<std::uint8_t> 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<std::uint8_t> 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 <typename T>
|
||||
[[nodiscard]] T* priv_as() noexcept {
|
||||
return static_cast<T*>(priv_data);
|
||||
}
|
||||
|
||||
template <typename T>
|
||||
[[nodiscard]] const T* priv_as() const noexcept {
|
||||
return static_cast<const T*>(priv_data);
|
||||
}
|
||||
|
||||
private:
|
||||
std::vector<std::uint8_t> 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<const CPCodec*> codecs);
|
||||
const CPCodec* cpcodec_find_decoder(CPCodecID id);
|
||||
const CPCodec* cpcodec_find_decoder_by_name(std::string_view name);
|
||||
std::vector<const CPCodec*> 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
|
||||
49
components/ffmsep/tactile/tacdec.h
Normal file
49
components/ffmsep/tactile/tacdec.h
Normal file
@@ -0,0 +1,49 @@
|
||||
//
|
||||
// High level helpers for the tactile sensor binary protocol.
|
||||
//
|
||||
|
||||
#pragma once
|
||||
|
||||
#include "../cpdecoder.hh"
|
||||
|
||||
#include <cstdint>
|
||||
#include <optional>
|
||||
#include <vector>
|
||||
|
||||
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<std::uint8_t> payload;
|
||||
};
|
||||
|
||||
std::optional<TactileFrame> parse_frame(const CPFrame& frame);
|
||||
std::vector<std::uint16_t> parse_pressure_values(const TactileFrame& frame);
|
||||
std::optional<MatrixSize> parse_matrix_size_payload(const TactileFrame& frame);
|
||||
std::optional<MatrixSize> parse_matrix_coordinate_payload(const TactileFrame& frame);
|
||||
|
||||
const CPCodec* tactile_codec();
|
||||
void register_tactile_codec();
|
||||
|
||||
} // namespace ffmsep::tactile
|
||||
286
components/ffmsep/tactile/tecdec.cc
Normal file
286
components/ffmsep/tactile/tecdec.cc
Normal file
@@ -0,0 +1,286 @@
|
||||
//
|
||||
// Decoder for the tactile sensor framed binary protocol.
|
||||
//
|
||||
|
||||
#include "tacdec.h"
|
||||
|
||||
#include <algorithm>
|
||||
#include <cstddef>
|
||||
#include <cstdint>
|
||||
#include <new>
|
||||
|
||||
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<std::uint8_t> 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<std::uint16_t>(data[i]);
|
||||
for (int bit = 0; bit < 8; ++bit) {
|
||||
if ((crc & 0x0001U) != 0U) {
|
||||
crc = static_cast<std::uint16_t>((crc >> 1U) ^ kCrcPolynomial);
|
||||
} else {
|
||||
crc = static_cast<std::uint16_t>(crc >> 1U);
|
||||
}
|
||||
}
|
||||
}
|
||||
return crc;
|
||||
}
|
||||
|
||||
TactileDecoderContext* get_priv(CPCodecContext* ctx) {
|
||||
return ctx ? ctx->priv_as<TactileDecoderContext>() : 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<TactileDecoderContext*>(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<std::uint8_t>& 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<std::uint16_t>(crc_lo) |
|
||||
static_cast<std::uint16_t>(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<std::ptrdiff_t>(total_frame_length));
|
||||
frame.pts = priv->next_pts++;
|
||||
frame.key_frame = true;
|
||||
frame.valid = true;
|
||||
|
||||
buf.erase(buf.begin(), buf.begin() + static_cast<std::ptrdiff_t>(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<TactileFrame> 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<FunctionCode>(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<std::uint16_t> parse_pressure_values(const TactileFrame& frame) {
|
||||
if (frame.payload.empty() || (frame.payload.size() % 2U != 0U)) {
|
||||
return {};
|
||||
}
|
||||
std::vector<std::uint16_t> 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<std::uint16_t>(
|
||||
static_cast<std::uint16_t>(frame.payload[idx]) |
|
||||
static_cast<std::uint16_t>(frame.payload[idx + 1U] << 8U));
|
||||
values.push_back(value);
|
||||
}
|
||||
return values;
|
||||
}
|
||||
|
||||
std::optional<MatrixSize> 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<MatrixSize> 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
|
||||
Reference in New Issue
Block a user